virtual bool configure(ResourceFinder &rf) { string name=rf.find("name").asString().c_str(); setName(name.c_str()); string partUsed=rf.find("part").asString().c_str(); if ((partUsed!="left_arm") && (partUsed!="right_arm")) { cout<<"Invalid part requested!"<<endl; return false; } Property config; config.fromConfigFile(rf.findFile("from").c_str()); Bottle &bGeneral=config.findGroup("general"); if (bGeneral.isNull()) { cout<<"Error: group general is missing!"<<endl; return false; } // parsing general config options Property option; for (int i=1; i<bGeneral.size(); i++) { Bottle *pB=bGeneral.get(i).asList(); if (pB->size()==2) option.put(pB->get(0).asString().c_str(),pB->get(1)); else { cout<<"Error: invalid option!"<<endl; return false; } } option.put("local",name.c_str()); option.put("part",rf.find("part").asString().c_str()); option.put("grasp_model_type",rf.find("grasp_model_type").asString().c_str()); option.put("grasp_model_file",rf.findFile("grasp_model_file").c_str()); option.put("hand_sequences_file",rf.findFile("hand_sequences_file").c_str()); // parsing arm dependent config options Bottle &bArm=config.findGroup("arm_dependent"); getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x); cout<<"***** Instantiating primitives for "<<partUsed<<endl; action=new AFFACTIONPRIMITIVESLAYER(option); if (!action->isValid()) { delete action; return false; } deque<string> q=action->getHandSeqList(); cout<<"***** List of available hand sequence keys:"<<endl; for (size_t i=0; i<q.size(); i++) cout<<q[i]<<endl; string fwslash="/"; inPort.open((fwslash+name+"/in").c_str()); rpcPort.open((fwslash+name+"/rpc").c_str()); attach(rpcPort); openPorts=true; // check whether the grasp model is calibrated, // otherwise calibrate it and save the results Model *model; action->getGraspModel(model); if (!model->isCalibrated()) { Property prop("(finger all)"); model->calibrate(prop); ofstream fout; fout.open(option.find("grasp_model_file").asString().c_str()); model->toStream(fout); fout.close(); } return true; }
int getDataToDump(ResourceFinder &rf, std::string *listOfData, int n, bool *needDebug) { std::string availableDataToDump[NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP]; std::string availableDebugDataToDump[NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP]; int j; // standard availableDataToDump[0] = "getEncoders"; availableDataToDump[1] = "getEncoderSpeeds"; availableDataToDump[2] = "getEncoderAccelerations"; availableDataToDump[3] = "getPositionErrors"; availableDataToDump[4] = "getOutputs"; availableDataToDump[5] = "getCurrents"; availableDataToDump[6] = "getTorques"; availableDataToDump[7] = "getTorqueErrors"; availableDataToDump[8] = "getPosPidReferences"; availableDataToDump[9] = "getTrqPidReferences"; availableDataToDump[10] = "getControlModes"; availableDataToDump[11] = "getInteractionModes"; availableDataToDump[12] = "getMotorEncoders"; availableDataToDump[13] = "getMotorSpeeds"; availableDataToDump[14] = "getMotorAccelerations"; availableDataToDump[15] = "getTemperatures"; availableDataToDump[16] = "getMotorsPwm"; // debug availableDebugDataToDump[0] = "getRotorPositions"; availableDebugDataToDump[1] = "getRotorSpeeds"; availableDebugDataToDump[2] = "getRotorAccelerations"; if (rf.check("dataToDumpAll")) { for (int i = 0; i < n; i++) listOfData[i] = availableDataToDump[i]; return 1; } if (!rf.check("dataToDump")) { yError("Missing option 'dataToDump' in given config file\n"); return 0; } Value& list = rf.find("dataToDump"); Bottle *pList = list.asList(); for (int i = 0; i < n; i++) { listOfData[i] = pList->get(i).toString(); // check if the requested data is a standard one for (j = 0; j< NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP; j++) { if (listOfData[i] == (availableDataToDump[j])) break; } // check if the requested data is a debug one for (j = 0; j< NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP; j++) { if (listOfData[i] == (availableDebugDataToDump[j])) { *needDebug = true; break; } } // if not found if (j == (NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP + NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP)) { yError("Illegal values for 'dataToDump': %s does not exist!\n",listOfData[i].c_str()); return 0; } } return 1; }
bool readMatrix(const string &tag, Matrix &matrix, const int &dimension, ResourceFinder &rf) { string tag_x=tag+"_x"; string tag_y=tag+"_y"; bool check_x; if(tag=="x0") { if (Bottle *b=rf.find(tag.c_str()).asList()) { Vector col; if (b->size()>=dimension) { for(size_t i=0; i<b->size();i++) col.push_back(b->get(i).asDouble()); matrix.setCol(0, col); } return true; } } else { if(tag=="bounds") { cout<<"in boudn "<<endl; tag_x=tag+"_l"; tag_y=tag+"_u"; } if (Bottle *b=rf.find(tag_x.c_str()).asList()) { cout<<"in boudn 2"<<endl; Vector col; if (b->size()>=dimension) { cout<<"in boudn 3"<<endl; for(size_t i=0; i<b->size();i++) col.push_back(b->get(i).asDouble()); matrix.setCol(0, col); } check_x=true; } if (Bottle *b=rf.find(tag_y.c_str()).asList()) { Vector col; if (b->size()>=dimension) { cout<<"in boudn 4"<<endl; for(size_t i=0; i<b->size();i++) col.push_back(b->get(i).asDouble()); matrix.setCol(1, col); } cout<<"matrix "<<matrix.toString().c_str()<<endl; if(check_x==true) return true; } } return false; }
int main(int argc, char *argv[]) { ResourceFinder rf; rf.configure(argc,argv); double t,t0; string nameFileOut, nameFileSolution; string mu_strategy,nlp_scaling_method; double tol, sum; int acceptable_iter,max_iter; int file_provided; pointClouds pC; nameFileOut=rf.find("nameFileOut").asString().c_str(); if(rf.find("nameFileOut").isNull()) nameFileOut="test"; nameFileSolution=rf.find("nameFileSolution").asString().c_str(); if(rf.find("nameFileSolution").isNull()) nameFileSolution="solution.txt"; tol=rf.find("tol").asDouble(); if(rf.find("tol").isNull()) tol=1e-4; acceptable_iter=rf.find("acceptable_iter").asInt(); if(rf.find("acceptable_iter").isNull()) acceptable_iter=0; max_iter=rf.find("max_iter").asInt(); if(rf.find("max_iter").isNull()) max_iter=2e19; mu_strategy=rf.find("mu_strategy").asString().c_str(); if(rf.find("mu_strategy").isNull()) mu_strategy="monotone"; nlp_scaling_method=rf.find("nlp_scaling_method").asString().c_str(); if(rf.find("nlp_scaling_method").isNull()) nlp_scaling_method="none"; pC.init(rf); file_provided=rf.find("point cloud file").asInt(); if(rf.find("point cloud file").isNull()) file_provided=1; pC.acquirePoints(file_provided,rf); //algorithm settings Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication; app->Options()->SetNumericValue("tol",tol); app->Options()->SetIntegerValue("acceptable_iter",acceptable_iter); app->Options()->SetStringValue("mu_strategy",mu_strategy); app->Options()->SetIntegerValue("max_iter",max_iter); app->Options()->SetStringValue("nlp_scaling_method",nlp_scaling_method); //app->Options()->SetNumericValue("nlp_scaling_max_gradient",10); //app->Options()->SetNumericValue("nlp_scaling_min_value",1e-2); app->Options()->SetStringValue("hessian_approximation","limited-memory"); //app->Options()->SetStringValue("derivative_test","first-order"); //app->Options()->SetStringValue("derivative_test_print_all","yes"); app->Options()->SetStringValue("output_file",nameFileOut+".out"); //app->Options()->SetStringValue("print_user_options","yes"); //app->Options()->SetStringValue("print_options_documentation","no"); app->Options()->SetIntegerValue("print_level",0); app->Initialize(); t0=Time::now(); Ipopt::SmartPtr<SuperQuadric_NLP> superQ_nlp= new SuperQuadric_NLP; superQ_nlp->init(pC); superQ_nlp->configure(rf); Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(superQ_nlp)); t=Time::now()-t0; sum=0.0; for(size_t i=0;i<superQ_nlp->t.size(); i++) { sum=sum+superQ_nlp->t[i]; } if(status==Ipopt::Solve_Succeeded) { Vector sol; sol=superQ_nlp->get_result(); cout<<"The optimal solution is: "<<sol.toString().c_str()<<endl; ofstream fout(nameFileSolution.c_str()); if(fout.is_open()) { fout<<"x: "<<endl<<endl; fout<<sol.toString().c_str()<<endl<<endl; fout<<"t for ipopt: "<<endl<<endl; fout<<t<<endl<<endl; fout<<"average time for each gradient step: "<<endl<<endl; fout<<sum/superQ_nlp->t.size()<<endl<<endl; fout<<"method to compute gradient:"<<endl<<endl; fout<<superQ_nlp->gradient_comp<<endl; fout<<"point cloud: "<<endl; fout<<pC.pointCloudFileName<<endl; } } else cout<<"Problem not solved!"<<endl; cout<<"average time to compute grad: "<<endl; cout<<sum/superQ_nlp->t.size()<<endl; cout<<"t "<<t<<endl; return 0; }
bool saliencyBlobFinderModule::configure(ResourceFinder &rf) { /* Process all parameters from both command-line and .ini file */ if(rf.check("help")) { printf("HELP \n"); printf("====== \n"); printf("--name : changes the rootname of the module ports \n"); printf("--robot : changes the name of the robot where the module interfaces to \n"); printf("--name : rootname for all the connection of the module \n"); printf("--camerasContext : context where camera parameters are stored \n"); printf("--camerasFile : file of parameters of the camera in the context \n"); printf(" \n"); printf("press CTRL-C to stop... \n"); return true; } /* get the module name which will form the stem of all module port names */ moduleName = rf.check("name", Value("/blobFinder/icub/left_cam"), "module name (string)").asString(); /* * before continuing, set the module name before getting any other parameters, * specifically the port names which are dependent on the module name */ setName(moduleName.c_str()); /* * get the thread rate which will define the period of the processing thread */ threadRate = rf.check("ratethread", Value(33), "processing ratethread (int)").asInt(); cout << "Module started with the parameter ratethread: " << threadRate << endl; /* * gets the minBounding area for blob neighbours definition */ minBoundingArea = rf.check("minBoundingArea", Value(225), "minBoundingArea (int)").asInt(); cout << "Module started with min bounding area " << minBoundingArea << endl; if (!cmdPort.open(getName())) { cout << getName() << ": Unable to open port " << endl; return false; } attach(cmdPort); // attach to port /* if (rf.check("config")) { configFile=rf.findFile(rf.find("config").asString().c_str()); if (configFile=="") { return false; } } else { configFile.clear(); } */ if (rf.check("camerasFile")) { if (rf.check("camerasContext")) { printf("found a new context %s \n", rf.find("camerasContext").asString().c_str()); // rf.find("camerasContext").asString().c_str() rf.setDefaultContext("cameraCalibration/conf"); } printf("got the cameraContext %s \n", rf.getContext().c_str()); camerasFile=rf.findFile(rf.find("camerasFile").asString().c_str()); if (camerasFile==""){ return false; } else { printf("found the camerasFile %s \n", camerasFile.c_str()); } } else { camerasFile.clear(); } printf("configFile: %s \n", camerasFile.c_str()); //initialization of the main thread blobFinder = new blobFinderThread(threadRate,camerasFile); blobFinder->setName(this->getName().c_str()); blobFinder->setMinBoundingArea(minBoundingArea); blobFinder->start(); cout << "waiting for connection of the input port" << endl; return true; }
bool BehaviorModule::configure(ResourceFinder &rf) { robotName = rf.find("robot").asString().c_str(); std::cout << robotName << " is the name of our robot" << std::endl; action_left = NULL; action_right = NULL; options_left.put("device", "remote_controlboard"); options_left.put("local", "/pos_ctrl_left_arm"); options_left.put("remote", ("/" + robotName + "/left_arm").c_str()); options_right.put("device", "remote_controlboard"); options_right.put("local", "/pos_ctrl_right_arm"); options_right.put("remote", ("/" + robotName + "/right_arm").c_str()); driver_left.open(options_left); driver_right.open(options_right); options_head.put("device", "remote_controlboard"); options_head.put("local", "/pos_ctrl_head"); options_head.put("remote", ("/" + robotName + "/head").c_str()); driver_head.open(options_head); options_torso.put("device", "remote_controlboard"); options_torso.put("local", "/pos_ctrl_torso"); options_torso.put("remote", ("/" + robotName + "/torso").c_str()); driver_torso.open(options_torso); emotP.open("/local/emoInt"); Network::connect("/local/emoInt", "/icub/face/emotions/in"); // become happy happy(); if (!driver_left.isValid() || !driver_right.isValid() || !driver_head.isValid() || !driver_torso.isValid()) { cerr << "A device is not available. Here are the known devices:" << endl; cerr << Drivers::factory().toString().c_str() << endl; exit(-1); } bool ok = true; ok = ok && driver_left.view(pos_ctrl_left); ok = ok && driver_left.view(encoders_left); ok = ok && driver_right.view(pos_ctrl_right); ok = ok && driver_right.view(encoders_right); ok = ok && driver_head.view(pos_ctrl_head); ok = ok && driver_head.view(encoders_head); ok = ok && driver_torso.view(pos_ctrl_torso); ok = ok && driver_torso.view(encoders_torso); ok = ok && driver_left.view(ictrl_left); ok = ok && driver_left.view(iimp_left); ok = ok && driver_left.view(itrq_left); ok = ok && driver_right.view(ictrl_right); ok = ok && driver_right.view(iimp_right); ok = ok && driver_right.view(itrq_right); chosen_arm = "left"; // needed for impedance controller //ok = ok && driver_right.view(ictrl); //ok = ok && driver_right.view(trq_ctrl_left); //ok = ok && driver_right.view(trq_ctrl_right); options_gaze.put("device", "gazecontrollerclient"); options_gaze.put("remote", "/iKinGazeCtrl"); options_gaze.put("local", "/client/gaze"); driver_gaze.open(options_gaze); driver_gaze.view(igaze); if (driver_gaze.isValid()) { ok = ok && driver_gaze.view(igaze); } igaze->setTrackingMode(false); igaze->setEyesTrajTime(1.0); igaze->setNeckTrajTime(2.0); igaze->bindNeckPitch(-38, 0); igaze->bindNeckRoll(-5, 5); igaze->bindNeckYaw(-30.0, 35.0); if (!ok) { cerr << "error getting interfaces" << std::endl; exit(-1); } int n_jnts = 0; pos_ctrl_left->getAxes(&n_jnts); positions_left_cmd.resize(n_jnts); positions_left_enc.resize(n_jnts); pos_ctrl_right->getAxes(&n_jnts); positions_right_cmd.resize(n_jnts); positions_right_enc.resize(n_jnts); pos_ctrl_head->getAxes(&n_jnts); positions_head_enc.resize(n_jnts); positions_head_cmd.resize(n_jnts); pos_ctrl_torso->getAxes(&n_jnts); positions_torso_enc.resize(n_jnts); positions_torso_cmd.resize(n_jnts); openPorts = false; firstRun = true; sim = false; string name = rf.find("name").asString().c_str(); // setName(name.c_str()); string sim_or_real = rf.find("sim").asString().c_str(); if (sim_or_real == "on") sim = true; else sim = false; Property config; config.fromConfigFile(rf.findFile("from").c_str()); Bottle &bGeneral = config.findGroup("general"); if (bGeneral.isNull()) { cout << "Error: group general is missing!" << endl; return false; } // parsing general config options Property option; for (int i = 1; i < bGeneral.size(); i++) { Bottle *pB = bGeneral.get(i).asList(); if (pB->size() == 2) option.put(pB->get(0).asString().c_str(), pB->get(1)); else { cout << "Error: invalid option!" << endl; return false; } } option.put("local", name.c_str()); option.put("grasp_model_type", rf.find("grasp_model_type").asString().c_str()); option.put("grasp_model_file", rf.findFile("grasp_model_file").c_str()); option.put("hand_sequences_file", rf.findFile("hand_sequences_file").c_str()); printf("%s\n", option.toString().c_str()); // parsing arm dependent config options Bottle &bArm = config.findGroup("arm_dependent"); graspOrien.resize(4); graspDisp.resize(4); dOffs.resize(3); dLift.resize(3); home_x.resize(3); getArmDependentOptions(bArm, graspOrien, graspDisp, dOffs, dLift, home_x); option.put("part", "right_arm"); printf("Options for right arm \n %s\n", option.toString().c_str()); cout << "***** Instantiating primitives for right_arm" << endl; action_right = new ActionPrimitivesLayer2(option); option.put("part", "left_arm"); printf("Options for left arm \n %s\n", option.toString().c_str()); cout << "***** Instantiating primitives for left_arm"<< endl; action_left = new ActionPrimitivesLayer2(option); if (!action_left->isValid()) { delete action_left; cout << "Action_left is not valid" << endl; return false; } if (!action_right->isValid()) { delete action_right; cout << "Action_right is not valid" << endl; return false; } deque<string> q = action_right->getHandSeqList(); cout << "***** List of available for right hand sequence keys:" << endl; for (size_t i = 0; i < q.size(); i++) cout << q[i] << endl; q = action_left->getHandSeqList(); cout << "***** List of available for left hand sequence keys:" << endl; for (size_t i = 0; i < q.size(); i++) cout << q[i] << endl; return true; }
bool AwareTouch::configure(ResourceFinder &rf) { // Defining module string moduleName = rf.check("name", Value("awareTouch")).asString().c_str(); // Check name of the module setName(moduleName.c_str()); // Assign this name for ports cout<< "|| Starting config "<< moduleName <<endl; printf("Naming the module \n"); string robot=rf.check("robot",Value("icubSim")).asString().c_str(); //type of robot cout<< "|| Robot :"<< robot <<endl; // port for skin data string rpcName="/" ; rpcName += getName ( rf.check("inPort",Value("/skin_contacts:i")).asString() ); //input port name // port for output events string eventsName="/" ; eventsName += getName ( rf.check("eventPort",Value("/events:o")).asString() ); //events port name cout<< "|| Opening port :"<< eventsName <<endl; eventsPort.open(eventsName.c_str()); string skinManagerName = rf.check("skinManagerPort", Value("/skinManager/skin_events:o")).asString().c_str(); // Check name of the module string opcName = rf.check("opcName", Value("OPC")).asString().c_str(); //Generating a copy of the world from OPC world = new OPCClient("OPCTouch"); while(!world->connect(opcName)) { cout<< "Trying to connect OPC Server"<<endl; } cout<< "|| Connected OPC :" <<endl; // Generating type of gestures cout<< "|| Reading files of gesture types ... :" <<endl; Bottle * gestureTypes=rf.find("gestureTypes").asList(); cout<< "|| Gesture types ("<< gestureTypes->size()<<") "<<endl; //Populating the world with gestures and subjects gestureSet.clear(); string gestureStr; for (int iGesture=0; iGesture<gestureTypes->size(); iGesture++) { gestureStr=(gestureTypes->get(iGesture).asString().c_str() ); gestureSet.push_back(gestureStr); cout<<gestureStr<<endl; world->addOrRetrieveEntity<Adjective>(gestureStr); } world->addOrRetrieveEntity<Agent>("icub"); touchLocation = world->addOrRetrieveEntity<Object>("touchLocation"); touchLocation->m_present = 0.0; world->commit(touchLocation); world->addOrRetrieveEntity<Action>("is"); world->addOrRetrieveEntity<Adjective>("none"); // holding time in OPC recordingPeriod=rf.check("recordingPeriod",Value(3.0)).asDouble(); //type of robot cout << "|| Recording Period is " << recordingPeriod << endl ; string pathG(rf.getHomeContextPath().c_str()); cout<< "|| Creating Touch Thread:" <<endl; estimationThread= new TouchEstimationThread(skinManagerName, rpcName, pathG, gestureSet, 50); cout<< "|| Starting Touch :" <<endl; estimationThread->start(); cout<< "|| Started Touch :" <<endl; return true; }
bool configure(ResourceFinder &rf) { string name=rf.find("name").asString().c_str(); string robot=rf.find("robot").asString().c_str(); string hand=rf.find("hand").asString().c_str(); string modelType=rf.find("modelType").asString().c_str(); fingerName=rf.find("finger").asString().c_str(); if (fingerName=="thumb") joint=10; else if (fingerName=="index") joint=12; else if (fingerName=="middle") joint=14; else if (fingerName=="ring") joint=15; else if (fingerName=="little") joint=15; else { fprintf(stdout,"unknown finger!\n"); return false; } Property driverOpt("(device remote_controlboard)"); driverOpt.put("remote",("/"+robot+"/"+hand+"_arm").c_str()); driverOpt.put("local",("/"+name).c_str()); if (!driver.open(driverOpt)) return false; driver.view(ipos); driver.view(ienc); IControlLimits *ilim; driver.view(ilim); ilim->getLimits(joint,&min,&max); double margin=0.1*(max-min); min=min+margin; max=max-margin; val=&min; Property genOpt; genOpt.put("name",(name+"/"+modelType).c_str()); genOpt.put("robot",robot.c_str()); genOpt.put("type",hand.c_str()); genOpt.put("verbose",1); string general(genOpt.toString().c_str()); string thumb( "(thumb (name thumb))"); string index( "(index (name index))"); string middle("(middle (name middle))"); string ring( "(ring (name ring))"); string little("(little (name little))"); Property options((general+" "+thumb+" "+index+" "+middle+" "+ring+" "+little).c_str()); fprintf(stdout,"configuring options: %s\n",options.toString().c_str()); if (modelType=="springy") model=new SpringyFingersModel; else if (modelType=="tactile") model=new TactileFingersModel; else { fprintf(stdout,"unknown model type!\n"); return false; } if (model->fromProperty(options)) return true; else { delete model; return false; } }
bool configure(ResourceFinder &rf) override { portName=rf.check("name",Value("/dump")).asString(); if (portName[0]!='/') portName="/"+portName; bool saveData=true; bool videoOn=false; string videoType=rf.check("videoType",Value("mkv")).asString(); if (rf.check("type")) { string optTypeName=rf.find("type").asString(); if (optTypeName=="bottle") type=bottle; else if (optTypeName=="image") { type=image; #ifdef ADD_VIDEO if (rf.check("addVideo")) videoOn=true; #endif } else if (optTypeName == "image_jpg") { type=image; save_jpeg = true; } #ifdef ADD_VIDEO else if (optTypeName=="video") { type=image; videoOn=true; saveData=false; } #endif else { yError() << "Error: invalid type"; return false; } } else type=bottle; dwnsample=rf.check("downsample",Value(1)).asInt32(); rxTime=rf.check("rxTime"); txTime=rf.check("txTime"); string templateDirName=rf.check("dir")?rf.find("dir").asString():portName; if (templateDirName[0]!='/') templateDirName="/"+templateDirName; string dirName; if (rf.check("overwrite")) dirName="."+templateDirName; else { // look for a proper directory int i=0; do { ostringstream checkDirName; if (i>0) checkDirName << "." << templateDirName << "_" << setw(5) << setfill('0') << i; else checkDirName << "." << templateDirName; dirName=checkDirName.str(); i++; } while (!yarp::os::stat(dirName.c_str())); } yarp::os::mkdir_p(dirName.c_str()); q=new DumpQueue(); t=new DumpThread(type,*q,dirName,100,saveData,videoOn,videoType); if (!t->start()) { delete t; delete q; return false; } reporter.setThread(t); if (type==bottle) { p_bottle=new DumpPort<Bottle>(*q,dwnsample,rxTime,txTime); p_bottle->useCallback(); p_bottle->open(portName); p_bottle->setStrict(); p_bottle->setReporter(reporter); } else { p_image=new DumpPort<Image>(*q,dwnsample,rxTime,txTime); p_image->useCallback(); p_image->open(portName); p_image->setStrict(); p_image->setReporter(reporter); } if (rf.check("connect")) { string srcPort=rf.find("connect").asString(); bool ok=Network::connect(srcPort.c_str(), (type==bottle)?p_bottle->getName().c_str(): p_image->getName().c_str(),"tcp"); ostringstream msg; msg << "Connection to " << srcPort << " " << (ok?"successful":"failed"); if (ok) yInfo() << msg.str(); else yWarning() << msg.str(); } // this port serves to handle the "quit" rpc command rpcPort.open(portName+"/rpc"); attach(rpcPort); yInfo() << "Service yarp port: " << portName; yInfo() << "Data stored in : " << dirName; return true; }
//Gathering resources for initialization bool configure(ResourceFinder &rf) { string name = rf.find("name").asString().c_str(); setName(name.c_str()); Property config; config.fromConfigFile(rf.findFile("from").c_str()); Bottle &bGeneral = config.findGroup("general"); if (bGeneral.isNull()) { cout << "Error: group general is missing!" << endl; return false; } // parsing general config options from config.ini file Property option(bGeneral.toString().c_str()); option.put("local", name.c_str()); option.put("part", "left_arm"); option.put("grasp_model_type", rf.find("grasp_model_type").asString().c_str()); option.put("grasp_model_file", rf.findFile("grasp_model_file").c_str()); option.put("hand_sequences_file", rf.findFile("hand_sequences_file").c_str()); // parsing arm dependent config options from config.ini file Bottle &bArm = config.findGroup("arm_dependent"); getArmDependentOptions(bArm, graspOrien, graspDisp, dOffs, dLift, home_x); cout << "***** Instantiating primitives for left arm" << endl; action = new AFFACTIONPRIMITIVESLAYER(option); if (!action->isValid()) { delete action; return false; } //createing YARP ports and connections deque <string> q = action->getHandSeqList(); cout << "***** List of available hand sequence keys:" << endl; for (size_t i = 0; i < q.size(); i++) cout << q[i] << endl; string fwslash = "/"; inPort.open((fwslash + name + "/in").c_str()); rpcPort.open((fwslash + name + "/rpc").c_str()); attach(rpcPort); // check for graspCalibration Model *model; action->getGraspModel(model); if (model != NULL) { if (!model->isCalibrated()) { Property prop("(finger all_parallel)"); model->calibrate(prop); string fileName = rf.getHomeContextPath(); fileName += "/"; fileName += option.find("grasp_model_file").asString().c_str(); ofstream fout; fout.open(fileName.c_str()); model->toStream(fout); fout.close(); } } return true; }
int main(int argc, char **argv) { using namespace yarp::os; ResourceFinder resourceFinder = ResourceFinder::getResourceFinderSingleton(); resourceFinder.configure(argc, argv); if (resourceFinder.check("help")) { std::cout<< "Possible parameters" << std::endl << std::endl; std::cout<< "\t--context :Where to find a user defined .ini file within $ICUB_ROOT/app e.g. /adaptiveControl/conf" << std::endl; std::cout<< "\t--robotURDFFile :URDF file name" << std::endl; std::cout<< "\t--comDes :Desired CoM of the robot." << std::endl; std::cout<< "\t--qDes :Desired joint positions of the robot." << std::endl; std::cout<< "\t--feetInSupport :left, right or both" << std::endl; std::cout<< "\t--jointMapping :[optional]ordered list of joints name which should be used in the optimization. Size must match size of qDes. If missing all joints are assumed" << std::endl; return 0; } //read model file name std::string filename = resourceFinder.check("robotURDFFile", Value("model.urdf"), "Checking for model URDF file").asString(); std::string filepath = resourceFinder.findFileByName(filename); yInfo() << "Robot model found in " << filepath; //read desired CoM if (!resourceFinder.check("comDes", "Checking desired CoM parameter")) { yError("Parameter comDes is required"); return -1; } Value &comDes = resourceFinder.find("comDes"); //Start checking: it should be a list of 3 if (!comDes.isList() || comDes.asList()->size() != 3) { yError("Number of elements in comDes parameter is wrong. Expecting 3 values"); return -2; } yarp::sig::Vector desiredCoM(3); Bottle* comList = comDes.asList(); desiredCoM[0] = comList->get(0).asDouble(); desiredCoM[1] = comList->get(1).asDouble(); desiredCoM[2] = comList->get(2).asDouble(); yInfo() << "Desired CoM is: " << desiredCoM.toString(); //read desired Joints configuration if (!resourceFinder.check("qDes", "Checking desired joint configuration parameter")) { yError("Parameter qDes is required"); return -1; } Value &qDes = resourceFinder.find("qDes"); if (!qDes.isList()) { yError("qDes parameter should be a list"); return -2; } Bottle *qDesBottle = qDes.asList(); yarp::sig::Vector desiredJoints(qDesBottle->size()); for (unsigned i = 0; i < qDesBottle->size(); i++) { desiredJoints[i] = qDesBottle->get(i).asDouble(); } using namespace yarp::math; yInfo() << "Desired joint configuration is (rad): " << desiredJoints.toString(); yInfo() << "Desired joint configuration is (deg): " << (desiredJoints * (180.0/M_PI)).toString(); //read initial Joints configuration Value &qInit = resourceFinder.find("qInit"); Bottle *qInitBottle = 0; yarp::sig::Vector initialJoints(0); if (!qInit.isNull()) { if (!qInit.isList()) { yError("qInit parameter should be a list"); return -2; } qInitBottle = qInit.asList(); initialJoints.resize(qInitBottle->size()); for (unsigned i = 0; i < qInitBottle->size(); i++) { initialJoints[i] = qInitBottle->get(i).asDouble(); } yInfo() << "Initial joint configuration is: " << initialJoints.toString(); } //read which foot/feet is in support if (!resourceFinder.check("feetInSupport", "Checking feet in support parameter")) { yError("Parameter feetInSupport is required"); return -1; } Value &feet = resourceFinder.find("feetInSupport"); std::string feetInSupport = feet.asString(); yInfo() << "Feet in support is: " << feetInSupport; std::vector<std::string> mapping; if (resourceFinder.check("jointMapping", "Checking joint mapping parameter")) { Bottle *mappingBottle = resourceFinder.find("jointMapping").asList(); if (!mappingBottle || mappingBottle->size() != desiredJoints.size()) { yError("jointMapping parameter list has wrong size."); return -2; } mapping.reserve(mappingBottle->size()); for (int i = 0; i < mappingBottle->size(); i++) { mapping.push_back(mappingBottle->get(i).asString()); } yInfo() << "Joint mapping is (index => joint name): " << mapping; } else { yInfo("Joint mapping not specified"); } OptimProblem problem; if (!problem.initializeModel(filepath, mapping)) { yError("Error initializing the robot model"); return -2; } problem.solveOptimization(desiredCoM, desiredJoints, feetInSupport); return 0; }
bool gOcraControllerModule::configure(ResourceFinder &rf) { //--------------------------READ FROM CONFIGURATION---------------------- if( rf.check("robot") ) { robotName = rf.find("robot").asString().c_str(); std::cout <<"robot name is " << robotName << std::endl; } if( rf.check("local") ) { moduleName = rf.find("local").asString().c_str(); } if( rf.check("replay") ) { replayJointAnglesPath = rf.find("replay").asString().c_str(); } yarp::os::Property yarpWbiOptions; // Get wbi options from the canonical file if ( !rf.check("wbi_conf_file") ) { fprintf(stderr, "[ERR] gOcraController: Impossible to open wholeBodyInterface: wbi_conf_file option missing"); } std::string wbiConfFile = rf.findFile("wbi_conf_file"); yarpWbiOptions.fromConfigFile(wbiConfFile); // Overwrite the robot parameter that could be present in wbi_conf_file yarpWbiOptions.put("robot", robotName); robotInterface = new yarpWholeBodyInterface(moduleName.c_str(), yarpWbiOptions); IDList robotJoints; std::string robotJointsListName = "ROBOT_MAIN_JOINTS"; if(!loadIdListFromConfig(robotJointsListName, yarpWbiOptions, robotJoints)) { fprintf(stderr, "[ERR] gOcraController: Impossible to load wbiId joint list with name %s\n", robotJointsListName.c_str()); } robotInterface->addJoints(robotJoints); if( rf.check("uses_external_torque_control") ) { /* if(yarp::os::NetworkBase::exists(string("/jtc/info:o").c_str())) printf ("The module jointTorqueControl is running. Proceeding with configuration of the interface...\n"); else{ printf ("ERROR [mdlStart] >> The jointTorqueControl module is not running... \n"); return false; } yarp::os::Value trueValue; trueValue.fromString ("true"); ( (yarpWholeBodyInterface*) robotInterface)->setActuactorConfigurationParameter (icubWholeBodyActuators::icubWholeBodyActuatorsUseExternalTorqueModule, trueValue); ( (yarpWholeBodyInterface*) robotInterface)->setActuactorConfigurationParameter (icubWholeBodyActuators::icubWholeBodyActuatorsExternalTorqueModuleAutoconnect, trueValue); ( (yarpWholeBodyInterface*) robotInterface)->setActuactorConfigurationParameter (icubWholeBodyActuators::icubWholeBodyActuatorsExternalTorqueModuleName, Value ("jtc")); */ } // Make sure all the add* functions are done before the "init" if(!robotInterface->init()) { fprintf(stderr, "Error while initializing whole body interface. Closing module\n"); return false; } //--------------------------CTRL THREAD-------------------------- yarp::os::Property controller_options; //If the printPeriod is found in the options, send it to the controller if( rf.check("printPeriod") && rf.find("printPeriod").isDouble() ) { controller_options.put("printPeriod",rf.find("printPeriod").asDouble()); } ctrlThread = new gOcraControllerThread(moduleName, robotName, period, robotInterface, controller_options, replayJointAnglesPath); if(!ctrlThread->start()){ fprintf(stderr, "Error while initializing control thread. Closing module.\n"); return false; } fprintf(stderr,"gOcraController thread started\n"); return true; }
virtual bool configure(ResourceFinder &rf) { Time::turboBoost(); fprintf(stderr, "Getting projections\n"); Matrix PiRight; Bottle b; b = rf.findGroup("CAMERA_CALIBRATION_RIGHT"); //fprintf(stderr, "CAMERA_CALIBRATION_RIGHT contains: %s\n", b.toString().c_str()); if (getProjectionMatrix(b, PiRight) == 0) { fprintf(stderr, "CAMERA_CALIBRATION_RIGHT was missing some params\n"); return false; } else { fprintf(stderr, "Working with RightProjection \n"); for (int i=0; i < PiRight.rows(); i++) fprintf(stderr, "%s\n", PiRight.getRow(i).toString().c_str()); } Matrix PiLeft; b = rf.findGroup("CAMERA_CALIBRATION_LEFT"); //fprintf(stderr, "CAMERA_CALIBRATION_LEFT contains: %s\n", b.toString().c_str()); if (getProjectionMatrix(b, PiLeft) == 0) { fprintf(stderr, "CAMERA_CALIBRATION_LEFT was missing some params\n"); return false; } else { fprintf(stderr, "Working with LeftProjection \n"); for (int i=0; i < PiLeft.rows(); i++) fprintf(stderr, "%s\n", PiLeft.getRow(i).toString().c_str()); } int period=50; if (rf.check("period")) period=rf.find("period").asInt(); bool enableKalman=false; if (rf.check("kalman")) enableKalman=true; string ctrlName=rf.find("name").asString().c_str(); string robotName=rf.find("robot").asString().c_str(); fprintf(stderr, "Initializing eT\n"); eT=new eyeTriangulation(rf, PiLeft, PiRight, enableKalman, period, ctrlName, robotName); Vector xr(3); xr(0)=PiRight(0,2); xr(1)=PiRight(1,2); xr(2)=1.0; Vector xl(3); xl(0)=PiLeft(0,2); xl(1)=PiLeft(1,2); xl(2)=1.0; eT->xInit(xl, xr); if (rf.check("const")) eT->xSetConstant(xl, xr); eT->start(); string rpcPortName="/"+ctrlName+"/rpc"; rpcPort.open(rpcPortName.c_str()); attach(rpcPort); return true; }