bool crawlGeneratorModule::configure(yarp::os::ResourceFinder &rf)
{
Property options(rf.toString());
int period = 25; // in ms
if(options.check("period"))
{
period = options.find("period").asInt();
}
if(options.check("part"))
{
partName=options.find("part").asString().c_str();
printf("module taking care of part %s\n",partName.c_str());
}
theThread = new GeneratorThread(period);
if(!theThread->init(rf))
{
printf("Failed to initialize the thread\n");
fflush(stdout);
return false;
}
theThread->start();
return true;
}
virtual bool configure(yarp::os::ResourceFinder &rf)
{
Property options;
options.fromString(rf.toString());
char robotName[255];
Bottle *jointsList=0;
std::string moduleName = "directPositionControl";
Time::turboBoost();
options.put("device", "remote_controlboard");
if(options.check("robot"))
strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
else
strncpy(robotName, "icub", sizeof(robotName));
if (options.check("name"))
{
moduleName = options.find("name").asString();
}
if(options.check("part"))
{
sprintf(partName, "%s", options.find("part").asString().c_str());
char tmp[255];
sprintf(tmp, "/%s/%s/%s/client", moduleName.c_str(), robotName, partName);
options.put("local",tmp);
sprintf(tmp, "/%s/%s", robotName, partName);
options.put("remote", tmp);
sprintf(tmp, "/%s/%s/rpc", moduleName.c_str(), partName);
rpc_port.open(tmp);
options.put("carrier", "tcp");
attach(rpc_port);
}
else
{
yError("Please specify part (e.g. --part head)\n");
return false;
}
if(options.check("joints"))
{
jointsList = options.find("joints").asList();
if (jointsList==0) yError("Unable to parts 'joints' parameter\n");
}
else
{
yError("Please specify the joints to control (e.g. --joints ""(0 1 2)"" ");
return false;
}
//opening the device driver
if (!driver.open(options))
{
yError("Error opening device, check parameters\n");
return false;
}
///starting the thread
int period = CONTROL_PERIOD;
if(options.check("period"))
period = options.find("period").asInt();
yInfo("control rate is %d ms",period);
pThread=new positionDirectControlThread(period);
pThread->init(&driver, moduleName, partName, robotName, jointsList);
pThread->start();
return true;
}
bool TorqueBalancingReferencesGenerator::configure (yarp::os::ResourceFinder &rf)
{
using namespace yarp::os;
using namespace yarp::sig;
using namespace Eigen;
if (!rf.check("wbi_config_file", "Checking wbi configuration file")) {
std::cout << "No WBI configuration file found.\n";
return false;
}
Property wbiProperties;
if (!wbiProperties.fromConfigFile(rf.findFile("wbi_config_file"))) {
std::cout << "Not possible to load WBI properties from file.\n";
return false;
}
wbiProperties.fromString(rf.toString(), false);
//retrieve the joint list
std::string wbiList = rf.find("wbi_joint_list").asString();
wbi::IDList iCubMainJoints;
if (!yarpWbi::loadIdListFromConfig(wbiList, wbiProperties, iCubMainJoints)) {
std::cout << "Cannot find joint list\n";
return false;
}
size_t actuatedDOFs = iCubMainJoints.size();
//create an instance of wbi
m_robot = new yarpWbi::yarpWholeBodyInterface("torqueBalancingReferencesGenerator", wbiProperties);
if (!m_robot) {
std::cout << "Could not create wbi object.\n";
return false;
}
m_robot->addJoints(iCubMainJoints);
if (!m_robot->init()) {
std::cout << "Could not initialize wbi object.\n";
return false;
}
robotName = rf.check("robot", Value("icubSim"), "Looking for robot name").asString();
// numberOfPostures = rf.check("numberOfPostures", Value(0), "Looking for numberOfPostures").asInt();
directionOfOscillation.resize(3,0.0);
frequencyOfOscillation = 0;
amplitudeOfOscillation = 0;
if (!loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation ))
{
return false;
}
loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation);
for(int i=0; i < postures.size(); i++ )
{
std::cerr << "[INFO] time_" << i << " = " << postures[i].time << std::endl;
std::cerr << "[INFO] posture_" << i << " = " << postures[i].qDes.toString()<< std::endl;
}
for(int i=0; i < comTimeAndSetPoints.size(); i++ )
{
std::cerr << "[INFO] time_" << i << " = " << comTimeAndSetPoints[i].time << std::endl;
std::cerr << "[INFO] com_" << i << " = " << comTimeAndSetPoints[i].comDes.toString()<< std::endl;
}
//
std::cout << "[INFO] Number of DOFs: " << actuatedDOFs << std::endl;
std::cerr << "[INFO] changePostural: " << changePostural << std::endl;
std::cerr << "[INFO] changeComWithSetPoints: " << changeComWithSetPoints << std::endl;
std::cerr << "[INFO] amplitudeOfOscillation: " << amplitudeOfOscillation << std::endl;
std::cout << "[INFO] frequencyOfOscillation " << frequencyOfOscillation << std::endl;
std::cerr << "[INFO] directionOfOscillation: " << directionOfOscillation.toString() << std::endl;
q0.resize(actuatedDOFs, 0.0);
com0.resize(3, 0.0);
comDes.resize(3, 0.0);
DcomDes.resize(3, 0.0);
DDcomDes.resize(3, 0.0);
m_robot->getEstimates(wbi::ESTIMATE_JOINT_POS, q0.data());
double world2BaseFrameSerialization[16];
double rotoTranslationVector[7];
wbi::Frame world2BaseFrame;
m_robot->getEstimates(wbi::ESTIMATE_BASE_POS, world2BaseFrameSerialization);
wbi::frameFromSerialization(world2BaseFrameSerialization, world2BaseFrame);
m_robot->forwardKinematics(q0.data(), world2BaseFrame, wbi::wholeBodyInterface::COM_LINK_ID, rotoTranslationVector);
com0[0] = rotoTranslationVector[0];
com0[1] = rotoTranslationVector[1];
com0[2] = rotoTranslationVector[2];
timeoutBeforeStreamingRefs = rf.check("timeoutBeforeStreamingRefs", Value(20), "Looking for robot name").asDouble();
period = rf.check("period", Value(0.01), "Looking for module period").asDouble();
std::cout << "timeoutBeforeStreamingRefs: " << timeoutBeforeStreamingRefs << "\n";
std::string group_name = "PORTS_INFO";
if( !rf.check(group_name) )
{
std::cerr << "[ERR] no PORTS_INFO group found" << std::endl;
return true;
}
else
{
yarp::os::Bottle group_bot = rf.findGroup(group_name);
if( !group_bot.check("portNameForStreamingQdes") ||
!group_bot.check("portNameForStreamingComDes") )
{
std::cerr << "[ERR] portNameForStreamingQdes or portNameForStreamingComDes not found in config file" << std::endl;
return false;
}
//Check coupling configuration is well formed
std::string portNameForStreamingComDes = group_bot.find("portNameForStreamingComDes").asString();
std::cout << "portNameForStreamingComDes: " << portNameForStreamingComDes << "\n";
std::string portNameForStreamingQdes = group_bot.find("portNameForStreamingQdes").asString();
std::cout << "portNameForStreamingQdes: " << portNameForStreamingQdes << "\n";
portForStreamingComDes.open(portNameForStreamingComDes);
portForStreamingQdes.open(portNameForStreamingQdes);
t0 = yarp::os::Time::now();
return true;
}
}
virtual bool configure(yarp::os::ResourceFinder &rf)
{
Property options;
options.fromString(rf.toString());
char robotName[255];
Time::turboBoost();
options.put("device", "remote_controlboard");
if(options.check("robot"))
strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
else
strncpy(robotName, "icub", sizeof(robotName));
if(options.check("part"))
{
char tmp[255];
sprintf(tmp, "/%s/vc/%s/client",
robotName,
options.find("part").asString().c_str());
options.put("local",tmp);
sprintf(tmp, "/%s/%s",
robotName,
options.find("part").asString().c_str());
options.put("remote", tmp);
sprintf(tmp,"/%s/vc/%s/input",
robotName,
options.find("part").asString().c_str());
input_port.open(tmp);
options.put("carrier", "mcast");
attach(input_port);
}
else
{
yError("Please specify part (e.g. --part head)\n");
return false;
}
////end of the modif////////////
if (!driver.open(options))
{
yError("Error opening device, check parameters\n");
return false;
}
///we start the thread
int period = CONTROL_RATE;
if(options.check("period"))
period = options.find("period").asInt();
yInfo("control rate is %d ms",period);
if (!options.check("part"))
return false;
sprintf(partName, "%s", options.find("part").asString().c_str());
vc=new velControlThread(period);
vc->init(&driver, partName,
robotName);
vc->start();
return true;
}
bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{
ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
setName(str.c_str()); // modulePortName
double maxDelay = rf.check("maxDelay", Value(0.010), "Max delay between image and encoders").asDouble();
// pass configuration over to bottle
Bottle botConfig(rf.toString().c_str());
botConfig.setMonitor(rf.getMonitor());
// Load from configuration group ([<group_name>]), if group option present
Value *valGroup; // check assigns pointer to reference
if(botConfig.check("group", valGroup, "Configuration group to load module options from (string).")) {
strGroup = valGroup->asString().c_str();
// is group a valid bottle?
if (botConfig.check(strGroup.c_str())){
Bottle &group=botConfig.findGroup(strGroup.c_str(),string("Loading configuration from group " + strGroup).c_str());
botConfig.fromString(group.toString());
} else {
yError() << "Group " << strGroup << " not found.";
return false;
}
} else {
yError ("There seem to be an error loading parameters (group section missing), stopping module");
return false;
}
string calibToolName = botConfig.check("projection",
Value("pinhole"),
"Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
_calibTool = CalibToolFactories::getPool().get(calibToolName.c_str());
if (_calibTool!=NULL) {
bool ok = _calibTool->open(botConfig);
if (!ok) {
delete _calibTool;
_calibTool = NULL;
return false;
}
}
if (yarp::os::Network::exists(getName("/in"))) {
yWarning() << "port " << getName("/in") << " already in use";
}
if (yarp::os::Network::exists(getName("/out"))) {
yWarning() << "port " << getName("/out") << " already in use";
}
if (yarp::os::Network::exists(getName("/conf"))) {
yWarning() << "port " << getName("/conf") << " already in use";
}
_prtImgIn.setSaturation(rf.check("saturation",Value(1.0)).asDouble());
_prtImgIn.open(getName("/in"));
_prtImgIn.setPointers(&_prtImgOut,_calibTool);
_prtImgIn.setVerbose(rf.check("verbose"));
_prtImgIn.setLeftEye((strGroup == "CAMERA_CALIBRATION_LEFT") ? true : false);
_prtImgIn.setMaxDelay(maxDelay);
_prtImgIn.setUseIMU(rf.check("useIMU"));
_prtImgIn.setUseIMU(rf.check("useTorso"));
_prtImgIn.setUseIMU(rf.check("useEyes"));
_prtImgIn.useCallback();
_prtImgOut.open(getName("/out"));
_configPort.open(getName("/conf"));
_prtTEncsIn.open(getName("/torso_encs/in"));
_prtTEncsIn._prtImgIn = &_prtImgIn;
// _prtTEncsIn.setStrict();
_prtTEncsIn.useCallback();
_prtHEncsIn.open(getName("/head_encs/in"));
_prtHEncsIn._prtImgIn = &_prtImgIn;
// _prtHEncsIn.setStrict();
_prtHEncsIn.useCallback();
_prtImuIn.open(getName("/imu/in"));
_prtImuIn._prtImgIn = &_prtImgIn;
// _prtImuIn.setStrict();
_prtImuIn.useCallback();
attach(_configPort);
fflush(stdout);
_prtImgIn.rpyPort.open(getName("/rpy"));
return true;
}
bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
{
ConstString str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
verboseExecTime = rf.check("verboseExecTime");
if (rf.check("w_align")) align=ALIGN_WIDTH;
else if (rf.check("h_align")) align=ALIGN_HEIGHT;
if (rf.check("fps"))
{
requested_fps=rf.find("fps").asDouble();
yInfo() << "Module will run at " << requested_fps;
}
else
{
yInfo() << "Module will run at max fps";
}
setName(str.c_str()); // modulePortName
Bottle botLeftConfig(rf.toString().c_str());
Bottle botRightConfig(rf.toString().c_str());
botLeftConfig.setMonitor(rf.getMonitor());
botRightConfig.setMonitor(rf.getMonitor());
string strLeftGroup = "CAMERA_CALIBRATION_LEFT";
if (botLeftConfig.check(strLeftGroup.c_str()))
{
Bottle &group=botLeftConfig.findGroup(strLeftGroup.c_str(),string("Loading configuration from group " + strLeftGroup).c_str());
botLeftConfig.fromString(group.toString());
}
else
{
yError() << "Group " << strLeftGroup << " not found.";
return false;
}
string strRightGroup = "CAMERA_CALIBRATION_RIGHT";
if (botRightConfig.check(strRightGroup.c_str()))
{
Bottle &group=botRightConfig.findGroup(strRightGroup.c_str(),string("Loading configuration from group " + strRightGroup).c_str());
botRightConfig.fromString(group.toString());
}
else
{
yError() << "Group " << strRightGroup << " not found.";
return false;
}
string calibToolLeftName = botLeftConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
string calibToolRightName = botRightConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
calibToolLeft = CalibToolFactories::getPool().get(calibToolLeftName.c_str());
if (calibToolLeft!=NULL)
{
bool ok = calibToolLeft->open(botLeftConfig);
if (!ok)
{
delete calibToolLeft;
calibToolLeft = NULL;
return false;
}
}
calibToolRight = CalibToolFactories::getPool().get(calibToolRightName.c_str());
if (calibToolRight!=NULL)
{
bool ok = calibToolRight->open(botRightConfig);
if (!ok)
{
delete calibToolRight;
calibToolRight = NULL;
return false;
}
}
if(rf.check("dual"))
{
dualImage_mode = true;
yInfo() << "Dual mode activate!!";
}
if(dualImage_mode)
{
leftImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
rightImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
// open a single port with name /dual:i
if (yarp::os::Network::exists(getName("/dual:i")))
{
yWarning() << "port " << getName("/dual:i") << " already in use";
}
if(! imageInLeft.open(getName("/dual:i")) )
return false;
imageInLeft.setStrict(false);
}
else
{
if (yarp::os::Network::exists(getName("/left:i")))
{
yWarning() << "port " << getName("/left:i") << " already in use";
}
if (yarp::os::Network::exists(getName("/right:i")))
{
yWarning() << "port " << getName("/right:i") << " already in use";
}
imageInLeft.open(getName("/left:i"));
imageInRight.open(getName("/right:i"));
imageInLeft.setStrict(false);
imageInRight.setStrict(false);
}
if (yarp::os::Network::exists(getName("/out")))
{
yWarning() << "port " << getName("/out") << " already in use";
}
if (yarp::os::Network::exists(getName("/conf")))
{
yWarning() << "port " << getName("/conf") << " already in use";
}
imageOut.open(getName("/out:o"));
configPort.open(getName("/conf"));
attach(configPort);
return true;
}
virtual bool configure(yarp::os::ResourceFinder &rf)
{
Property options;
options.fromString(rf.toString());
char robotName[255];
Bottle *jointsList=0;
std::string moduleName = "torqueObserver";
Time::turboBoost();
reset_offset = true;
for (int i=0; i<6; i++) initial_offset[i]=0.0;
/* options.put("device", "remote_controlboard");
if(options.check("robot"))
strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
else
strncpy(robotName, "icub", sizeof(robotName));
if (options.check("name"))
{
moduleName = options.find("name").asString();
}
if(options.check("part"))
{
sprintf(partName, "%s", options.find("part").asString().c_str());
sprintf(tmp, "/%s/%s/%s/client", moduleName.c_str(), robotName, partName);
options.put("local",tmp);
sprintf(tmp, "/%s/%s", robotName, partName);
options.put("remote", tmp);
sprintf(tmp, "/%s/%s/rpc", moduleName.c_str(), partName);
rpc_port.open(tmp);
options.put("carrier", "tcp");
attach(rpc_port);
}
else
{
yError("Please specify part (e.g. --part head)\n");
return false;
}
//opening the device driver
if (!driver.open(options))
{
yError("Error opening device, check parameters\n");
return false;
}
bool ret = true;
idir = 0;
icmd = 0;
ret &= driver.view(idir);
ret &= driver.view(icmd);
*/
char pin[255];
sprintf(pin, "/%s/torque:i", moduleName.c_str());
inport.open(pin);
char pout[255];
sprintf(pout, "/%s/torque:o", moduleName.c_str());
outport.open(pout);
bool b1 = yarp::os::Network::connect("/icub/left_leg/analog:o",pin);
bool b2 = yarp::os::Network::connect(pout,"/icub/left_leg/analog:o");
if (!b1)
{
yWarning("failed input connection");
}
if (!b2)
{
yWarning("failed ouput connection");
}
return true;
}
//CTmodified: virtual bool open(yarp::os::Searchable &s)
virtual bool configure(yarp::os::ResourceFinder &rf)
{
//CTmodified: Property options(s.toString());
Property options(rf.toString());
Property stiffnessOptions;
char robotName[255];
Time::turboBoost();
options.put("device", "remote_controlboard");
if(options.check("robot"))
strncpy(robotName, options.find("robot").asString().c_str(),sizeof(robotName));
else
strncpy(robotName, "icub", sizeof(robotName));
if(options.check("part"))
{
char tmp[255];
sprintf(tmp, "/%s/vc/%s/client",
robotName,
options.find("part").asString().c_str());
options.put("local",tmp);
sprintf(tmp, "/%s/%s",
robotName,
options.find("part").asString().c_str());
options.put("remote", tmp);
sprintf(tmp,"/%s/vc/%s/input",
robotName,
options.find("part").asString().c_str());
input_port.open(tmp);
options.put("carrier", "mcast");
//CTmodified: attach(input_port,true); Eliminated: Bufferedport is attaching in the constructor??
}
else
{
fprintf(stderr, "Please specify part (e.g. --part head)\n");
return false;
}
////end of the modif////////////
if (!driver.open(options))
{
fprintf(stderr, "Error opening device, check parameters\n");
return false;
}
///we start the thread
int period = CONTROL_RATE;
if(options.check("period"))
period = options.find("period").asInt();
printf("control rate is %d ms",period);
if (!options.check("part"))
return false;
sprintf(partName, "%s", options.find("part").asString().c_str());
vc=new velImpControlThread(period);
if(options.check("file"))
{
stiffnessOptions.fromConfigFile(options.find("file").asString().c_str());
}
else
{
const char *cubPath;
cubPath = yarp::os::getenv("ICUB_ROOT"); //previously set to ICUB_DIR
if(cubPath == NULL)
{
printf("velImpControl::init>> ERROR getting the environment variable ICUB_DIR, exiting\n");
return false;
}
string cubPathStr(cubPath);
//stiffnessOptions.fromConfigFile((cubPathStr + "/app/crawlingApplication/conf/" + partName + "StiffnessConfig.ini").c_str());
stiffnessOptions.fromConfigFile((cubPathStr + "/contrib/src/crawlingTest/app/conf/" + partName + "StiffnessConfig.ini").c_str());
}
if(stiffnessOptions.check("njoints"))
{
vc->njoints = stiffnessOptions.find("njoints").asInt();
printf("\nControlling %d dofs\n", vc->njoints);
}
else
{
printf("Please specify the number of joints in the config file");
return false;
}
vc->impContr.resize(vc->njoints);
vc->swingStiff.resize(vc->njoints);
vc->stanceStiff.resize(vc->njoints);
vc->swingDamp.resize(vc->njoints);
vc->stanceDamp.resize(vc->njoints);
vc->Grav.resize(vc->njoints);
if(stiffnessOptions.check("ImpJoints"))
{
printf("Joints controlled with impedance: ");
Bottle& bot = stiffnessOptions.findGroup("ImpJoints");
for(int i=0; i<vc->njoints; i++)
{
vc->impContr[i] = bot.get(i+1).asDouble();
printf("%4.2f ", vc->impContr[i]);
}
printf("\n");
}
else
{
printf("Please specify which joints are controlled with impedance in the config file");
return false;
}
if(stiffnessOptions.check("SwingStiff"))
{
printf("Stiffness swing : ");
Bottle& bot = stiffnessOptions.findGroup("SwingStiff");
for(int i=0; i<vc->njoints; i++)
{
vc->swingStiff[i] = bot.get(i+1).asDouble();
printf("%4.2f ", vc->swingStiff[i]);
}
printf("\n");
}
else
{
printf("Please specify the stiffness for the swing in the config file\n");
return false;
}
if(stiffnessOptions.check("StanceStiff"))
{
printf("Stiffness stance: ");
Bottle& bot = stiffnessOptions.findGroup("StanceStiff");
for(int i=0; i<vc->njoints; i++)
{
vc->stanceStiff[i] = bot.get(i+1).asDouble();
printf("%4.2f ", vc->stanceStiff[i]);
}
printf("\n");
}
else
{
printf("Please specify the stiffness for the stance in the config file\n");
return false;
}
if(stiffnessOptions.check("SwingDamp"))
{
printf("Damping swing ");
Bottle& bot = stiffnessOptions.findGroup("SwingDamp");
for(int i = 0; i < vc->njoints; i++)
{
vc->swingDamp[i] = bot.get(i+1).asDouble();
printf("%4.2f ", vc->swingDamp[i]);
}
printf("\n");
}
else
{
printf("Please specify the damping for the swing in the config file\n");
return false;
}
if(stiffnessOptions.check("StanceDamp"))
{
printf("Damping stance: ");
Bottle& bot = stiffnessOptions.findGroup("StanceDamp");
for(int i=0; i<vc->njoints; i++)
{
vc->stanceDamp[i] = bot.get(i+1).asDouble();
printf("%4.2f ", vc->stanceDamp[i]);
}
printf("\n");
}
else
{
printf("Please specify the Dampness for the stance in the config file\n");
return false;
}
if(stiffnessOptions.check("Grav"))
{
printf("Gravity compensation: ");
Bottle& bot = stiffnessOptions.findGroup("Grav");
for(int i=0; i<vc->njoints; i++)
{
vc->Grav[i] = bot.get(i+1).asDouble();
printf("%4.2f ", vc->Grav[i]);
}
printf("\n");
}
else
{
printf("Please specify if gravity compensation in the config file\n");
return false;
}
vc->init(&driver, partName, robotName);
vc->start();
return true;
}
