C++ IEncoders::getEncoders方法代码示例

    void run()
    {
        //do the work
        iencs->getEncoders(encoders.data());

        count++;

        if (count%2)
            commands=5;
        else
            commands=-5;
    
        ivel->velocityMove(commands.data());

        printf(".");
    }

    bool updateModule()
    {
        ImageOf<PixelRgb> *imgIn=imgInPort.read(true);
        if (imgIn==NULL)
            return false;

        ImageOf<PixelRgb> &imgOut=imgOutPort.prepare();
        imgOut=*imgIn;

        cv::Mat img=cv::cvarrToMat(imgOut.getIplImage());

        Vector xa,oa;
        iarm->getPose(xa,oa);

        Matrix Ha=axis2dcm(oa);
        xa.push_back(1.0);
        Ha.setCol(3,xa);

        Vector pc;
        igaze->get2DPixel(camSel,xa,pc);

        cv::Point point_c((int)pc[0],(int)pc[1]);
        cv::circle(img,point_c,4,cv::Scalar(0,255,0),4);

        Vector analogs,encs(nEncs),joints;
        if (ianalog!=NULL)
            ianalog->read(analogs);
        iencs->getEncoders(encs.data());

        for (int i=0; i<3; i++)
        {
            if (ianalog!=NULL)
                finger[i].getChainJoints(encs,analogs,joints);
            else
                finger[i].getChainJoints(encs,joints);
            finger[i].setAng(CTRL_DEG2RAD*joints);
        }

        for (int fng=0; fng<3; fng++)
        {
            deque<cv::Point> point_f;
            for (int i=-1; i<(int)finger[fng].getN(); i++)
            {
                Vector fc;
                igaze->get2DPixel(camSel,Ha*(i<0?finger[fng].getH0().getCol(3):
                                                 finger[fng].getH(i,true).getCol(3)),fc);
                point_f.push_front(cv::Point((int)fc[0],(int)fc[1]));
                cv::circle(img,point_f.front(),3,cv::Scalar(0,0,255),4);

                if (i>=0)
                {
                    cv::line(img,point_f.front(),point_f.back(),cv::Scalar(255,255,255),2);
                    point_f.pop_back();
                }
                else
                    cv::line(img,point_c,point_f.front(),cv::Scalar(255,0,0),2);
            }
        }

        imgOutPort.writeStrict();
        return true;
    }

	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;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
  static yarp::dev::InteractionModeEnum interactionModes[MAX_NUMBER_OF_JOINTS];
  static yarp::dev::InteractionModeEnum interactionModesOld[MAX_NUMBER_OF_JOINTS];

  int k;
  int NUMBER_OF_JOINTS=0;
  bool done = false;
  bool ret = false;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  if (NUMBER_OF_JOINTS == 0)
  {
      fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" );
      Time::delay(0.1);
      pColor=&color_grey;
      strcpy(frame_title,"DISCONNECTED");
      for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++)
      {   
          if (currentPart->framesArray[k]!=0)
          {
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          }
      }
      return true;
  }

  for (k = 0; k < NUMBER_OF_JOINTS; k++) 
  {
      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

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

void partMover::fixed_time_move(const double *cmdPositions, double cmdTime, partMover* currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  ITorqueControl *itrq= currentPart->trq;

  int *SEQUENCE_TMP = currentPart->SEQUENCE;
  double *TIMING_TMP = currentPart->TIMING;
  double **STORED_POS_TMP = currentPart->STORED_POS;
  double **STORED_VEL_TMP = currentPart->STORED_VEL;
  GtkWidget **sliderAry = currentPart->sliderArray;
  GtkWidget **sliderVelAry = currentPart->sliderVelArray;
  int NUM_JOINTS;
  ipos->getAxes(&NUM_JOINTS);
  double *cmdVelocities = new double[NUM_JOINTS];
  double *startPositions = new double[NUM_JOINTS];

  while (!iiencs->getEncoders(startPositions))
    Time::delay(0.001);
    //fprintf(stderr, "getEncoders is returning false\n");
    //fprintf(stderr, "Getting the following values for the encoders");
  //for(int k=0; k<NUM_JOINTS; k++)
  //  fprintf(stderr, "%.1f ", startPositions[k]);
  //fprintf(stderr, "\n");  

  int k;
  for(k=0; k<NUM_JOINTS; k++)
    {
      cmdVelocities[k] = 0;
		
      if (fabs(startPositions[k] - cmdPositions[k]) > 0.01)
	cmdVelocities[k] = fabs(startPositions[k] - cmdPositions[k])/cmdTime;
      else
	cmdVelocities[k] = 1.0;
    }

  //fprintf(stderr, "ExplorerThread-> Start pos:\n");
  //for(int j=0; j < NUM_JOINTS; j++)
  //  fprintf(stderr, "%.2lf\t", startPositions[j]);
  //fprintf(stderr, "\n");
  //fprintf(stderr, "ExplorerThread-> Moving arm to:\n");
  //for(int j=0; j < NUM_JOINTS; j++)
  //  fprintf(stderr, "%.2lf\t", cmdPositions[j]);
  //fprintf(stderr, "\n");
  //fprintf(stderr, "ExplorerThread-> with velocity:\n");
  //for(int ii=0; ii < NUM_JOINTS; ii++)
  //  fprintf(stderr, "%.2lf\t", cmdVelocities[ii]);
  //fprintf(stderr, "\n");

  ipos->setRefSpeeds(cmdVelocities);
  ipos->positionMove(cmdPositions);	

  currentPart->sequence_port_stamp.update();
  currentPart->sequence_port.setEnvelope(currentPart->sequence_port_stamp);
  Vector v(NUM_JOINTS,cmdPositions);
  currentPart->sequence_port.write(v);
  delete cmdVelocities;
  delete startPositions;
  return;
}

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

        case VOCAB_IMPEDANCE:
            {
                handleImpedanceMsg(iimp, p, response, &rec, &ok);
                printf("%s\n", response.toString().c_str());
                break;
            }

		case VOCAB_TORQUE:
			{
				handleTorqueMsg(itorque, p, response, &rec, &ok);
				printf("%s\n", response.toString().c_str());
				break;
			}

        case VOCAB_INTERFACE_INTERACTION_MODE:
            {
                handleInteractionModeMsg(iInteract, p, response, &rec, &ok);
                printf("%s\n", response.toString().c_str());
                break;
            }

        case VOCAB_GET:
            switch(p.get(1).asVocab()) {
                case VOCAB_AXES: {
                    int nj = 0;
                    enc->getAxes(&nj);
                    printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj);
                }
                break;

                case VOCAB_ENCODERS: {
                    enc->getEncoders(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;

                case VOCAB_PID: {
                    Pid pd;
                    int j = p.get(2).asInt();
                    pid->getPid(j, &pd);
                    printf("%s: ", Vocab::decode(VOCAB_PID).c_str());
                    printf("kp %.2f ", pd.kp);
                    printf("kd %.2f ", pd.kd);
                    printf("ki %.2f ", pd.ki);
                    printf("maxi %.2f ", pd.max_int);
                    printf("maxo %.2f ", pd.max_output);
                    printf("off %.2f ", pd.offset);
                    printf("scale %.2f ", pd.scale);
                    printf("\n");
                }
                break;

               case VOCAB_PIDS: {
                    Pid *p = new Pid[jnts];
                    ok = pid->getPids(p);
                    Bottle& b = response.addList();
                    int i;
                    for (i = 0; i < jnts; i++)
                        {
                          Bottle& c = b.addList();
                          c.addDouble(p[i].kp);

//.........这里部分代码省略.........
            printf("[%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str());
            printf("[%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str());            
            printf("[%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
            printf("[%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
            printf("[%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
            printf("[%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
            printf("[%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
            printf("[%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());          
            printf("[%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str());
            printf("[%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
            printf("[%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
            printf("[%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
            printf("[%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str());
            printf("[%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str());
            printf("[%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str());
            printf("[%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
            printf("\n");
            break;

        case VOCAB_QUIT:
            goto ApplicationCleanQuit;
            break;

        case VOCAB_GET:
            switch(p.get(1).asVocab()) {
                case VOCAB_AXES: {
                    int nj = 0;
                    enc->getAxes(&nj);
                    printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj);
                }
                break;

                case VOCAB_ENCODERS: {
                    enc->getEncoders(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;

                case VOCAB_PID: {
                    Pid pd;
                    int j = p.get(2).asInt();
                    pid->getPid(j, &pd);
                    printf("%s: ", Vocab::decode(VOCAB_PID).c_str());
                    printf("kp %.2f ", pd.kp);
                    printf("kd %.2f ", pd.kd);
                    printf("ki %.2f ", pd.ki);
                    printf("maxi %.2f ", pd.max_int);
                    printf("maxo %.2f ", pd.max_output);
                    printf("off %.2f ", pd.offset);
                    printf("scale %.2f ", pd.scale);
                    printf("\n");
                }
                break;

                case VOCAB_LIMITS: {
                    double min, max;
                    int j = p.get(2).asInt();
                    lim->getLimits(j, &min, &max);
                    printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str());
                    printf("limits: (%.2f %.2f)\n", min, max);
                }
                break;

//.........这里部分代码省略.........
    joint_states.position[45] = 0.0;
    joint_states.name[46] ="rij4";
    joint_states.position[46] = 0.0;
    joint_states.name[47] ="rij5";
    joint_states.position[47] = 0.0;
    joint_states.name[48] ="raj4";
    joint_states.position[48] = 0.0;
    joint_states.name[49] ="raj5";
    joint_states.position[49] = 0.0;
    joint_states.name[50] ="lij3";
    joint_states.position[50] = 0.0;
    joint_states.name[51] ="lij4";
    joint_states.position[51] = 0.0;
    joint_states.name[52] ="lij5";
    joint_states.position[52] = 0.0;
    joint_states.name[53] ="left_wrist_yaw";
    joint_states.position[53] = 0.0;
    joint_states.name[54] ="ltj2";
    joint_states.position[54] = 0.0;
    joint_states.name[55] ="ltj4";
    joint_states.position[55] = 0.0;
    joint_states.name[56] ="ltj5";
    joint_states.position[56] = 0.0;
    joint_states.name[57] ="ltj6";
    joint_states.position[57] = 0.0;
    joint_states.name[58] ="laij3";
    joint_states.position[58] = 0.0;
    joint_states.name[59] ="laij4";
    joint_states.position[59] = 0.0;
    joint_states.name[60] ="laij5";
    joint_states.position[60] = 0.0;
    joint_states.name[61] ="lmj3";
    joint_states.position[61] = 0.0;
    joint_states.name[62] ="lmj4";
    joint_states.position[62] = 0.0;
    joint_states.name[63] ="lmj5";
    joint_states.position[63] = 0.0;
    joint_states.name[64] ="lrij3";
    joint_states.position[64] = 0.0;
    joint_states.name[65] ="lrij4";
    joint_states.position[65] = 0.0;
    joint_states.name[66] ="lrij5";
    joint_states.position[66] = 0.0;
    joint_states.name[67] ="llij3";
    joint_states.position[67] = 0.0;
    joint_states.name[68] ="llij4";
    joint_states.position[68] = 0.0;
    joint_states.name[69] ="llij5";
    joint_states.position[69] = 0.0;

    joint_pub.write(joint_states);

    while (1) {

      encsTorso->getEncoders(encodersTorso.data());
      encsRArm->getEncoders(encodersRArm.data());
      encsLArm->getEncoders(encodersLArm.data());
      encsHead->getEncoders(encodersHead.data());


      clock_gettime(CLOCK_REALTIME, &currentTime);

      joint_states.header.stamp.sec = currentTime.tv_sec;
      joint_states.header.stamp.nsec = currentTime.tv_nsec;


      // update torso positions
      joint_states.position[0] = encodersTorso[2] * degtorad;
      joint_states.position[1] = encodersTorso[1] * degtorad;
      joint_states.position[2] = encodersTorso[0] * degtorad;

      // update head positions
      joint_states.position[3] = encodersHead[0] * degtorad;
      joint_states.position[4] = encodersHead[1] * degtorad;
      joint_states.position[5] = encodersHead[2] * degtorad;

      // update right arm positions
      joint_states.position[10] = encodersRArm[0] * degtorad;
      joint_states.position[11] = encodersRArm[1] * degtorad;
      joint_states.position[12] = encodersRArm[2] * degtorad;
      joint_states.position[13] = encodersRArm[3] * degtorad;
      joint_states.position[14] = encodersRArm[4] * degtorad;
      joint_states.position[15] = encodersRArm[5] * degtorad;

      // update left arm positions
      joint_states.position[16] = encodersLArm[0] * degtorad;
      joint_states.position[17] = encodersLArm[1] * degtorad;
      joint_states.position[18] = encodersLArm[2] * degtorad;
      joint_states.position[19] = encodersLArm[3] * degtorad;
      joint_states.position[20] = encodersLArm[4] * degtorad;
      joint_states.position[21] = encodersLArm[5] * degtorad;

      joint_pub.write(joint_states);

      Time::delay(0.01);
    }


  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");
        fprintf(stderr, "--part name (right_arm or left_arm)\n");
        fprintf(stderr, "--durationInS duration (in seconds)\n");
        fprintf(stderr, "--sampleInMS sample time (in milliseconds)\n");
        fprintf(stderr, "--refTimeInMS Reference time of minimum jerk trajectory generator (in milliseconds)\n");
        return 1;
    }

    if( !params.check("sampleInMS") ||
        !params.check("durationInS") ||
        !params.check("refTimeInMS") )
    {
        fprintf(stderr, "Necessary params not passed\n");
        return EXIT_FAILURE;
    }

    std::string robotName=params.find("robot").asString().c_str();
    std::string partName=params.find("part").asString().c_str();
    double durationInS = params.find("durationInS").asDouble();
    double samplesInMS = params.find("sampleInMS").asDouble();
    double refTimeInMS = params.find("refTimeInMS").asDouble();

    std::cout << "robotName : " << robotName << std::endl;
    std::cout << "partName : " << partName << std::endl;
    std::cout << "durationInS : " << durationInS << std::endl;
    std::cout << "samplesInMS : " << samplesInMS << std::endl;
    std::cout << "refTimeInMS : " << refTimeInMS << std::endl;

    std::string remotePorts="/";
    remotePorts+=robotName;
    remotePorts+="/"+partName;

    std::string localPorts="/randomShoulderMovements/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;
    }

    IPositionDirect *pos;
    IEncoders *encs;
    IControlLimits * lims;
    IControlMode2 * ictrl;


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

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

    int nj=0;
    pos->getAxes(&nj);
    Vector encodersInDeg(nj);
    Vector desPosInDeg(3);
    Vector commandInDeg(3);
    Vector minShoulderInDeg(3);
    Vector maxShoulderInDeg(3);
    Vector lowerBoundConstraint;
    Vector upperBoundConstraint;
    Matrix constraintMatrix;

    for(int i = 0; i < 3; i++ )
    {
        lims->getLimits(i,&(minShoulderInDeg[i]),&(maxShoulderInDeg[i]));
    }

    bool encodersRead = encs->getEncoders(encodersInDeg.data());

    while( !encodersRead )
    {
        encodersRead = encs->getEncoders(encodersInDeg.data());
    }


    getShoulderConstraint(lowerBoundConstraint,upperBoundConstraint,constraintMatrix);

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

//.........这里部分代码省略.........
    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]);
    }

    //pos->setRefSpeeds(tmp.data()))
    
    //fisrst zero all joints
    //
    command=0;
    //now set the shoulder to some value
    command[0]=50;
    command[1]=20;
    command[2]=-10;
    command[3]=50;
    pos->positionMove(command.data());
    
    bool done=false;

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

    int times=0;
    while(true)
    {
        times++;
        if (times%2)
        {
             command[0]=50;
             command[1]=20;
             command[2]=-10;
             command[3]=50;
//             pos->positionMove(command.data());
             pos->positionMove(0, command[0]);
             pos->positionMove(1, command[1]);
             pos->positionMove(2, command[2]);
             pos->positionMove(3, command[3]);
        }
        else
        {
             command[0]=20;
             command[1]=10;
             command[2]=-10;
             command[3]=30;
//             pos->positionMove(command.data());
             pos->positionMove(0, command[0]);
             pos->positionMove(1, command[1]);
             pos->positionMove(2, command[2]);
             pos->positionMove(3, command[3]);
        }

        int count=50;
        while(count--)
            {
                Time::delay(0.1);
                encs->getEncoders(encoders.data());
                printf("%.1lf %.1lf %.1lf %.1lf\n", encoders[0], encoders[1], encoders[2], encoders[3]);
            }
    }

    robotDevice.close();
    
    return 0;
}

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

    int maxSpeed;
    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;
    }

    if (!params.check("repetitions"))
    {
        fprintf(stderr, "Please specify number of repetitions\n");
        fprintf(stderr, "--repetitions num (e.g. 10)\n");
        return -1;
    }

    if (!params.check("speed"))
    {
        fprintf(stderr, "Speed not specified using default\n");
        fprintf(stderr, "--speed num (e.g. 2)\n");
        maxSpeed = 10.0;
    }
    else
    {
        maxSpeed = params.find("speed").asInt();
    }

    // sanity check on argument value
    if(maxSpeed <0 || maxSpeed>50)
    {
        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;");
//.........这里部分代码省略.........

//.........这里部分代码省略.........
  static int controlModes[MAX_NUMBER_OF_JOINTS];
  static int controlModesOld[MAX_NUMBER_OF_JOINTS];

  int k;
  int NUMBER_OF_JOINTS=0;
  bool done = false;
  bool ret = false;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  if (NUMBER_OF_JOINTS == 0)
  {
      fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" );
      Time::delay(0.1);
      pColor=&color_grey;
      strcpy(frame_title,"DISCONNECTED");
      for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++)
      {   
          if (currentPart->framesArray[k]!=0)
          {
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          }
      }
      return true;
  }

  for (k = 0; k < NUMBER_OF_JOINTS; k++) 
  {
	  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

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