Beispiel #1
0
    virtual void run()
    {
        t=Time::now();

        generateTarget();

        if (state==STATE_TRACK)
        {
            // look at the target (streaming)
            igaze->lookAtFixationPoint(fp);

            // some verbosity
            printStatus();

            // we collect from time to time
            // some interesting points (POI)
            // where to look at soon afterwards
            storeInterestingPoint();

            if (t-t2>=SWITCH_STATE_PER)
            {
                // switch state
                state=STATE_RECALL;
            }
        }

        if (state==STATE_RECALL)
        {
            // pick up the first POI
            // and clear the list
            Vector ang=poiList.front();
            poiList.clear();

            fprintf(stdout,"Retrieving POI #0 ... (%s) [deg]\n",
                    ang.toString(3,3).c_str());

            // register the motion-done event, attaching the callback
            // that will be executed as soon as the gaze is accomplished
            igaze->registerEvent(*this);

            // look at the chosen POI
            igaze->lookAtAbsAngles(ang);

            // switch state
            state=STATE_WAIT;
        }

        if (state==STATE_STILL)
        {
            if (t-t4>=STILL_STATE_TIME)
            {
                fprintf(stdout,"done\n");

                t1=t2=t3=t;

                // switch state
                state=STATE_TRACK;
            }
        }
    }
 void look_down()
 {
     // look at 0.5 meter in front of the robot
     Vector pos(3);
     pos[0] = -0.3;
     pos[1] =  0.0;
     pos[2] =  0.0;
     igaze->lookAtFixationPoint(pos);
 }
