int main(int argc, char *argv[]) { if (argc<3) { fprintf(stderr, "Please supply (1) a port name for the client\n"); fprintf(stderr, " (2) a port name for the server\n"); return 1; } Network yarp; const char *client_name = argv[1]; const char *server_name = argv[2]; RpcClient port; port.open(client_name); int ct = 0; while (true) { if (port.getOutputCount()==0) { printf("Trying to connect to %s\n", server_name); yarp.connect(client_name,server_name); } else { Bottle cmd; cmd.addString("COUNT"); cmd.addInt32(ct); ct++; printf("Sending message... %s\n", cmd.toString().c_str()); Bottle response; port.write(cmd,response); printf("Got response: %s\n", response.toString().c_str()); } Time::delay(1); } }
int main(int argc, char *argv[]) { if (argc!=3) { fprintf(stderr,"Call as %s X Y\n", argv[0]); return 1; } Network yarp; RpcClient client; yarp_test::AddTwoInts example; client.promiseType(example.getType()); if (!client.open("/add_two_ints@/yarp_add_int_client")) { fprintf(stderr,"Failed to open port\n"); return 1; } yarp_test::AddTwoInts msg; yarp_test::AddTwoIntsReply reply; msg.a = atoi(argv[1]); msg.b = atoi(argv[2]); if (!client.write(msg,reply)) { fprintf(stderr,"Failed to call service\n"); return 1; } printf("Got %d\n", (int)reply.sum); return 0; }
bool configure(ResourceFinder &rf) { rf1=rf; if(algorithm==1) loc5=new UnscentedParticleFilter(); else loc5=new ScalingSeries(); loc5->configure(rf1); state=0; meas_given=1; if(loc5->measurementsString!=1) { namePorts = rf.check("namePorts", Value("visual-localization"), "Getting module name").asString(); portIn.open("/"+namePorts+"/pnt:i"); rpcOut.open("/"+namePorts+"/toolFeat:rpc"); meas_given=0; } cout<<"meas_given "<<meas_given<<endl; error_thres=loc5->error_thres; cout<<"thresconf "<<error_thres<<endl; rpcPort.open(("/"+namePorts+"/rpc").c_str()); attach(rpcPort); delete loc5; return true; }
int main(int argc, char *argv[]) { if (argc!=3) { fprintf(stderr,"Call as %s X Y\n", argv[0]); return 1; } Network yarp; Node node("/yarp_add_int_client"); RpcClient client; if (!client.open("add_two_ints")) { fprintf(stderr,"Failed to open client\n"); return 1; } Bottle msg, reply; msg.addInt(atoi(argv[1])); msg.addInt(atoi(argv[2])); if (!client.write(msg,reply)) { fprintf(stderr,"Failed to call service\n"); return 1; } printf("Got %d\n", reply.get(0).asInt()); return 0; }
void configure(ResourceFinder &rf) { string name=rf.find("name").asString().c_str(); string robot=rf.find("robot").asString().c_str(); emotions.open(("/"+name+"/emotions:o").c_str()); state="sur"; setRate(rf.check("period",Value(200)).asInt()); }
bool configure(ResourceFinder &rf) { portDispIn.open("/3d-points/disp:i"); portDispOut.open("/3d-points/disp:o"); portImgIn.open("/3d-points/img:i"); portOutPoints.open("/3d-points/pnt:o"); portContour.open("/3d-points/contour:i"); portSFM.open("/3d-points/SFM:rpc"); portRpc.open("/3d-points/rpc"); portContour.setReader(*this); attach(portRpc); homeContextPath=rf.getHomeContextPath().c_str(); downsampling=std::max(1,rf.check("downsampling",Value(1)).asInt()); spatial_distance=rf.check("spatial_distance",Value(0.004)).asDouble(); color_distance=rf.check("color_distance",Value(6)).asInt(); go=flood3d=flood=false; return true; }
bool configure(ResourceFinder &rf) { Time::turboBoost(); string name=rf.find("name").asString().c_str(); port.open(("/"+name+":o").c_str()); if (!Network::connect(port.getName().c_str(),"/icubSim/world")) { cout<<"unable to connect to the world!"<<endl; port.close(); return false; } x0=0.0; y0=1.0; z0=0.75; init=true; return true; }
bool configure(ResourceFinder &rf) { printf(" Starting Configure\n"); this->rf=&rf; //****************** PARAMETERS ****************** robot = rf.check("robot",Value("icub")).asString().c_str(); name = rf.check("name",Value("demoINNOROBO")).asString().c_str(); arm = rf.check("arm",Value("right_arm")).asString().c_str(); verbosity = rf.check("verbosity",Value(0)).asInt(); rate = rf.check("rate",Value(0.5)).asDouble(); if (arm!="right_arm" && arm!="left_arm") { printMessage(0,"ERROR: arm was set to %s, putting it to right_arm\n",arm.c_str()); arm="right_arm"; } printMessage(1,"Parameters correctly acquired\n"); printMessage(1,"Robot: %s \tName: %s \tArm: %s\n",robot.c_str(),name.c_str(),arm.c_str()); printMessage(1,"Verbosity: %i \t Rate: %g \n",verbosity,rate); //****************** DETECTOR ****************** certainty = rf.check("certainty", Value(10.0)).asInt(); strCascade = rf.check("cascade", Value("haarcascade_frontalface_alt.xml")).asString().c_str(); detectorL=new Detector(); detectorL->open(certainty,strCascade); detectorR=new Detector(); detectorR->open(certainty,strCascade); printMessage(1,"Detectors opened\n"); //****************** DRIVERS ****************** Property optionArm("(device remote_controlboard)"); optionArm.put("remote",("/"+robot+"/"+arm).c_str()); optionArm.put("local",("/"+name+"/"+arm).c_str()); if (!drvArm.open(optionArm)) { printf("Position controller not available!\n"); return false; } Property optionCart("(device cartesiancontrollerclient)"); optionCart.put("remote",("/"+robot+"/cartesianController/"+arm).c_str()); optionCart.put("local",("/"+name+"/cart/").c_str()); if (!drvCart.open(optionCart)) { printf("Cartesian controller not available!\n"); // return false; } Property optionGaze("(device gazecontrollerclient)"); optionGaze.put("remote","/iKinGazeCtrl"); optionGaze.put("local",("/"+name+"/gaze").c_str()); if (!drvGaze.open(optionGaze)) { printf("Gaze controller not available!\n"); // return false; } // quitting conditions bool andArm=drvArm.isValid()==drvCart.isValid()==drvGaze.isValid(); if (!andArm) { printMessage(0,"Something wrong occured while configuring drivers... quitting!\n"); return false; } drvArm.view(iencs); drvArm.view(iposs); drvArm.view(ictrl); drvArm.view(iimp); drvCart.view(iarm); drvGaze.view(igaze); iencs->getAxes(&nEncs); iimp->setImpedance(0, 0.4, 0.03); iimp->setImpedance(1, 0.35, 0.03); iimp->setImpedance(2, 0.35, 0.03); iimp->setImpedance(3, 0.2, 0.02); iimp->setImpedance(4, 0.2, 0.00); ictrl -> setImpedancePositionMode(0); ictrl -> setImpedancePositionMode(1); ictrl -> setImpedancePositionMode(2); ictrl -> setImpedancePositionMode(3); ictrl -> setImpedancePositionMode(2); ictrl -> setImpedancePositionMode(4); igaze -> storeContext(&contextGaze); igaze -> setSaccadesStatus(false); igaze -> setNeckTrajTime(0.75); igaze -> setEyesTrajTime(0.5); iarm -> storeContext(&contextCart); printMessage(1,"Drivers opened\n"); //****************** PORTS ****************** imagePortInR -> open(("/"+name+"/imageR:i").c_str()); imagePortInL -> open(("/"+name+"/imageL:i").c_str()); imagePortOutR.open(("/"+name+"/imageR:o").c_str()); imagePortOutL.open(("/"+name+"/imageL:o").c_str()); portOutInfo.open(("/"+name+"/info:o").c_str()); if (robot=="icub") { Network::connect("/icub/camcalib/left/out",("/"+name+"/imageL:i").c_str()); Network::connect("/icub/camcalib/right/out",("/"+name+"/imageR:i").c_str()); } else { Network::connect("/icubSim/cam/left",("/"+name+"/imageL:i").c_str()); Network::connect("/icubSim/cam/right",("/"+name+"/imageR:i").c_str()); } Network::connect(("/"+name+"/imageL:o").c_str(),"/demoINN/left"); Network::connect(("/"+name+"/imageR:o").c_str(),"/demoINN/right"); Network::connect(("/"+name+"/info:o").c_str(),"/iSpeak"); rpcClnt.open(("/"+name+"/rpc:o").c_str()); rpcSrvr.open(("/"+name+"/rpc:i").c_str()); attach(rpcSrvr); printMessage(0,"Configure Finished!\n"); return true; }
/* * Configure function. Receive a previously initialized * resource finder object. Use it to configure your module. * Open port and attach it to message handler. */ bool configure(yarp::os::ResourceFinder &rf) { /* list.push_back("The market is open."); // 4 list.push_back("The orange is sweet."); // 4 list.push_back("I love to play guitar."); // 5 list.push_back("He is in the kitchen."); // 5 list.push_back("I can speak English."); // 4 list.push_back("He is going home."); // 4 list.push_back("Tom is a funny man."); // 5 list.push_back("I have three apples."); // 4 list.push_back("The music; was good."); // 4 list.push_back("I am not a robot."); // 5 list.push_back("She is very pretty."); // 4 list.push_back("This song is great."); // 4 list.push_back("My friend has a horse."); // 5 list.push_back("The apple tasted good."); // 4 list.push_back("I like red flowers."); // 4 list.push_back("My horse runs very fast."); // 5 list.push_back("I have a nice house."); // 5 list.push_back("Jill wants a doll."); // 4 list.push_back("I have a computer."); // 4 list.push_back("Tom is stronger than Dan."); // 5 list.push_back("We, sing; a song."); // 4 list.push_back("I was very happy yesterday."); // 5 list.push_back("He eats white bread."); // 4 list.push_back("Jack wants a toy."); // 4 list.push_back("She is in the shower."); // 5 list.push_back("The baby plays with toys."); // 5 list.push_back("She is a teacher."); // 4 list.push_back("I have a nice box."); // 5 list.push_back("You look very happy."); // 4 list.push_back("The baby fell asleep."); // 4 list.push_back("They are; my friends."); // 4 list.push_back("I went to school."); // 4 */ english = 1; if(english) { list.push_back("Benvenuti."); list.push_back("Io sono AICAB."); list.push_back("Sono felice di vederti."); list.push_back("Ciao."); list.push_back("Come va?"); list.push_back("Questa e casa mia."); list.push_back("Io sono un robot."); list.push_back("Ciao."); /* list.push_back("Hi, how are you?"); list.push_back("Hello."); list.push_back("I'm looking at you."); list.push_back("How is it going?"); list.push_back("Hello, I'm eye cub."); list.push_back("Welcome to my home."); list.push_back("Hi there."); list.push_back("Hello."); */ /* list.push_back("We sing a song."); list.push_back("I was very happy yesterday."); list.push_back("I have a computer."); list.push_back("Jill wants a doll."); list.push_back("He eats white bread."); list.push_back("I love to play guitar."); list.push_back("He is in the kitchen."); list.push_back("She has a nice bike."); list.push_back("I went to school."); list.push_back("Tom is stronger than Dan."); list.push_back("She is a teacher."); list.push_back("I like red flowers."); list.push_back("The music was good."); list.push_back("The apple tasted good."); list.push_back("You look very happy."); list.push_back("I have three apples."); list.push_back("Jack wants a toy."); list.push_back("The baby plays with toys."); list.push_back("I have a nice box."); list.push_back("This song is great."); list.push_back("Tom is a funny man."); list.push_back("My friend has a horse."); list.push_back("The baby fell asleep."); list.push_back("I can speak English."); list.push_back("I am not a robot."); list.push_back("My horse runs very fast."); list.push_back("They are going home."); list.push_back("She is very pretty."); list.push_back("The market is open."); list.push_back("She is in the shower."); list.push_back("They are; my friends."); list.push_back("The apple is sweet.");*/ } else { list.push_back("Il bar è aperto."); list.push_back("Mi piace molto ballare."); list.push_back("Non bevo il caffè."); list.push_back("Marco ha tanti cani."); list.push_back("Il mare è calmo."); list.push_back("Il cane abbaia spesso."); list.push_back("Sono allergico al latte."); list.push_back("La sedia è in camera."); list.push_back("La luce è rossa."); list.push_back("Lui ha sempre ragione."); list.push_back("Giovedí scorso era festa."); list.push_back("Oggi splende il sole."); list.push_back("Ho comprato il pane."); list.push_back("Guido tutti i giorni."); list.push_back("Vado spesso al mare."); list.push_back("La notte è buia."); list.push_back("Mi piace il gelato."); list.push_back("Devo scrivere un tema."); list.push_back("Lei ama cucire."); list.push_back("Io ballo la polka."); list.push_back("Gino canta molto bene."); list.push_back("Vado a dormire presto."); list.push_back("La lettera è firmata."); list.push_back("Gioco spesso a carte."); list.push_back("Mi piace l'opera."); list.push_back("Ho mangiato le mele."); list.push_back("Sua nonna sta bene."); list.push_back("Ho una maglia blu."); list.push_back("Il piatto è sul tavolo."); list.push_back("La giornata è piovosa."); list.push_back("Il film dura due ore."); list.push_back("La bottiglia è piena."); } gazeCount=0; speech_counter=0; online=1; prev = Time::now(); waittime=5.0; withgaze=1; prevyes=0; state=1; order=2; initstate=1; prevStr = "quiet"; donespeaking=1; wordnumber=0; charnumber=0; prevgaze=0; facecounter=0; gazelength=0.0; gazelen=0.0; motorson=1; firstspeech=0; if(motorson==1) { Property optGaze("(device gazecontrollerclient)"); optGaze.put("remote","/iKinGazeCtrl"); optGaze.put("local","/icub_eyetrack/gaze"); printf("\nHello.\n"); if (!clientGaze.open(optGaze)) { printf("\nGAZE FAILED\n"); return false; } else printf("\nGAZE OPEN\n"); clientGaze.view(igaze); igaze->blockNeckRoll(0.0); igaze->setNeckTrajTime(0.8); igaze->setEyesTrajTime(0.4); yarp::sig::Vector azelr(3); /// azelr[0] = -30.0; /// azelr[1] = 0.0; azelr[0] = 0.0; azelr[1] = 20.0; azelr[2] = 0.0; igaze->lookAtAbsAngles(azelr); } handlerPort.open("/dictationcontroller"); gazeIn.open( "/dictationcontroller/gaze:i" ); speechOut.open( "/dictationcontroller/speech:o" ); logOut.open( "/dictationcontroller/log:o" ); attach(handlerPort); Network::connect("/dlibgazer/out", "/dictationcontroller/gaze:i"); Network::connect("/dictationcontroller/speech:o", "/iSpeak"); speechStatusPort.open("/dictationcontroller/iSpeakrpc"); Network::connect("/dictationcontroller/iSpeakrpc", "/iSpeak/rpc"); frame_counter=0; cout<<"Done configuring!" << endl; return true; }
void LoggerEngine::discover (std::list<std::string>& ports) { RpcClient p; string logger_portname = log_updater->getPortName(); p.open(logger_portname+"/discover"); std::string yarpservername = yarp::os::Network::getNameServerName(); yarp::os::Network::connect(logger_portname+"/discover",yarpservername.c_str()); Bottle cmd,response; cmd.addString("bot"); cmd.addString("list"); p.write(cmd,response); printf ("%s\n\n", response.toString().c_str()); int size = response.size(); for (int i=1; i<size; i++) //beware: skip i=0 is intentional! { Bottle* n1 = response.get(i).asList(); if (n1 && n1->get(0).toString()=="port") { Bottle* n2 = n1->get(1).asList(); if (n2 && n2->get(0).toString()=="name") { char* log_off = nullptr; char* yarprun_log_off = nullptr; log_off = std::strstr((char*)(n2->get(1).toString().c_str()), "/log/"); if (log_off) { std::string logport = n2->get(1).toString(); printf ("%s\n", logport.c_str()); ports.push_back(logport); } yarprun_log_off = std::strstr((char*)(n2->get(1).toString().c_str()), "/yarprunlog/"); if (yarprun_log_off) { std::string logport = n2->get(1).toString(); printf ("%s\n", logport.c_str()); ports.push_back(logport); } } } } std::list<std::string>::iterator ports_it; for (ports_it=ports.begin(); ports_it!=ports.end(); ports_it++) { LogEntry entry; entry.logInfo.port_complete = (*ports_it); yarp::os::Contact contact = yarp::os::Network::queryName(entry.logInfo.port_complete); if (contact.isValid()) { entry.logInfo.ip_address = contact.getHost(); } else { printf("ERROR: invalid contact: %s\n", entry.logInfo.port_complete.c_str()); } std::istringstream iss(*ports_it); std::string token; getline(iss, token, '/'); getline(iss, token, '/'); getline(iss, token, '/'); entry.logInfo.port_prefix = "/"+ token; getline(iss, token, '/'); entry.logInfo.process_name = token; getline(iss, token, '/'); entry.logInfo.process_pid = token; std::list<LogEntry>::iterator it; this->log_updater->mutex.lock(); bool found = false; for (it = this->log_updater->log_list.begin(); it != log_updater->log_list.end(); it++) { if (it->logInfo.port_complete==entry.logInfo.port_complete) { found=true; break; } } if (found==false) { log_updater->log_list.push_back(entry); } this->log_updater->mutex.unlock(); } }
bool configure(ResourceFinder &rf) { name = "visuoTactileRF"; robot = "icub"; modality = "1D"; verbosity = 0; // verbosity rate = 20; // rate of the vtRFThread //****************************************************** //********************** CONFIGS *********************** //******************* NAME ****************** if (rf.check("name")) { name = rf.find("name").asString(); yInfo("Module name set to %s", name.c_str()); } else yInfo("Module name set to default, i.e. %s", name.c_str()); setName(name.c_str()); //******************* ROBOT ****************** if (rf.check("robot")) { robot = rf.find("robot").asString(); yInfo("Robot is %s", robot.c_str()); } else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str()); //***************** MODALITY ***************** if (rf.check("modality")) { modality = rf.find("modality").asString(); yInfo("modality is %s", modality.c_str()); } else yInfo("Could not find modality option in the config file; using %s as default",modality.c_str()); //******************* VERBOSE ****************** if (rf.check("verbosity")) { verbosity = rf.find("verbosity").asInt(); yInfo("vtRFThread verbosity set to %i", verbosity); } else yInfo("Could not find verbosity option in config file; using %i as default",verbosity); //****************** rate ****************** if (rf.check("rate")) { rate = rf.find("rate").asInt(); yInfo("vtRFThread working at %i ms", rate); } else yInfo("Could not find rate in the config file; using %i ms as default",rate); //************* skinTaxels' Resource finder ************** ResourceFinder skinRF; skinRF.setVerbose(false); skinRF.setDefaultContext("skinGui"); //overridden by --context parameter skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter skinRF.configure(0,NULL); vector<string> filenames; int partNum=4; Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS"); if(!skinEventsConf.isNull()) { yInfo("SKIN_EVENTS section found\n"); if(skinEventsConf.check("skinParts")) { Bottle* skinPartList = skinEventsConf.find("skinParts").asList(); partNum=skinPartList->size(); } if(skinEventsConf.check("taxelPositionFiles")) { Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList(); if (rf.check("leftHand") || rf.check("leftForeArm") || rf.check("rightHand") || rf.check("rightForeArm")) { if (rf.check("leftHand")) { string taxelPosFile = taxelPosFiles->get(0).asString().c_str(); string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { yInfo("[visuoTactileRF] filePath [%i] %s\n",0,filePath.c_str()); filenames.push_back(filePath); } } if (rf.check("leftForeArm")) { string taxelPosFile = taxelPosFiles->get(1).asString().c_str(); string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { yInfo("[visuoTactileRF] filePath [%i] %s\n",1,filePath.c_str()); filenames.push_back(filePath); } } if (rf.check("rightHand")) { string taxelPosFile = taxelPosFiles->get(2).asString().c_str(); string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { yInfo("[visuoTactileRF] filePath [%i] %s\n",2,filePath.c_str()); filenames.push_back(filePath); } } if (rf.check("rightForeArm")) { string taxelPosFile = taxelPosFiles->get(3).asString().c_str(); string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { yInfo("[visuoTactileRF] filePath [%i] %s\n",3,filePath.c_str()); filenames.push_back(filePath); } } } else { for(int i=0;i<partNum;i++) // all of the skinparts { string taxelPosFile = taxelPosFiles->get(i).asString().c_str(); string filePath(skinRF.findFile(taxelPosFile.c_str())); if (filePath!="") { yInfo("[visuoTactileRF] filePath [%i] %s\n",i,filePath.c_str()); filenames.push_back(filePath); } } } } } else { yError(" No skin's configuration files found."); return 0; } //*************** eyes' Resource finder **************** ResourceFinder gazeRF; gazeRF.setVerbose(verbosity!=0); gazeRF.setDefaultContext("iKinGazeCtrl"); robot=="icub"?gazeRF.setDefaultConfigFile("config.ini"):gazeRF.setDefaultConfigFile("configSim.ini"); gazeRF.configure(0,NULL); double head_version=gazeRF.check("head_version",Value(1.0)).asDouble(); ResourceFinder eyeAlignRF; Bottle &camerasGroup = gazeRF.findGroup("cameras"); if(!camerasGroup.isNull()) { eyeAlignRF.setVerbose(verbosity!=0); camerasGroup.check("context")? eyeAlignRF.setDefaultContext(camerasGroup.find("context").asString().c_str()): eyeAlignRF.setDefaultContext(gazeRF.getContext().c_str()); eyeAlignRF.setDefaultConfigFile(camerasGroup.find("file").asString().c_str()); eyeAlignRF.configure(0,NULL); } else { yWarning("Did not find camera calibration group into iKinGazeCtrl ResourceFinder!"); } //****************************************************** //*********************** THREAD ********************** if( filenames.size() > 0 ) { vtRFThrd = new vtRFThread(rate, name, robot, modality, verbosity, rf, filenames, head_version, eyeAlignRF); if (!vtRFThrd -> start()) { delete vtRFThrd; vtRFThrd = 0; yError("vtRFThread wasn't instantiated!!"); return false; } yInfo("VISUO TACTILE RECEPTIVE FIELDS: vtRFThread instantiated..."); } else { vtRFThrd = 0; yError("vtRFThread wasn't instantiated. No filenames have been given!"); return false; } //****************************************************** //************************ PORTS *********************** rpcClnt.open(("/"+name+"/rpc:o").c_str()); rpcSrvr.open(("/"+name+"/rpc:i").c_str()); attach(rpcSrvr); return true; }
bool configure(ResourceFinder &rf) { string name=rf.check("name",Value("teleop-icub")).asString().c_str(); string robot=rf.check("robot",Value("icub")).asString().c_str(); string geomagic=rf.check("geomagic",Value("geomagic")).asString().c_str(); double Tp2p=rf.check("Tp2p",Value(1.0)).asDouble(); part=rf.check("part",Value("right_arm")).asString().c_str(); simulator=rf.check("simulator",Value("off")).asString()=="on"; gaze=rf.check("gaze",Value("off")).asString()=="on"; minForce=fabs(rf.check("min-force-feedback",Value(3.0)).asDouble()); maxForce=fabs(rf.check("max-force-feedback",Value(15.0)).asDouble()); bool torso=rf.check("torso",Value("on")).asString()=="on"; Property optGeo("(device hapticdeviceclient)"); optGeo.put("remote",("/"+geomagic).c_str()); optGeo.put("local",("/"+name+"/geomagic").c_str()); if (!drvGeomagic.open(optGeo)) return false; drvGeomagic.view(igeo); if (simulator) { simPort.open(("/"+name+"/simulator:rpc").c_str()); if (!Network::connect(simPort.getName().c_str(),"/icubSim/world")) { yError("iCub simulator is not running!"); drvGeomagic.close(); simPort.close(); return false; } } if (gaze) { Property optGaze("(device gazecontrollerclient)"); optGaze.put("remote","/iKinGazeCtrl"); optGaze.put("local",("/"+name+"/gaze").c_str()); if (!drvGaze.open(optGaze)) { drvGeomagic.close(); simPort.close(); return false; } drvGaze.view(igaze); } Property optCart("(device cartesiancontrollerclient)"); optCart.put("remote",("/"+robot+"/cartesianController/"+part).c_str()); optCart.put("local",("/"+name+"/cartesianController/"+part).c_str()); if (!drvCart.open(optCart)) { drvGeomagic.close(); if (simulator) simPort.close(); if (gaze) drvGaze.close(); return false; } drvCart.view(iarm); Property optHand("(device remote_controlboard)"); optHand.put("remote",("/"+robot+"/"+part).c_str()); optHand.put("local",("/"+name+"/"+part).c_str()); if (!drvHand.open(optHand)) { drvGeomagic.close(); if (simulator) simPort.close(); if (gaze) drvGaze.close(); drvCart.close(); return false; } drvHand.view(imod); drvHand.view(ipos); drvHand.view(ivel); iarm->storeContext(&startup_context); iarm->restoreContext(0); Vector dof(10,1.0); if (!torso) dof[0]=dof[1]=dof[2]=0.0; else dof[1]=0.0; iarm->setDOF(dof,dof); iarm->setTrajTime(Tp2p); Vector accs,poss; for (int i=0; i<9; i++) { joints.push_back(7+i); modes.push_back(VOCAB_CM_POSITION); accs.push_back(1e9); vels.push_back(100.0); poss.push_back(0.0); } poss[0]=20.0; poss[1]=70.0; imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst()); ipos->setRefAccelerations(joints.size(),joints.getFirst(),accs.data()); ipos->setRefSpeeds(joints.size(),joints.getFirst(),vels.data()); ipos->positionMove(joints.size(),joints.getFirst(),poss.data()); joints.clear(); modes.clear(); vels.clear(); for (int i=2; i<9; i++) { joints.push_back(7+i); modes.push_back(VOCAB_CM_VELOCITY); vels.push_back(40.0); } vels[vels.length()-1]=100.0; s0=s1=idle; c0=c1=0; onlyXYZ=true; stateStr[idle]="idle"; stateStr[triggered]="triggered"; stateStr[running]="running"; Matrix T=zeros(4,4); T(0,1)=1.0; T(1,2)=1.0; T(2,0)=1.0; T(3,3)=1.0; igeo->setTransformation(SE3inv(T)); igeo->setCartesianForceMode(); igeo->getMaxFeedback(maxFeedback); Tsim=zeros(4,4); Tsim(0,1)=-1.0; Tsim(1,2)=1.0; Tsim(1,3)=0.5976; Tsim(2,0)=-1.0; Tsim(2,3)=-0.026; Tsim(3,3)=1.0; pos0.resize(3,0.0); rpy0.resize(3,0.0); x0.resize(3,0.0); o0.resize(4,0.0); if (simulator) { Bottle cmd,reply; cmd.addString("world"); cmd.addString("mk"); cmd.addString("ssph"); // radius cmd.addDouble(0.02); // position cmd.addDouble(0.0); cmd.addDouble(0.0); cmd.addDouble(0.0); // color cmd.addInt(1); cmd.addInt(0); cmd.addInt(0); // collision cmd.addString("FALSE"); simPort.write(cmd,reply); } forceFbPort.open(("/"+name+"/force-feedback:i").c_str()); feedback.resize(3,0.0); return true; }
bool configure(yarp::os::ResourceFinder &rf) { Vector newDof, curDof; cout<<"Configuring module!"<<endl; moduleName=rf.check("name",Value("torsoModule")).asString().c_str(); robotName=rf.check("robot",Value("icub")).asString().c_str(); OPCName=rf.check("OPC",Value("memory")).asString().c_str(); period=rf.check("period",Value(0.2)).asDouble(); kp=rf.check("kp",Value(KP)).asDouble(); maxTorsoTrajTime=rf.check("torsoTime",Value(MAX_TORSO_TRAJ_TIME)).asDouble(); maxTorsoVelocity=rf.check("maxTorsoVelocity",Value(MAX_TORSO_VELOCITY)).asDouble(); maxArmTrajTime=rf.check("armTime",Value(MAX_ARM_TRAJ_TIME)).asDouble(); handlerPort.open(("/"+moduleName+"/rpc:i").c_str()); attach(handlerPort); objectsPort.open(("/"+moduleName+"/OPC:io").c_str()); if(!objectsPort.addOutput(("/"+OPCName+"/rpc").c_str())){ cout<<"Error connecting to OPC client!"<<endl; return false; } Property optionGaze; optionGaze.put("device","gazecontrollerclient"); optionGaze.put("remote","/iKinGazeCtrl"); optionGaze.put("local",("/"+moduleName+"/gaze").c_str()); if(!clientGazeCtrl.open(optionGaze)){ cout<<"Error opening gaze client!"<<endl; return false; } clientGazeCtrl.view(igaze); igaze->restoreContext(0); igaze->storeContext(&startupGazeContextID); gazeHomePosition.push_back(GAZE_HOME_POS_X); gazeHomePosition.push_back(GAZE_HOME_POS_Y); gazeHomePosition.push_back(GAZE_HOME_POS_Z); Property leftArmOption; leftArmOption.put("device","cartesiancontrollerclient"); leftArmOption.put("remote",("/"+robotName+"/cartesianController/left_arm").c_str()); leftArmOption.put("local",("/"+moduleName+"/left_arm").c_str()); if(!clientArmLeft.open(leftArmOption)){ cout<<"Error opening left arm client!"<<endl; return false; } clientArmLeft.view(icartLeft); icartLeft->restoreContext(0); icartLeft->storeContext(&startupArmLeftContextID); leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_X); leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_Y); leftArmHomePosition.push_back(LEFT_ARM_HOME_POS_Z); icartLeft->setTrajTime(maxArmTrajTime); icartLeft->storeContext(¤tArmLeftContextID); Property rightArmOption; rightArmOption.put("device","cartesiancontrollerclient"); rightArmOption.put("remote",("/"+robotName+"/cartesianController/right_arm").c_str()); rightArmOption.put("local",("/"+moduleName+"/right_arm").c_str()); if(!clientArmRight.open(rightArmOption)){ cout<<"Error opening right arm client!"<<endl; return false; } clientArmRight.view(icartRight); icartRight->restoreContext(0); icartRight->storeContext(&startupArmRightContextID); rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_X); rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_Y); rightArmHomePosition.push_back(RIGHT_ARM_HOME_POS_Z); icartRight->setTrajTime(maxArmTrajTime); icartRight->storeContext(¤tArmRightContextID); computeArmOr(); Property torsoOptions; torsoOptions.put("device", "remote_controlboard"); torsoOptions.put("remote",("/"+robotName+"/torso").c_str()); torsoOptions.put("local",("/"+moduleName+"/torso").c_str()); if(!clientTorso.open(torsoOptions)){ cout<<"Error opening torso client!"<<endl; return false; } clientTorso.view(itorsoVelocity); clientTorso.view(itorsoMode); torsoHomePosition.push_back(TORSO_HOME_POS_ROLL); torsoHomePosition.push_back(TORSO_HOME_POS_PITCH); torsoHomePosition.push_back(TORSO_HOME_POS_YAW); torsoAcceleration.push_back(TORSO_ACCELERATION_ROLL); torsoAcceleration.push_back(TORSO_ACCELERATION_PITCH); torsoAcceleration.push_back(TORSO_ACCELERATION_YAW); clientTorso.view(iTorsoEncoder); waypoints.resize(3,3); waypoints(0,0) = 0.0; waypoints(0,1) = -15.0; waypoints(0,2) = 10.0; waypoints(1,0) = 0.0; waypoints(1,1) = 15.0; waypoints(1,2) = 10.0; waypoints(2,0) = 0.0; waypoints(2,1) = 0.0; waypoints(2,2) = 20.0; index = 0; running = false; /*Bottle bAdd, bReply; bAdd.addVocab(Vocab::encode("add")); Bottle &bTempAdd=bAdd.addList(); Bottle &bEntity=bTempAdd.addList(); bEntity.addString("entity"); bEntity.addString("action"); Bottle &bName=bTempAdd.addList(); bName.addString("name"); bName.addString("ball"); Bottle &bX= bTempAdd.addList(); bX.addString("position_3d"); Bottle &coord = bX.addList(); coord.addDouble(-0.05); coord.addDouble(0.0); coord.addDouble(0.2); objectsPort.write(bAdd,bReply); cout<<bReply.get(0).asVocab()<<endl; */ return true; }