virtual void threadRelease()
{
// we require an immediate stop
// before closing the client for safety reason
arm->stopControl();
right_arm->stopControl();
// it's a good rule to restore the controller
// context as it was before opening the module
arm->restoreContext(startup_context_id);
right_arm->restoreContext(startup_context_id);
client.close();
clientR.close();
}
bool close()
{
igaze->restoreContext(startup_context_gaze);
drvGaze.close();
iarm->restoreContext(startup_context_arm);
drvArm.close();
imgLPortIn.close();
imgRPortIn.close();
imgLPortOut.close();
imgRPortOut.close();
rpcPort.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;
}
virtual void threadRelease()
{
armRest();
gazeRest();
igaze->restoreContext(startup_gazeContext_id);
clientGaze.close();
if (arm!="none")
{
iarm->restoreContext(startup_armContext_id);
clientArm.close();
}
portImgIn.interrupt();
portImgOut.interrupt();
portTopDown.interrupt();
portSetFace.interrupt();
portImgIn.close();
portImgOut.close();
portTopDown.close();
portSetFace.close();
}
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;
}
void CalibModule::postureHelper(const Vector &gaze_ang, const Matrix &targetL,
const Matrix &targetR)
{
IControlMode2 *imod;
IPositionControl *ipos;
ICartesianControl *icart;
int ctxtL,ctxtR;
Vector dof;
yInfo("looking at target");
igaze->lookAtAbsAngles(gaze_ang);
if (useArmL)
{
drvArmL.view(imod);
drvArmL.view(ipos);
openHand(imod,ipos);
drvCartL.view(icart);
icart->storeContext(&ctxtL);
icart->getDOF(dof);
dof=1.0;
icart->setDOF(dof,dof);
icart->setLimits(0,0.0,0.0);
icart->setLimits(1,0.0,0.0);
icart->setLimits(2,0.0,0.0);
icart->setTrajTime(1.0);
yInfo("reaching for left target");
icart->goToPoseSync(targetL.getCol(3),dcm2axis(targetL));
}
if (useArmR)
{
drvArmR.view(imod);
drvArmR.view(ipos);
openHand(imod,ipos);
drvCartR.view(icart);
icart->storeContext(&ctxtR);
icart->getDOF(dof);
dof=1.0;
dof[0]=dof[1]=dof[2]=0.0;
icart->setDOF(dof,dof);
icart->setTrackingMode(true);
icart->setTrajTime(1.0);
yInfo("reaching for right target");
icart->goToPoseSync(targetR.getCol(3),dcm2axis(targetR));
yInfo("waiting for right arm... ");
icart->waitMotionDone();
yInfo("done");
}
if (useArmL)
{
drvCartL.view(icart);
yInfo("waiting for left arm... ");
icart->waitMotionDone();
yInfo("done");
icart->restoreContext(ctxtL);
icart->deleteContext(ctxtL);
}
if (useArmR)
{
drvCartR.view(icart);
icart->restoreContext(ctxtR);
icart->deleteContext(ctxtR);
}
}
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;
}
bool respond(const Bottle& command, Bottle& reply)
{
if(command.size()==0)
{
reply.addString("No command received.");
return true;
}
switch(command.get(0).asVocab()){
case HOME:
if(command.size()>1)
switch(command.get(1).asVocab()){
case ARM:
int tempCxL,tempCxR;
icartLeft->storeContext(&tempCxL);
icartLeft->storeContext(&tempCxR);
icartLeft->restoreContext(currentArmLeftContextID);
icartLeft->restoreContext(currentArmRightContextID);
icartLeft->goToPositionSync(leftArmHomePosition);
icartRight->goToPositionSync(rightArmHomePosition);
icartRight->waitMotionDone(0.1,2);
icartLeft->waitMotionDone(0.1,2);
icartLeft->restoreContext(tempCxR);
icartLeft->restoreContext(tempCxL);
icartLeft->deleteContext(tempCxR);
icartLeft->deleteContext(tempCxL);
reply.addString("Arm home position reached.");
return true;
case TORSO:
exploreTorso(torsoHomePosition);
reply.addString("Torso home position reached.");
return true;
case GAZE:
int tempCx;
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazeHomePosition);
igaze->waitMotionDone(0.1,2);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze home position reached.");
return true;
default:
reply.addString("Wrong device for home position.");
return true;
}
else{
int tempCxL,tempCxR,tempCx;
icartLeft->storeContext(&tempCxL);
icartLeft->storeContext(&tempCxR);
icartLeft->restoreContext(currentArmLeftContextID);
icartLeft->restoreContext(currentArmRightContextID);
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
icartLeft->goToPositionSync(leftArmHomePosition);
icartRight->goToPositionSync(rightArmHomePosition);
igaze->lookAtFixationPoint(gazeHomePosition);
exploreTorso(torsoHomePosition);
igaze->waitMotionDone(0.1,2);
icartRight->waitMotionDone(0.1,2);
icartLeft->waitMotionDone(0.1,2);
icartLeft->restoreContext(tempCxR);
igaze->restoreContext(tempCx);
icartLeft->restoreContext(tempCxL);
icartLeft->deleteContext(tempCxR);
icartLeft->deleteContext(tempCxL);
igaze->deleteContext(tempCx);
reply.addString("Ok, moving to home position.");
return true;
}
case LOOK_AT:
if(command.size()==2){
Bottle bAsk,bReply, bGet;
bAsk.addVocab(Vocab::encode("ask"));
Bottle &bTempAsk=bAsk.addList().addList();
bTempAsk.addString("name");
bTempAsk.addString("==");
bTempAsk.addString(command.get(1).asString());
objectsPort.write(bAsk,bReply);
cout<<"first"<<endl;
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false ||
bReply.get(1).asList()->find("id").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
bGet.addVocab(Vocab::encode("get"));
Bottle &bTempGet=bGet.addList().addList();
bTempGet.addString("id");
bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt());
objectsPort.write(bGet,bReply);
cout<<"second"<<endl;
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_3d")==false ||
bReply.get(1).asList()->find("position_3d").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
cout<<"third"<<endl;
Vector gazePosition(3);
gazePosition[0] = bReply.get(1).asList()->find("position_3d").asList()->get(0).asDouble();
gazePosition[1] = bReply.get(1).asList()->find("position_3d").asList()->get(1).asDouble();
gazePosition[2] = bReply.get(1).asList()->find("position_3d").asList()->get(2).asDouble();
int tempCx;
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazePosition);
igaze->waitMotionDone(0.2,3);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze position reached.");
return true;
}
else if(command.size()==4){
Vector gazePosition(3);
int tempCx;
gazePosition[0] = command.get(1).asDouble();
gazePosition[1] = command.get(2).asDouble();
gazePosition[2] = command.get(3).asDouble();
igaze->storeContext(&tempCx);
igaze->restoreContext(currentGazeContextID);
igaze->lookAtFixationPoint(gazePosition);
igaze->waitMotionDone(0.2,3);
igaze->restoreContext(tempCx);
igaze->deleteContext(tempCx);
reply.addString("Gaze position reached.");
return true;
}
else{
reply.addString("Wrong number of parameters for lookAt.");
return true;
}
case GET:
if(command.size()==2){
Bottle bAsk,bReply, bGet;
bAsk.addVocab(Vocab::encode("ask"));
Bottle &bTempAsk=bAsk.addList().addList();
bTempAsk.addString("name");
bTempAsk.addString("==");
bTempAsk.addString(command.get(1).asString());
objectsPort.write(bAsk,bReply);
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("id")==false ||
bReply.get(1).asList()->find("id").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
bGet.addVocab(Vocab::encode("get"));
Bottle &bTempGet=bGet.addList().addList();
bTempGet.addString("id");
bTempGet.addInt(bReply.get(1).asList()->find("id").asList()->get(0).asInt());
objectsPort.write(bGet,bReply);
if(bReply.size()==0 || bReply.get(0).asVocab()!=Vocab::encode("ack") || bReply.get(1).asList()->check("position_2d_left")==false ||
bReply.get(1).asList()->find("position_2d_left").asList()->size()==0){
reply.addVocab(Vocab::encode("nack"));
return true;
}
Vector objPosition(4);
objPosition[0] = bReply.get(1).asList()->find("position_2d_left").asList()->get(0).asInt();
objPosition[1] = bReply.get(1).asList()->find("position_2d_left").asList()->get(1).asInt();
objPosition[2] = bReply.get(1).asList()->find("position_2d_left").asList()->get(2).asInt();
objPosition[3] = bReply.get(1).asList()->find("position_2d_left").asList()->get(3).asInt();
reply.addVocab(VOCAB3('a','c','k'));
Bottle &coord = reply.addList();
coord.addInt((int)(objPosition[0]+objPosition[2])/2);
coord.addInt((int)(objPosition[1]+objPosition[3])/2);
return true;
}
else{
reply.addString("Wrong number of parameters for get.");
return true;
}
case TRACK:
if(command.size()==3)
switch(command.get(1).asVocab()){
case ARM:
if (command.get(2).asString() == "on"){
int tempCxL,tempCxR;
//icartLeft->storeContext(&tempCxL);
//icartLeft->storeContext(&tempCxR);
//icartLeft->restoreContext(currentArmLeftContextID);
//icartLeft->restoreContext(currentArmRightContextID);
icartLeft->setTrackingMode(true);
icartRight->setTrackingMode(true);
icartRight->storeContext(¤tArmRightContextID);
icartLeft->storeContext(¤tArmLeftContextID);
//icartLeft->restoreContext(tempCxR);
// icartLeft->restoreContext(tempCxL);
// icartLeft->deleteContext(tempCxR);
// icartLeft->deleteContext(tempCxL);
reply.addString("Arm tracking mode enabled.");
}
else if (command.get(2).asString() == "off"){
int tempCxL,tempCxR;
//icartLeft->storeContext(&tempCxL);
//icartLeft->storeContext(&tempCxR);
//icartLeft->restoreContext(currentArmLeftContextID);
//icartLeft->restoreContext(currentArmRightContextID);
icartLeft->setTrackingMode(false);
icartRight->setTrackingMode(false);
icartRight->storeContext(¤tArmRightContextID);
icartLeft->storeContext(¤tArmLeftContextID);
//icartLeft->restoreContext(tempCxR);
// icartLeft->restoreContext(tempCxL);
// icartLeft->deleteContext(tempCxR);
// icartLeft->deleteContext(tempCxL);
reply.addString("Arm tracking mode disabled.");
}
else
reply.addString("Wrong parameter: on/off");
return true;
case GAZE:
if (command.get(2).asString() == "on"){
int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->setTrackingMode(true);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze tracking mode enabled.");
}
else if (command.get(2).asString() == "off"){
int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->setTrackingMode(false);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze tracking mode disabled.");
}
else
reply.addString("Wrong parameter for trac: on/off");
return true;
default:
reply.addString("Wrong device for tracking mode.");
return true;
}
else{
reply.addString("Missing parameters for track.");
return true;
}
case BLOCK:
if(command.size()>1)
switch(command.get(1).asVocab()){
case GAZE:
if(command.size()==3){
// int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->blockEyes(command.get(2).asDouble());
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Gaze blocking mode enabled.");
}
else{
//int tempCx;
//igaze->storeContext(&tempCx);
//igaze->restoreContext(currentGazeContextID);
igaze->blockEyes(DEFAULT_VERGENCE);
igaze->storeContext(¤tGazeContextID);
//igaze->restoreContext(tempCx);
//igaze->deleteContext(tempCx);
reply.addString("Default vergence set.");
}
return true;
default:
reply.addString("Wrong device for blocking mode.");
return true;
}
else{
reply.addString("Missing parameters for block.");
return true;
}
case GOTO:
if(command.size()==4){
Vector torsoTarget(3);
torsoTarget[0] = command.get(1).asDouble();
torsoTarget[1] = command.get(2).asDouble();
torsoTarget[2] = command.get(3).asDouble();
exploreTorso(torsoTarget);
reply.addString("Torso position reached.");
}
else{
reply.addString("Missing parameters for goto.");
return true;
}
return true;
case RUN:
running = true;
reply.addString("Ready for exploration. next or stop?");
return true;
case NEXT:
if(running){
exploreTorso(waypoints.getRow(index));
index++;
if (index > 2){
running = false;
index = 0;
reply.addString("Waypoint reached. End of waypoints.");
}
else{
reply.addString("Waypoint reached. next or stop?");
}
}
else
reply.addString("Not running.");
return true;
case STOP:
index = 0;
running = false;
reply.addString("Run stopped. Index reset.");
return true;
default:
RFModule::respond(command,reply);
return true;
}
return true;
}