Beispiel #3
0
	virtual void run() {

		while (isStopping() != true) {

			/* poll the click ports containers to see if we have left/right ready to go */
			bfL.lock();	bfR.lock();
			if (bfL.size() == 2 && bfR.size() == 2) {

				printf("got a hit!\n");

				/* if they are, raise the flag that action is beginning, save current joint configuration */
				Bottle susmsg;
				susmsg.addInt(1);
				susPort->write(susmsg);

				//get the current joint configuration for the torso, head, and arm
				tang.clear(); tang.resize(3);
				tEnc->getEncoders(tang.data());
				hang.clear(); hang.resize(6);
				hEnc->getEncoders(hang.data());
				aang.clear(); aang.resize(16);
				aEnc->getEncoders(aang.data());

				/* get the xyz location of the gaze point */
				Vector bvL(2); Vector bvR(2);
				bvL[0] = bfL.get(0).asDouble(); bvL[1] = bfL.get(1).asDouble();
				bvR[0] = bfR.get(0).asDouble(); bvR[1] = bfR.get(1).asDouble();
				objPos.clear(); objPos.resize(3);
				gaze->triangulate3DPoint(bvL,bvR,objPos);

				/* servo the head to gaze at that point */
				//gaze->lookAtStereoPixels(bvL,bvR);
				gaze->lookAtFixationPoint(objPos);
				gaze->waitMotionDone(1.0,10.0);
				gaze->stopControl();

				printf("object position estimated as: %f, %f, %f\n", objPos[0], objPos[1], objPos[2]);
				printf("is this ok?\n");
				string posResp = Network::readString().c_str();

				if (posResp == "yes" || posResp == "y") {

					/* move to hover the hand over the XY position of the target: [X, Y, Z=0.2], with palm upright */
					objPos[2] = 0.1;

					Vector axa(4);
					axa.zero();
					if (armInUse) {
						axa[2] = 1.0; axa[3] = M_PI;
					} else {
						axa[1] = 1.0; axa[3] = M_PI;
					}

					carm->goToPoseSync(objPos,axa);
					carm->waitMotionDone(1.0,10.0);
					Time::delay(2.0);

					//curl fingers and thumb slightly to hold object
					Vector armCur(16);
					aEnc->getEncoders(armCur.data());
					armCur[8] = 3; armCur[10] = 25;
					armCur[11] = 25; armCur[12] = 25;
					armCur[13] = 25; armCur[14] = 25;
					armCur[15] = 55;
					aPos->positionMove(armCur.data());
					Time::delay(2.0);

					/* wait for terminal signal from user that object has been moved to the hand */
					bool validTarg = false;
					printf("object position reached, place in hand and enter target xy position\n");
					while (!validTarg) {

						string objResp = Network::readString().c_str();

						/* ask the user to enter in an XY target location, or confirm use of previous one */
						Bottle btarPos(objResp.c_str());
						if (btarPos.size() < 2) {

							//if user enters no target position, try to use last entered position
							if (targetPos.length() != 3) {
								printf("no previous target position available, please re-enter:\n");
							} else {
								validTarg = true;
							}

						} else {

							targetPos.clear(); targetPos.resize(3);
							targetPos[0] = btarPos.get(0).asDouble();
							targetPos[1] = btarPos.get(1).asDouble();
							targetPos[2] = 0.1;
							validTarg = true;

						}
					}

					/* move the arm to above the target location */
					axa.zero();
					if (armInUse) {
						axa[2] = 1.0; axa[3] = M_PI;
					} else {
						axa[1] = 1.0; axa[3] = M_PI;
					}
					carm->goToPoseSync(targetPos,axa);
					//carm->goToPosition(targetPos);
					carm->waitMotionDone(1.0,10.0);
					Time::delay(2.0);

					/* wait for user signal that the object has been removed */
					printf("object has been moved to target location. please remove object and hit enter\n");
					string tarResp = Network::readString().c_str();


				}
				/* return to saved motor configuration, clear click buffers, lower flag signaling action done */

				printf("gaze done, attempting reset\n");
				tPos->positionMove(tang.data());
				hPos->positionMove(hang.data());
				aPos->positionMove(aang.data());

				bfL.clear(); bfR.clear();
				bfL.unlock(); bfR.unlock();

				susmsg.clear();
				susmsg.addInt(0);
				susPort->write(susmsg);


			}
			else {

				bfL.unlock(); bfR.unlock();

			}
		}
	}
	virtual void run(){
		tmp = command;
		(*command)[0] = -60*(gsl_rng_uniform(r));
		(*command)[1] = 100*(gsl_rng_uniform(r));
		(*command)[2] = -35 + 95*(gsl_rng_uniform(r));
		(*command)[3] = 10 + 90*(gsl_rng_uniform(r));
		printf("%.1lf %.1lf %.1lf %.1lf\n", (*command)[0], (*command)[1], (*command)[2], (*command)[3]);
		//above 0 doesn't seem to be safe for joint 0
		if ((*command)[0] > 0 || (*command)[0] < -60){
			(*command)[0] = (*tmp)[0];
		}
		if ((*command)[1] > 100 || (*command)[1] < -0){
			(*command)[1] = (*tmp)[1];
		}
		if ((*command)[2] > 60 || (*command)[2] < -35){
			(*command)[2] = (*tmp)[2];
		}
		if ((*command)[3] > 100 || (*command)[3] < 10){
			(*command)[3] = (*tmp)[3];
		}
		//use fwd kin to find end effector position
		Bottle plan, pred;
		for (int i = 0; i < nj; i++){
			plan.add((*command)[i]);
		}
		armPlan->write(plan);
		armPred->read(pred);
		Vector commandCart(3);
		for (int i = 0; i < 3; i++){
			commandCart[i] = pred.get(i).asDouble();
		}
		printf("Cartesian safety check\n");
		double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
		// safety radius back to 30 cm
		if (rad > 0.3){
			pos->positionMove(command->data());
			bool done = false;
			while(!done){
				pos->checkMotionDone(&done);
				Time::delay(0.1);
			}
			printf("Moved arm to new location\n");
			Vector &armJ = armLocJ->prepare();
			Vector encoders(nj);
			enc->getEncoders(encoders.data());
			armJ = encoders;
			Vector noisyArm(3);
			for(int i = 0; i < 3; i++){
				//noisyArm[i] = commandCart[i] + 0.01*(2*gsl_rng_uniform(r)-1);
				//sanity check
				noisyArm[i] = commandCart[i] + 0.005*(2*gsl_rng_uniform(r)-1);
			}
			//insert here:
			//read off peak saliences
			//fixate there
			//calculate cartesian value, compare to real cart. value of arm


			printf("Looking at arm\n");
			igaze->lookAtFixationPoint(noisyArm);
			done = false;
			while(!done){
				igaze->checkMotionDone(&done);
				Time::delay(0.5);
			}
			//igaze->waitMotionDone(0.1,30);

			printf("Saw arm\n");

			Vector &headAng = headLoc->prepare();
			igaze->getAngles(headAng);
			Bottle tStamp;
			tStamp.clear();
			tStamp.add(Time::now());
			headLoc->setEnvelope(tStamp);


			headLoc->write();
			armLocJ->write();
			headLoc->unprepare();
			armLocJ->unprepare();

		}
		else{
			printf("Self collision detected!\n");
		}
	}
Beispiel #5
0
 void run()
 {
     Vector x,o;
     action->getPose(x,o);
     igaze->lookAtFixationPoint(x+offset);
 }
 void fixate(const Vector &x)
 {
     igaze->lookAtFixationPoint(x);
 }
