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

    bool goHome()
    {
        printMessage(0,"Going home...\n");
        yarp::sig::Vector poss(7,0.0);
        yarp::sig::Vector vels(7,0.0);
        printMessage(1,"Configuring arm...\n");
        poss[0]=-30.0;  vels[0]=10.0;                     poss[1]=30.0;  vels[1]=10.0;
        poss[2]= 00.0;  vels[2]=10.0;                     poss[3]=45.0;  vels[3]=10.0;
        poss[4]= 00.0;  vels[4]=10.0;                     poss[5]=00.0;  vels[5]=10.0;
        poss[6]= 00.0;  vels[6]=10.0;

        for (int i=0; i<7; i++)
        {
            iposs->setRefSpeed(i,vels[i]);
            iposs->positionMove(i,poss[i]);
        }

        printMessage(1,"Configuring hand...\n");
        poss.resize(9,0.0);
        vels.resize(9,0.0);

        poss[0]=40.0;  vels[0]=60.0;                     poss[1]=10.0;  vels[1]=60.0;
        poss[2]=60.0;  vels[2]=60.0;                     poss[3]=70.0;  vels[3]=60.0;
        poss[4]=00.0;  vels[4]=60.0;                     poss[5]=00.0;  vels[5]=60.0;
        poss[6]=70.0;  vels[6]=60.0;                     poss[7]=100.0; vels[7]=60.0;
        poss[8]=240.0; vels[8]=120.0; 
        for (int i=7; i<nEncs; i++)
        {
            iposs->setRefAcceleration(i,1e9);
            iposs->setRefSpeed(i,vels[i-7]);
            iposs->positionMove(i,poss[i-7]);
        }
        return true;
    }

void partMover::slider_release(GtkRange *range, gtkClassData* currentClassData)
{    
  partMover *currentPart = currentClassData->partPointer;
  int * joint = currentClassData->indexPointer;
  bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
  IPositionControl *ipos = currentPart->pos;
  IPidControl      *ipid = currentPart->pid;
  IPositionDirect  *iDir = currentPart->iDir;
  GtkWidget **sliderVel = currentPart->sliderVelArray;

  double val = gtk_range_get_value(range);
  double valVel = gtk_range_get_value((GtkRange *)sliderVel[*joint]);

  IControlMode     *iCtrl = currentPart->ctrlmode2;
  int mode;

  iCtrl->getControlMode(*joint, &mode);

  if (!POS_UPDATE[*joint])
    {
      if( ( mode == VOCAB_CM_POSITION) || (mode == VOCAB_CM_MIXED) )
      {
         ipos->setRefSpeed(*joint, valVel);
         ipos->positionMove(*joint, val);
      }
      else if( ( mode == VOCAB_CM_IMPEDANCE_POS))
      {
         fprintf(stderr, " using old 'impedance_position' mode, this control mode is deprecated!");
         ipos->setRefSpeed(*joint, valVel);
         ipos->positionMove(*joint, val);
      }
      else if ( mode == VOCAB_CM_POSITION_DIRECT)
      {
         if (position_direct_enabled)
         {
             iDir->setPosition(*joint, val);
         }
         else
         {
             fprintf(stderr, "You cannot send direct position commands without using --direct option!");
         }

      }
      else
      {
          fprintf(stderr, "Joint not in position nor positionDirect so cannot send references");
      }
    }
  return;
}

void partMover::home_click(GtkButton *button, gtkClassData* currentClassData)
{
  partMover *currentPart = currentClassData->partPointer;
  int * joint = currentClassData->indexPointer;
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  IControlCalibration2 *ical = currentPart->cal;
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  //fprintf(stderr, "Retrieving finder \n");
  ResourceFinder *fnd = currentPart->finder;
  //fprintf(stderr, "Retrieved finder: %p \n", fnd);
  char buffer1[800];
  char buffer2[800];

  strcpy(buffer1, currentPart->partLabel);
  strcpy(buffer2, strcat(buffer1, "_zero"));
  //fprintf(stderr, "Finder retrieved %s\n", buffer2);

  if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
    {
      //fprintf(stderr, "Home group was not empty \n");
      bool ok = true;
      Bottle xtmp;
      xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double positionZero = xtmp.get(*joint+1).asDouble();
      //fprintf(stderr, "%f\n", positionZero);

      xtmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
      //fprintf(stderr, "VALUE VEL is %d \n", fnd->findGroup(buffer2).find("VelocityZero").toString().c_str());
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      double velocityZero = xtmp.get(*joint+1).asDouble();
      //fprintf(stderr, "%f\n", velocityZero);

      if(!ok)
    dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group",  buffer2, true);
      else
    {
      ipos->setRefSpeed(*joint, velocityZero);
      ipos->positionMove(*joint, positionZero);
    }
    }
  else
    {
      //        currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);        
      dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable",  buffer2, true);   
    }
  return;
}

    void _send(const ActionItem *x)
    {
        if (!connected)
        {
            cerr<<"Error: not connected to control board skipping"<<endl;
            return;
        }

        int size=x->getCmd().size();
        int offset=x->getOffset();
        double time=x->getTime();
        int nJoints=0;

        enc->getAxes(&nJoints);
        if ((offset+size)>nJoints)
        {
            cerr<<"Error: detected possible overflow, skipping"<<endl;
            cerr<<"For debug --> joints: "<<nJoints<< " off: "<<offset<<" cmd length: "<<size<<endl;
            return;
        }

        Vector disp(size);

        if (time==0)
        {
            return;
        }

        for (size_t i=0; i<disp.length(); i++)
        {
            double q;


            if (!enc->getEncoder(offset+i,&q))
            {
                cerr<<"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
                return;
            }

            disp[i]=x->getCmd()[i]-q;

            if (disp[i]<0.0)
                disp[i]=-disp[i];
        }

        for (size_t i=0; i<disp.length(); i++)
        {
            pos->setRefSpeed(offset+i,disp[i]/time);
            pos->positionMove(offset+i,x->getCmd()[i]);
        }

        cout << "Script port: " << const_cast<Vector &>(x->getCmd()).toString() << endl;
    }

