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; }