Beispiel #7
0
    void reachingHandler(const bool b, const Vector &pos,
                         const Vector &rpy)
    {
        if (b)
        {
            if (s0==idle)
                s0=triggered;
            else if (s0==triggered)
            {
                if (++c0*getPeriod()>0.5)
                {
                    pos0[0]=pos[0];
                    pos0[1]=pos[1];
                    pos0[2]=pos[2];

                    rpy0[0]=rpy[0];
                    rpy0[1]=rpy[1];
                    rpy0[2]=rpy[2];

                    iarm->getPose(x0,o0);
                    s0=running;
                }
            }
            else
            {
                Vector xd(4,0.0);
                xd[0]=pos[0]-pos0[0];
                xd[1]=pos[1]-pos0[1];
                xd[2]=pos[2]-pos0[2];
                xd[3]=1.0;

                Matrix H0=eye(4,4);
                H0(0,3)=x0[0];
                H0(1,3)=x0[1];
                H0(2,3)=x0[2];

                xd=H0*xd;

                Matrix Rd;
                if (onlyXYZ)
                    Rd=axis2dcm(o0);
                else
                {
                    Vector drpy(3);
                    drpy[0]=rpy[0]-rpy0[0];
                    drpy[1]=rpy[1]-rpy0[1];
                    drpy[2]=rpy[2]-rpy0[2];

                    Vector ax(4,0.0),ay(4,0.0),az(4,0.0);
                    ax[0]=1.0; ax[3]=drpy[2];
                    ay[1]=1.0; ay[3]=drpy[1]*((part=="left_arm")?1.0:-1.0);
                    az[2]=1.0; az[3]=drpy[0]*((part=="left_arm")?1.0:-1.0);

                    Rd=axis2dcm(o0)*axis2dcm(ax)*axis2dcm(ay)*axis2dcm(az);
                }

                Vector od=dcm2axis(Rd);
                iarm->goToPose(xd,od);

                if (gaze)
                    igaze->lookAtFixationPoint(xd);

                yInfo("going to (%s) (%s)",
                      xd.toString(3,3).c_str(),od.toString(3,3).c_str());

                if (simulator)
                    updateSim(xd);
            }
        }
        else
        {
            if (s0==triggered)
                onlyXYZ=!onlyXYZ;

            if (c0!=0)
            {
                iarm->stopControl();
                if (simulator)
                {
                    Vector x,o;
                    iarm->getPose(x,o);
                    updateSim(x);
                }
                if (gaze)
                    igaze->stopControl();
            }

            s0=idle;
            c0=0;
        }
    }
    virtual void run()
    {
        t=Time::now();

        generateTarget();        

        if (state==STATE_TRACK)
        {
            // look at the target (streaming)
            igaze->lookAtFixationPoint(fp);

            // some verbosity
            printStatus();

            // we collect from time to time
            // some interesting points (POI)
            // where to look at soon afterwards
            storeInterestingPoint();

            if (t-t2>=SWITCH_STATE_PER)
            {
                // switch state
                state=STATE_RECALL;
            }
        }

        if (state==STATE_RECALL)
        {
            // pick up the first POI
            // and clear the list
            Vector ang=poiList.front();
            poiList.clear();

            fprintf(stdout,"Retrieving POI #0 ... %s [deg]\n",
                    ang.toString().c_str());

            // look at the chosen POI
            igaze->lookAtAbsAngles(ang);

            // switch state
            state=STATE_WAIT;
        }

        if (state==STATE_WAIT)
        {
            bool done=false;
            igaze->checkMotionDone(&done);

            if (done)
            {
                Vector ang;
                igaze->getAngles(ang);            

                fprintf(stdout,"Actual gaze configuration: %s [deg]\n",
                        ang.toString().c_str());
                
                /* pramod modified starts 
                fprintf(stdout,"Moving the torso; see if the gaze is compensated ... ");
                
                // move the torso yaw
                
                double val;
                ienc->getEncoder(0,&val);
                ipos->positionMove(0,val>0.0?-30.0:30.0);
                pramod modified ends */
                t4=t;

                // switch state
                state=STATE_STILL;
            }
        }

        if (state==STATE_STILL)
        {
            if (t-t4>=STILL_STATE_TIME)
            {
                fprintf(stdout,"done\n");

                t1=t2=t3=t;

                // switch state
                state=STATE_TRACK;
            }
        }
    }
    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(&currentArmRightContextID);
								icartLeft->storeContext(&currentArmLeftContextID);
						//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(&currentArmRightContextID);
								icartLeft->storeContext(&currentArmLeftContextID);

						//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(&currentGazeContextID);
								//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(&currentGazeContextID);
								//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(&currentGazeContextID);
								//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(&currentGazeContextID);
								//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;

    }