void partMover::home_all(GtkButton *button, partMover* currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  IControlCalibration2 *ical = currentPart->cal;
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  //fprintf(stderr, "Retrieving finder \n");
  ResourceFinder *fnd = currentPart->finder;
  //fprintf(stderr, "Retrieved finder: %p \n", fnd);
  char buffer1[800];
  char buffer2[800];

  strcpy(buffer1, currentPart->partLabel);
  strcpy(buffer2, strcat(buffer1, "_zero"));
  //fprintf(stderr, "Finder retrieved %s\n", buffer2);


  if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
    {
      bool ok = true;
      Bottle xtmp, ytmp;
      xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
      ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
      ytmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
      ok = ok && (ytmp.size() == NUMBER_OF_JOINTS+1);
      if(ok)
	{
	  for (int joint = 0; joint < NUMBER_OF_JOINTS; joint++)
	    {
	      double positionZero = xtmp.get(joint+1).asDouble();
	      //fprintf(stderr, "%f ", positionZero);

	      double velocityZero = ytmp.get(joint+1).asDouble();
	      //fprintf(stderr, "%f ", velocityZero);

	      ipos->setRefSpeed(joint, velocityZero);
	      ipos->positionMove(joint, positionZero);
	    }
	}
      else
	dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group",  buffer2, true);
    }
  else
    {
      //		currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);        
      dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable",  buffer2, true);   
    }
  return;
}

void partMover::sliderVel_release(GtkRange *range, gtkClassData* currentClassData)
{	
  partMover *currentPart = currentClassData->partPointer;
  int * joint = currentClassData->indexPointer;
  IPositionControl *ipos = currentPart->pos;
  GtkWidget **sliderAry = currentPart->sliderArray;

  double val = gtk_range_get_value(range);
  double posit = gtk_range_get_value((GtkRange *) sliderAry[*joint]);
  ipos->setRefSpeed(*joint, val);
  ipos->positionMove(*joint, posit);
  return;
}

    bool updateModule()
    {
        if (calibrate)
        {
            Property options;
            options.put("finger",fingerName.c_str());
            model->calibrate(options);
            calibrate=false;

            ipos->setRefAcceleration(joint,1e9);
            if ((fingerName=="ring")||(fingerName=="little"))
                ipos->setRefSpeed(joint,60.0);
            else
                ipos->setRefSpeed(joint,30.0);

            ipos->positionMove(joint,*val);
        }
        else
        {
            if (Node *finger=model->getNode(fingerName))
            {
                Value data; finger->getSensorsData(data);
                Value out;  finger->getOutput(out);
                fprintf(stdout,"%s sensors data = %s; output = %s\n",
                        finger->getName().c_str(),data.toString().c_str(),out.toString().c_str());
            }
            
            double fb; ienc->getEncoder(joint,&fb);
            if (fabs(*val-fb)<5.0)
            {
                val==&min?val=&max:val=&min;
                ipos->positionMove(joint,*val);
            }
        }

        return true;
    }

