void handHandler(const bool b) { if (b) { if (s1==idle) s1=triggered; else if (s1==triggered) { if (++c1*getPeriod()>0.5) { imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst()); s1=running; } } else ivel->velocityMove(joints.size(),joints.getFirst(),vels.data()); } else { if (s1==triggered) vels=-1.0*vels; if (c1!=0) ivel->stop(joints.size(),joints.getFirst()); s1=idle; c1=0; } }
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 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 exploreTorso(Vector target) { Vector torsoInitialJoints; Vector torsoActualJoints; Vector torsoVelocityCommand; Vector torsoAccCommand; Vector error,integral,derivative,preError; int jointsNumber=0; int i; double time= 0.0; itorsoVelocity->getAxes(&jointsNumber); torsoAccCommand.resize(jointsNumber); torsoVelocityCommand.resize(jointsNumber); torsoInitialJoints.resize(jointsNumber); torsoActualJoints.resize(jointsNumber); error.resize(jointsNumber); integral.resize(jointsNumber); derivative.resize(jointsNumber); preError.resize(jointsNumber); preError[0] = 0.0; preError[1] = 0.0; preError[2] = 0.0; integral[0] = 0.0; integral[1] = 0.0; integral[2] = 0.0; derivative[0] = 0.0; derivative[1] = 0.0; derivative[2] = 0.0; for(i =0; i< jointsNumber;i++); torsoAccCommand[i] = torsoAcceleration[i]; itorsoVelocity->setRefAccelerations(torsoAccCommand.data()); if (!iTorsoEncoder->getEncoders(torsoInitialJoints.data())){ cout<<"Error in reading encoders."<<endl; return false; } VectorOf<int> modes(3); modes[0]=modes[1]=modes[2]=VOCAB_CM_VELOCITY; itorsoMode->setControlModes(modes.getFirst()); error = target-torsoInitialJoints; integral = integral + (error * DT); derivative = (error - preError) / DT; preError = error; torsoVelocityCommand = kp * error + KI * integral + KD * derivative; time = Time::now(); while (norm(error)>0.2){ if(Time::now()-time>maxTorsoTrajTime){ cout<<"Max time reached."<<endl; itorsoVelocity->stop(); return true; } itorsoVelocity->velocityMove(torsoVelocityCommand.data()); if (!iTorsoEncoder->getEncoders(torsoActualJoints.data())){ cout<<"Error in reading encoders."<<endl; itorsoVelocity->stop(); return false; } error = target-torsoActualJoints; integral = integral + (error * DT); derivative = (error - preError) / DT; preError = error; torsoVelocityCommand = kp * error + KI * integral + KD * derivative; Time::delay(DT); } itorsoVelocity->stop(); return true; }