C++ IPositionControl::checkMotionDone方法代码示例

//.........这里部分代码省略.........
	}
	remotePorts += "_arm";
	std::string localPorts = "/learnedReach/cmd";
	if(!params.check("map")){
		fprintf(stderr, "Please specify learned visuomotor map file\n");
		fprintf(stderr, "e.g. --map map.dat\n");
		return -1;
	}
	string fName = params.find("map").asString().c_str();

	Property options;
	options.put("device", "remote_controlboard");
	options.put("local", localPorts.c_str());
	options.put("remote", remotePorts.c_str());

	PolyDriver robotDevice(options);
	if (!robotDevice.isValid()){
		printf("Device not available. Here are known devices: \n");
		printf("%s", Drivers::factory().toString().c_str());
		Network::fini();
		return 1;
	}

	IPositionControl *pos;
	IEncoders *enc;

	bool ok;
	ok = robotDevice.view(pos);
	ok = ok && robotDevice.view(enc);

	if (!ok){
		printf("Problems acquiring interfaces\n");
		return 0;
	}

	int nj = 0;
	pos->getAxes(&nj);
	Vector encoders;
	Vector command;
	Vector commandCart;
	Vector tmp;
	encoders.resize(nj);
	tmp.resize(nj);
	command.resize(nj);
	commandCart.resize(3);

	for (int i = 0; i < nj; i++) {
		tmp[i] = 25.0;
	}
	pos->setRefAccelerations(tmp.data());

	for (int i = 0; i < nj; i++) {
		tmp[i] = 20.0;
	    pos->setRefSpeed(i, tmp[i]);
	}

	command = 0;

	//set the arm joints to "middle" values
	command[0] = -45;
	command[1] = 45;
	command[2] = 0;
	command[3] = 45;
	pos->positionMove(command.data());

	bool done = false;
	while (!done){
		pos->checkMotionDone(&done);
		Time::delay(0.1);
	}

	//not really yaw and pitch
	int azMin = -80; int azMax = 0;
	int elMin = -60; int elMax = 0;
	int verMin = 0; int verMax = 20;

	int Y; int P; int V;
	int mmapSize; int usedJoints;

	//read in first lines to get map dimensions
	string line;
	string buf;
	ifstream mapFile(fName.c_str());
	if(mapFile.is_open()){
		getline(mapFile,line);
		stringstream ss(line);
		ss >> buf;
		Y = atoi(buf.c_str());
		ss >> buf;
		P = atoi(buf.c_str());
		ss >> buf;
		V = atoi(buf.c_str());
		ss.clear();
		getline(mapFile,line);
		ss.str(line);
		ss >> buf;
		mmapSize = atoi(buf.c_str());
		ss >> buf;
		usedJoints = atoi(buf.c_str());
	}