void ReachManager::InitPositionControl(string partName)
{
    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", ("/reach_manager/position_control/" + partName).c_str());   //local port names

    string remotePortName = "/" + (string)parameters["robot"]->asString() + "/" + partName + "_arm";
    options.put("remote", remotePortName.c_str());         //where we connect to

    // create a device
    polydrivers[partName] = new PolyDriver(options);
    if (!polydrivers[partName]->isValid()) {
        printf("Device not available.  Here are the known devices:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return;
    }

    IPositionControl *pos;
    IEncoders *encs;

    if (!(polydrivers[partName]->view(pos) && polydrivers[partName]->view(encs))) {
        printf("Problems acquiring interfaces\n");
        return;
    }

    pos->getAxes(&nbJoints[partName]);
    Vector encoders;
    Vector tmp;
    encoders.resize(nbJoints[partName]);
    tmp.resize(nbJoints[partName]);


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

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

void partMover::slider_release(GtkRange *range, gtkClassData* currentClassData)
{	
  partMover *currentPart = currentClassData->partPointer;
  int * joint = currentClassData->indexPointer;
  bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
  IPositionControl *ipos = currentPart->pos;
  IPidControl      *ipid = currentPart->pid;
  GtkWidget **sliderVel = currentPart->sliderVelArray;

  double val = gtk_range_get_value(range);
  double valVel = gtk_range_get_value((GtkRange *)sliderVel[*joint]);

  if (!POS_UPDATE[*joint])
    {
      ipos->setRefSpeed(*joint, valVel);
      ipos->positionMove(*joint, val);
      //ipid->setReference(*joint, val);
    }
  return;
}

//.........这里部分代码省略.........
                case VOCAB_AMP_CURRENTS: {
                    amp->getCurrents(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;
            }
            break;

        case VOCAB_SET:
            switch(p.get(1).asVocab()) {
                case VOCAB_POSITION_MOVE: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
                    pos->positionMove(j, ref);
                }
                break;

                case VOCAB_VELOCITY_MOVE: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
                    vel->velocityMove(j, ref);
                }
                break;

                case VOCAB_REF_SPEED: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref);
                    pos->setRefSpeed(j, ref);
                }
                break;

                case VOCAB_REF_ACCELERATION: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
                    pos->setRefAcceleration(j, ref);
                }
                break;

                case VOCAB_POSITION_MOVES: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
                    pos->positionMove(tmp);
                }
                break;

                case VOCAB_VELOCITY_MOVES: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
                    vel->velocityMove(tmp);
                }
                break;

                case VOCAB_REF_SPEEDS: {

//.........这里部分代码省略.........
		else {
			string tmpcrname = "/" + robotname + "/cartesianController/left_arm";
			optiona.put("remote",tmpcrname.c_str());
			optiona.put("local","/cartesian_client/left_arm");
		}

		clientArmCart.open(optiona);
		carm = NULL;
		if (clientArmCart.isValid())
		{
			clientArmCart.view(carm);
		} else {
			printf("could not initialize arm cartesian control interface, failing...\n");
			return false;
		}

		// set trajectory time
		carm->setTrajTime(trajTime);	//slow for safety

		// get the torso dofs
		Vector newDof, curDof;
		carm->getDOF(curDof);
		newDof=curDof;

		//enable torso pitch and yaw, also wrist movements
		newDof[0]=1;
		newDof[1]=0;
		newDof[2]=1;
		newDof[7]=1;
		newDof[8]=1;
		newDof[9]=1;

		// impose some restriction on the torso pitch
		double tpmin, tpmax;
		carm->getLimits(0,&tpmin,&tpmax);
		carm->setLimits(0,tpmin,maxPitch);

		// send the request for dofs reconfiguration
		carm->setDOF(newDof,curDof);


		string lname, rname;
		Property doption;
		doption.put("device", "remote_controlboard");

		lname = "/"+name+"/torso"; rname = "/"+robotname+"/torso";
		doption.put("local", lname.c_str());
		doption.put("remote", rname.c_str());

		robotTorso.open(doption);
		if (!robotTorso.isValid()) {
			printf("could not initialize torso control interface, failing...\n");
			return false;
		}
		robotTorso.view(tPos); robotTorso.view(tEnc);

		lname = "/"+name+"/head"; rname = "/"+robotname+"/head";
		doption.put("local", lname.c_str());
		doption.put("remote", rname.c_str());

		robotHead.open(doption);
		if (!robotHead.isValid()) {
			printf("could not initialize head control interface, failing...\n");
			return false;
		}
		robotHead.view(hPos); robotHead.view(hEnc);

		lname = "/"+name+"/"+armname+"_arm"; rname = "/"+robotname+"/"+armname+"_arm";
		doption.put("local", lname.c_str());
		doption.put("remote", rname.c_str());

		robotArm.open(doption);
		if (!robotArm.isValid()) {
			printf("could not initialize arm control interface, failing...\n");
			return false;
		}
		robotArm.view(aPos); robotArm.view(aEnc);

		for (int i=0; i<7; i++) {
			aPos->setRefSpeed(i, 10.0);
		}

		/* Create and open ports */
		cportL=new ClickPort(bfL);
		string cplName="/"+name+"/clk:l";
		cportL->open(cplName.c_str());
		cportL->useCallback();

		cportR=new ClickPort(bfR);
		string cprName="/"+name+"/clk:r";
		cportR->open(cprName.c_str());
		cportR->useCallback();

		susPort=new Port;
		string suspName="/"+name+"/sus:o";
		susPort->open(suspName.c_str());

		return true;

	}

