bool close() { if(meas_given!=1) { portIn.close(); rpcOut.close(); } rpcPort.close(); return true; }
bool close() { portDispIn.close(); portDispOut.close(); portOutPoints.close(); portImgIn.close(); portContour.close(); portSFM.close(); portRpc.close(); 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 close() { iarm->stopControl(); iarm->restoreContext(startup_context); drvCart.close(); ivel->stop(joints.size(),joints.getFirst()); for (size_t i=0; i<modes.size(); i++) modes[i]=VOCAB_CM_POSITION; imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst()); drvHand.close(); if (simulator) { Bottle cmd,reply; cmd.addString("world"); cmd.addString("del"); cmd.addString("all"); simPort.write(cmd,reply); simPort.close(); } if (gaze) { igaze->stopControl(); drvGaze.close(); } igeo->stopFeedback(); igeo->setTransformation(eye(4,4)); drvGeomagic.close(); forceFbPort.close(); return true; }
bool close() { handlerPort.close(); objectsPort.close(); igaze->stopControl(); igaze->restoreContext(startupGazeContextID); igaze->deleteContext(startupGazeContextID); igaze->deleteContext(currentGazeContextID); if (clientGazeCtrl.isValid()) clientGazeCtrl.close(); icartLeft->stopControl(); icartLeft->restoreContext(startupArmLeftContextID); icartLeft->deleteContext(startupArmLeftContextID); icartLeft->deleteContext(currentArmLeftContextID); if (clientArmLeft.isValid()) clientArmLeft.close(); icartRight->stopControl(); icartRight->restoreContext(startupArmRightContextID); icartRight->deleteContext(startupArmRightContextID); icartRight->deleteContext(currentArmRightContextID); if (clientArmLeft.isValid()) clientArmLeft.close(); itorsoVelocity->stop(); if (clientTorso.isValid()) clientTorso.close(); return true; }
bool close() { port.interrupt(); port.close(); 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; }
void threadRelease() { emotions.interrupt(); emotions.close(); }