//.........这里部分代码省略.........
        mask[3] = false;
        mask[4] = true;
        mask[5] = false;
        mask[6] = true;
        mask[7] = false;
        mask[8] = true;
        mask[9] = true;
        mask[10] = true;
        mask[11] = true;
        mask[12] = true;
        mask[13] = true;
        mask[14] = true;
        mask[15] = true;

        cout << "Closing the hand" << endl;
        // closing the hand:
        pos->positionMove(4, 30.0);
        pos->positionMove(6, 10.0);
        pos->positionMove(8, 10.0);
        pos->positionMove(9, 17.0);
        pos->positionMove(10, 170.0);
        pos->positionMove(11, 0.0);
        pos->positionMove(12, 0.0);
        pos->positionMove(13, 90.0);
        pos->positionMove(14, 180.0);
        pos->positionMove(15, 180.0);

        cout << "Waiting to be in initial position...";
        bool motionDone=false;
        while (!motionDone)
        {
            motionDone=true;
            for (int i=0; i<jnts; i++)
            {
                if (mask[i])
                {
                    bool jntMotionDone = false;
                    pos->checkMotionDone(i, &jntMotionDone);
                    motionDone &= jntMotionDone;
                }
            }

            Time::yield();  // to avoid killing cpu
        }
        cout << "ok" << endl;
        // wait for the hand to be in initial position

        Matrix R=zeros(3,3);
        R(0,0)=-1.0; R(1,1)=1; R(2,2)=-1.0;
        orientation=dcm2axis(R);
        
        // enable torso movements as well
        // in order to enlarge the workspace
        Vector dof;
        armCart->getDOF(dof);
        dof=1.0; dof[1]=0.0;    // every dof but the torso roll
        armCart->setDOF(dof,dof);
        armCart->setTrajTime(1.0);

        Vector initPos(3,0.0);
        if (rf.check("rightHandInitial"))
        {
            Bottle *botPos = rf.find("rightHandInitial").asList();
            initPos[0] = botPos->get(0).asDouble();
            initPos[1] = botPos->get(1).asDouble();
            initPos[2] = botPos->get(2).asDouble();
            cout<<"Reaching initial position with right hand"<<initPos.toString(3,3).c_str()<<endl;            
            armCart->goToPoseSync(initPos,orientation);
            armCart->waitMotionDone(0.1,3.0);
        }
        else
        {
            cout << "Cannot find the inital position" << endl << "closing module" << endl;
            return false;
        }        

        minY    = rf.find("min").asDouble();
        maxY    = rf.find("max").asDouble();
        gap     = rf.find("gap").asDouble();
        delay   = rf.find("delay").asDouble();
        thrMove = rf.find("threshold_move").asDouble();
        yDivisions = rf.check("yDivisions",Value(10)).asInt();
        Bottle* bsequenceToPlay = rf.find("sequence").asList();
        if (bsequenceToPlay)
        {
            cout << "Using sequence, not random." << endl;
            for (int i = 0; i < bsequenceToPlay->size(); i++)
                sequenceToPlay.push_back(bsequenceToPlay->get(i).asInt());
            indexInSequence = 0;
        }

        tempPos=initPos;
        state=up;
        forward = false;

        timeBeginIdle = Time::now();

        Rand::init();
        return true;
    }

	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");
		}
	}

//.........这里部分代码省略.........
		T = gsl_rng_default;
		r = gsl_rng_alloc(T);

		igaze = NULL;

		Property options;
		options.put("device","gazecontrollerclient");
		options.put("remote","/iKinGazeCtrl");
		options.put("local","/client/gaze");

		clientGazeCtrl = new PolyDriver;
		clientGazeCtrl->open(options);

		options.clear();
		string localPorts = "/babbleTrack/cmd";
		string remotePorts = "/" + robotName + "/" + arm + "_arm";

		options.put("device", "remote_controlboard");
		options.put("local", localPorts.c_str());
		options.put("remote", remotePorts.c_str());

		robotDevice = new PolyDriver;
		robotDevice->open(options);

		if(clientGazeCtrl->isValid()){
			clientGazeCtrl->view(igaze);
		}
		else{
			return false;
		}


		if (!robotDevice->isValid()){
			printf("Device not available. Here are known devices: \n");
			printf("%s", Drivers::factory().toString().c_str());
			Network::fini();
			return false;
		}

		bool ok;
		ok = robotDevice->view(pos);
		ok = ok && robotDevice->view(enc);

		if (!ok){
			printf("Problems acquiring interfaces\n");
			return false;
		}

		pos->getAxes(&nj);

		command = new Vector;
		tmp = new Vector;
		command->resize(nj);
		tmp->resize(nj);

		for (int i = 0; i < nj; i++) {
		         (*tmp)[i] = 25.0;
		}
		pos->setRefAccelerations(tmp->data());

		for (int i = 0; i < nj; i++) {
			(*tmp)[i] = 20.0;
			pos->setRefSpeed(i, (*tmp)[i]);
		}

		*command = 0;

		//set the arm joints to "middle" values
		(*command)[0] = -45;
		(*command)[1] = 45;
		(*command)[2] = 0;
		(*command)[3] = 45;

		//flex hand
		(*command)[4] = 60;
		(*command)[7] = 20;
		(*command)[10] = 15;
		(*command)[11] = 15;
		(*command)[12] = 15;
		(*command)[13] = 15;
		(*command)[14] = 15;
		(*command)[15] = 15;
		pos->positionMove(command->data());

		bool done = false;
		while (!done){
			pos->checkMotionDone(&done);
			Time::delay(0.1);
		}

		bool fwCvOn = 0;
		fwCvOn = Network::connect("/babbleTrack/plan:o","/fwdConv:i");
		fwCvOn *= Network::connect("/fwdConv:o","/babbleTrack/pred:i");
		if (!fwCvOn){
			printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n");
			return false;
		}

		return true;
	}