//.........这里部分代码省略.........
                case VOCAB_AMP_CURRENTS: {
                    amp->getCurrents(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;
            }
            break;

        case VOCAB_SET:
            switch(p.get(1).asVocab()) {
                case VOCAB_POSITION_MOVE: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
                    pos->positionMove(j, ref);
                }
                break;

                case VOCAB_VELOCITY_MOVE: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
                    vel->velocityMove(j, ref);
                }
                break;

                case VOCAB_REF_SPEED: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref);
                    pos->setRefSpeed(j, ref);
                }
                break;

                case VOCAB_REF_ACCELERATION: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
                    pos->setRefAcceleration(j, ref);
                }
                break;

                case VOCAB_POSITION_MOVES: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
                    pos->positionMove(tmp);
                }
                break;

                case VOCAB_VELOCITY_MOVES: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
                    vel->velocityMove(tmp);
                }
                break;

                case VOCAB_REF_SPEEDS: {

//.........这里部分代码省略.........
        {
            yInfo("elapsed %d [s]",(int)(Time::now()-t0));
            Time::delay(1.0);
            if (interrupting)
            {
                idlingCoupledJoints(i,false);
                return false;
            }
        }

        Property pResults;
        designer.getResults(pResults);
        double tau=pResults.find("tau_mean").asDouble();
        double K=pResults.find("K_mean").asDouble();
        yInfo("plant = %g/s * 1/(1+s*%g)",K,tau);

        Property pControllerRequirements,pController;
        pControllerRequirements.put("tau",tau);
        pControllerRequirements.put("K",K);
        pControllerRequirements.put("f_c",0.75);

        if (i!=15)
        {
            pControllerRequirements.put("T_dr",1.0);
            pControllerRequirements.put("type","PI");
        }
        else
            pControllerRequirements.put("type","P");

        designer.tuneController(pControllerRequirements,pController);
        yInfo("tuning results: %s",pController.toString().c_str());
        double Kp=pController.find("Kp").asDouble();
        double Ki=pController.find("Ki").asDouble();
        pid.scale=4.0;
        int scale=(int)pid.scale; int shift=1<<scale;
        double fwKp=floor(Kp*pid.encs_ratio*shift);
        double fwKi=floor(Ki*pid.encs_ratio*shift/1000.0);
        pid.Kp=yarp::math::sign(pid.Kp*fwKp)>0.0?fwKp:-fwKp;
        pid.Ki=yarp::math::sign(pid.Ki*fwKi)>0.0?fwKi:-fwKi;
        pid.Kd=0.0;
        yInfo("Kp (FW) = %g; Ki (FW) = %g; Kd (FW) = %g; shift factor = %d",pid.Kp,pid.Ki,pid.Kd,scale);

        Property pStictionEstimation;
        pStictionEstimation.put("max_time",60.0);
        pStictionEstimation.put("Kp",Kp);
        pStictionEstimation.put("Ki",0.0);
        pStictionEstimation.put("Kd",0.0);
        designer.startStictionEstimation(pStictionEstimation);

        yInfo("Estimating stiction for joint %d: max duration = %g seconds",
              i,pStictionEstimation.find("max_time").asDouble());

        t0=Time::now();
        while (!designer.isDone())
        {
            yInfo("elapsed %d [s]",(int)(Time::now()-t0));
            Time::delay(1.0);
            if (interrupting)
            {
                idlingCoupledJoints(i,false);
                return false;
            }
        }

        designer.getResults(pResults);
        pid.st_up=floor(pResults.find("stiction").asList()->get(0).asDouble());
        pid.st_down=floor(pResults.find("stiction").asList()->get(1).asDouble());
        yInfo("Stiction values: up = %g; down = %g",pid.st_up,pid.st_down);

        IControlMode2 *imod;
        IPositionControl *ipos;
        IEncoders *ienc;
        driver->view(imod);
        driver->view(ipos);
        driver->view(ienc);
        imod->setControlMode(i,VOCAB_CM_POSITION);
        ipos->setRefSpeed(i,50.0);
        ipos->positionMove(i,0.0);
        yInfo("Driving the joint back to rest... ");
        t0=Time::now();
        while (Time::now()-t0<5.0)
        {
            double enc;
            ienc->getEncoder(i,&enc);
            if (fabs(enc)<1.0)
                break;

            if (interrupting)
            {
                idlingCoupledJoints(i,false);
                return false;
            }

            Time::delay(0.2);
        }
        yInfo("done!");

        idlingCoupledJoints(i,false);
        return true;
    }

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

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