//.........这里部分代码省略.........
    {
        rname=parameters.find("robot").asString();
    }
    else
        rname="controlboard";

    PolyDriver dd;
    Property p;

    Random::seed((unsigned int)Time::now());
     
    string remote=string("/")+rname.c_str();
    if (part!="")
        {
            remote+=string("/");
            remote+=part;
        }
    string local=string("/")+string(rname.c_str());
    if (part!="")
        {
            local+=string("/");
            local+=part;
        }
    local+=string("/stress");

    stringstream lStream; 
    lStream << id;
    local += lStream.str();

    p.put("device", "remote_controlboard");
    p.put("local", local.c_str());
    p.put("remote", remote.c_str());
    p.put("carrier", protocol.c_str());
    dd.open(p);
    
    if (!dd.isValid())
    {
        fprintf(stderr, "Error, could not open controlboard\n");
        return -1;
    }

    IEncoders *ienc;
    IPositionControl *ipos;
    IAmplifierControl *iamp;
    IPidControl *ipid;
    IControlLimits *ilim;
    IControlCalibration2 *ical;

    dd.view(ienc);
    dd.view(ipos);
    dd.view(iamp);
    dd.view(ipid);
    dd.view(ilim);
    dd.view(ical);
    
    int c=100;
    int nj;
    Vector encoders;
    ienc->getAxes(&nj);
    encoders.resize(nj);

    int count=0;
    bool done=false;
    double startT=Time::now();
    double now=0;
    while((!done) || (time==-1))
        {
            count++;
            double v;
            int jj=0;
            
            for(jj=0; jj<nj; jj++)
                {
                    //    ienc->getEncoder(jj, encoders.data()+jj);
                    //    iamp->enableAmp(jj);
                    //    ilim->setLimits(jj, 0, 0);
                    //    double max;
                    //    double min;
                    //    ilim->getLimits(jj, &min, &max);
                    fprintf(stderr, ".");
                    // Pid pid;
                    // ipid->getPid(jj, &pid);
                    bool done;
                    ipos->checkMotionDone(jj, &done);
                    fprintf(stderr, "#\n");
                }

            fprintf(stderr, "%u\n", count);

            Time::delay(0.1);

            now=Time::now();
            if ((now-startT)>time)
                done=true;
        }

    printf("bye bye from %d\n", id);
    
    return 0;
}

//.........这里部分代码省略.........
  {
      max_torques[k]=0;
      min_torques[k]=0;
      torques[k]=0;
  }

  if (!iiencs->getEncoders(positions)) 
      return true;
  itrq->getTorques(torques);
  iiencs->getEncoderSpeeds(speeds);
  
  //update all joints positions
  for (k = 0; k < NUMBER_OF_JOINTS; k++)
    {
      sprintf(buffer, "%.1f", positions[k]);  
      gtk_entry_set_text((GtkEntry*) pos_entry[k],  buffer);
      sprintf(buffer, "%.3f", torques[k]);  
      gtk_entry_set_text((GtkEntry*) trq_entry[k],  buffer);
      sprintf(buffer, "%.1f", speeds[k]);  
      gtk_entry_set_text((GtkEntry*) speed_entry[k],  buffer);
    }
  //update all joint sliders
  for (k = 0; k < NUMBER_OF_JOINTS; k++) 
    if(POS_UPDATE[k])
      gtk_range_set_value((GtkRange*)sliderAry[k],  positions[k]);

  // *** update the checkMotionDone box section ***
  // (only one at a time in order to save badwidth)
  k = slowSwitcher%NUMBER_OF_JOINTS;
  slowSwitcher++;
#if DEBUG_GUI
  gtk_entry_set_text((GtkEntry*) inEntry[k],  "off");
#else
  ipos->checkMotionDone(k, &done);
  if (!done)
      gtk_entry_set_text((GtkEntry*) inEntry[k],  " "); 
  else
      gtk_entry_set_text((GtkEntry*) inEntry[k],  "@");
#endif

  // *** update the controlMode section ***
  // the new icubinterface does not increase the bandwidth consumption
  // ret = true; useless guys!
  ret=ictrl->getControlModes(controlModes);
  if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" );
  ret=iint->getInteractionModes(interactionModes);
  if (ret==false) fprintf(stderr,"iint->getInteractionlMode failed\n" );

  for (k = 0; k < NUMBER_OF_JOINTS; k++)
  {
      if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue;
      controlModesOld[k]=controlModes[k];
      sprintf(frame_title,"Joint %d ",k );

      switch (controlModes[k])
      {
          case VOCAB_CM_IDLE:
              pColor=&color_yellow;
                strcat(frame_title," (IDLE)");
                gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_POSITION:
              pColor=&color_green;
              strcat(frame_title," (POSITION)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);

//.........这里部分代码省略.........
	}


     int jnts = 0;
     pos->getAxes(&jnts);
     printf("Working with %d axes\n", jnts);

     
     // limits

     vector<pair<int, int> > vLimitJoints;
     for (int i = 0; i < jnts; i++) {
         double min, max;
         lim->getLimits(i, &min, &max);
         vLimitJoints.push_back(pair<int, int> ((int)min, (int)max));
     }

     // get value of the arm
     // we move : 4 5 6 7 8 9 10 11 12 13 14 15 
     // we don't move: 0 1 2 3
    
	 vector<bool> mask;
	 mask.resize(jnts);
	 mask[0]	= false;
	 mask[1]	= false;
	 mask[2]	= false;
	 mask[3]	= false;
	 mask[4] = false;
	 mask[5] = false;
	 mask[6] = false;
	 mask[7] = false;
	 mask[8] = false;
	 mask[9] = false;
	 mask[10] = false;
	 mask[11] = true;
	 mask[12] = true;
	 mask[13] = true;
	 mask[14] = true;
	 mask[15] = true;
     
	 Vector tmp;
	 tmp.resize(jnts,0.0);

	 for (int i = 4; i < jnts; i++)
	 {
		pos->positionMove(i, 0.0);
	 }

	 bool initDone = false;
	 while (!initDone)
	 {
		 initDone = true;
		 for (int i = 4; i < jnts; i++)
		 {
				 bool jntMotionDone = false;
				 pos->checkMotionDone(i, &jntMotionDone);
				 initDone &= jntMotionDone;
			 }
	 }



     while(true)
     {
		 cout << "Moving to new posture..." << endl;

         for (int i = 4; i < jnts; i++) 
		 {
			 if (mask[i])
			 {
				 double newValue = yarp::os::Random::uniform(vLimitJoints[i].first, vLimitJoints[i].second);
				 tmp[i] = newValue;
				 pos->positionMove(i, tmp[i]);
			 }
		 }

		 cout << "Waiting for posture to be reached... ("<<tmp.toString(3,3)<<" ) ..." ;

		 bool motionDone = false;
		 while (!motionDone)
		 {
			 motionDone = true;
			 for (int i = 4; i < jnts; i++)
			 {
				 if (mask[i])
				 {
					 bool jntMotionDone = false;
					 pos->checkMotionDone(i, &jntMotionDone);
					 motionDone &= jntMotionDone;
				 }
			 }
		 }
		 Time::delay(15.0);
		 cout << "ok" << endl;
     }

	 dd.close();

    return 0;
}

int main(int argc, char *argv[]) 
{
    Network yarp;

    Property params;
    params.fromCommand(argc, argv);

    if (!params.check("robot"))
    {
        fprintf(stderr, "Please specify the name of the robot\n");
        fprintf(stderr, "--robot name (e.g. icub)\n");
        return -1;
    }
    std::string robotName=params.find("robot").asString().c_str();
    std::string remotePorts="/";
    remotePorts+=robotName;
    remotePorts+="/left_arm";

    std::string localPorts="/test/client";

    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", localPorts.c_str());   //local port names
    options.put("remote", remotePorts.c_str());         //where we connect to

    // create a device
    PolyDriver robotDevice(options);
    if (!robotDevice.isValid()) {
        printf("Device not available.  Here are the known devices:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    IPositionControl *pos;
    IEncoders *encs;

    bool ok;
    ok = robotDevice.view(pos);
    ok = ok && robotDevice.view(encs);

    if (!ok) {
        printf("Problems acquiring interfaces\n");
        return 0;
    }

    int nj=0;
    pos->getAxes(&nj);
    Vector encoders;
    Vector command;
    Vector tmp;
    encoders.resize(nj);
    tmp.resize(nj);
    command.resize(nj);
    
    int i;
    for (i = 0; i < nj; i++) {
         tmp[i] = 50.0;
    }
    pos->setRefAccelerations(tmp.data());

    for (i = 0; i < nj; i++) {
        tmp[i] = 4.0;
        pos->setRefSpeed(i, tmp[i]);
    }

    //pos->setRefSpeeds(tmp.data()))
    
    //fisrst zero all joints
    //
    command=0;
    //now set the shoulder to some value
    command[0]=-26.0;
    command[1]=20.0;
    command[2]=0.0;
    command[3]=49.0;
    command[4]=0.0;

    pos->positionMove(command.data());
    
    bool done=false;
    while(!done) {
        Time::delay(0.5);
        pos->checkMotionDone(&done);
    }

    printf("\niCub @ HOME. Press any key...");
    mygetch();

    int times=0;
    while(true)
    {
        times++;
        if (times%2)
        {
             printf("\n\nSet pos1: ");
             //command[0]=-50;
             command[1]=64.0;
             //command[2]=-10;
             //command[3]=50;
             //command[4]=0;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        maxSpeed = 10;
    }

    std::string robotName=params.find("robot").asString().c_str();
    std::string remotePorts="/";
    remotePorts+=robotName;
    remotePorts+="/head";

    int numTimes = params.find("repetitions").asInt();
    std::string localPorts="/headMovement_koroibot/client";

    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", localPorts.c_str());   //local port names
    options.put("remote", remotePorts.c_str());         //where we connect to

    // create a device
    PolyDriver robotDevice(options);
    if (!robotDevice.isValid()) {
        printf("Device not available.  Here are the known devices:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    IPositionControl *pos;
    IEncoders *encs;

    bool ok;
    ok = robotDevice.view(pos);
    ok = ok && robotDevice.view(encs);

    if (!ok) {
        printf("Problems acquiring interfaces\n");
        return 0;
    }

    int nj=0;
    pos->getAxes(&nj);
    Vector encoders;
    Vector command;
    Vector tmp;
    encoders.resize(nj);
    tmp.resize(nj);
    command.resize(nj);

    int i;
    for (i = 0; i < nj; i++) {
        tmp[i] = 50.0;
    }
    pos->setRefAccelerations(tmp.data());

    for (i = 0; i < nj; i++) {
        tmp[i] = 10.0;
        pos->setRefSpeed(i, tmp[i]);
    }

    //first read all encoders
    printf("waiting for encoders");
    while(!encs->getEncoders(encoders.data()))
    {
        Time::delay(0.1);
        printf(".");
    }
    printf("\n;");

    int ctr =0;
    bool done = false;
    while(ctr<numTimes)
    {


        printf("Starting headMovement\n");
        command=encoders;
        if(ctr%2 == 0)
            command[2]=HEAD_YAW_MAX;
        else
            command[2]=HEAD_YAW_MIN;
        done = false;
        pos->positionMove(command.data());
        while(!done)
        {
            pos->checkMotionDone(&done);
            Time::delay(0.1);
        }
        ctr++;
    }

    command[2] = 0;
    pos->positionMove(command.data());
    while(!done)
    {

        pos->checkMotionDone(&done);
        Time::delay(0.1);
    }

    robotDevice.close();

    return 0;
}

//.........这里部分代码省略.........
  {
	  max_torques[k]=0;
	  min_torques[k]=0;
	  torques[k]=0;
  }

  if (!iiencs->getEncoders(positions)) 
	  return true;
  itrq->getTorques(torques);
  iiencs->getEncoderSpeeds(speeds);
  
  //update all joints positions
  for (k = 0; k < NUMBER_OF_JOINTS; k++)
    {
      sprintf(buffer, "%.1f", positions[k]);  
      gtk_entry_set_text((GtkEntry*) pos_entry[k],  buffer);
      sprintf(buffer, "%.3f", torques[k]);  
	  gtk_entry_set_text((GtkEntry*) trq_entry[k],  buffer);
      sprintf(buffer, "%.1f", speeds[k]);  
	  gtk_entry_set_text((GtkEntry*) speed_entry[k],  buffer);
    }
  //update all joint sliders
  for (k = 0; k < NUMBER_OF_JOINTS; k++) 
    if(POS_UPDATE[k])
      gtk_range_set_value((GtkRange*)sliderAry[k],  positions[k]);

  // *** update the checkMotionDone box section ***
  // (only one at a time in order to save badwidth)
  k = slowSwitcher%NUMBER_OF_JOINTS;
  slowSwitcher++;
#if DEBUG_GUI
  gtk_entry_set_text((GtkEntry*) inEntry[k],  "off");
#else
  ipos->checkMotionDone(k, &done);
  if (!done)
      gtk_entry_set_text((GtkEntry*) inEntry[k],  " "); 
  else
      gtk_entry_set_text((GtkEntry*) inEntry[k],  "@");
#endif

  // *** update the controlMode section ***
  // the new icubinterface does not increase the bandwidth consumption
  // ret = true; useless guys!
  ret=ictrl->getControlModes(controlModes);

  if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" );
  for (k = 0; k < NUMBER_OF_JOINTS; k++)
  {
	  if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue;
	  controlModesOld[k]=controlModes[k];
	  sprintf(frame_title,"Joint %d ",k );

	  switch (controlModes[k])
	  {
		  case VOCAB_CM_IDLE:
			  pColor=&color_yellow;
  			  strcat(frame_title," (IDLE)");
  			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
		  break;
		  case VOCAB_CM_POSITION:
			  pColor=&color_green;
			  strcat(frame_title," (POSITION)");
			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
			  gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:");

//.........这里部分代码省略.........
	}

	IPositionControl *pos;
	IEncoders *enc;

	bool ok;
	ok = robotDevice.view(pos);
	ok = ok && robotDevice.view(enc);

	if (!ok){
		printf("Problems acquiring interfaces\n");
		return 0;
	}

	int nj = 0;
	pos->getAxes(&nj);
	Vector encoders;
	Vector command;
	Vector commandCart;
	Vector tmp;
	encoders.resize(nj);
	tmp.resize(nj);
	command.resize(nj);
	commandCart.resize(nj);

    for (int i = 0; i < nj; i++) {
         tmp[i] = 25.0;
    }
    pos->setRefAccelerations(tmp.data());

    for (int i = 0; i < nj; i++) {
        tmp[i] = 5.0;
        pos->setRefSpeed(i, tmp[i]);
    }

    command = 0;

    //set the arm joints to "middle" values
    command[0] = -45;
    command[1] = 45;
    command[2] = 0;
    command[3] = 45;
    pos->positionMove(command.data());

    bool done = false;
    while (!done){
    	pos->checkMotionDone(&done);
    	Time::delay(0.1);
    }

    while (true){
    	tmp = command;
    	command[0] += 15*(2*gsl_rng_uniform(r)-1);
    	command[1] += 15*(2*gsl_rng_uniform(r)-1);
    	command[2] += 15*(2*gsl_rng_uniform(r)-1);
    	command[3] += 15*(2*gsl_rng_uniform(r)-1);
    	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] < -90){
    		command[0] = tmp[0];
    	}
    	if (command[1] > 160 || command[1] < -0){
    		command[1] = tmp[1];
    	}
    	if (command[2] > 100 || 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);
    	for (int i = 0; i < 3; i++){
    		commandCart[i] = pred.get(i).asDouble();
    	}
    	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());
    		done = false;
    		while(!done){
    			pos->checkMotionDone(&done);
    			Time::delay(0.1);
    		}
    	}
    	else{
    		printf("Self collision detected!\n");
    	}
    }

    robotDevice.close();
    gsl_rng_free(r);

    return 0;
}