Ejemplo n.º 1
0
    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;
    }
Ejemplo n.º 2
0
bool partMover::sequence_iterator(partMover* currP)
{
  //fprintf(stderr, "calling sequence iterator \n");
  IPositionControl *ipos = currP->pos;
  IEncoders *iiencs = currP->iencs;
  IAmplifierControl *iamp = currP->amp;
  IPidControl *ipid = currP->pid;
  int *SEQUENCE_TMP = currP->SEQUENCE;
  double *TIMING_TMP = currP->TIMING;
  double **STORED_POS_TMP = currP->STORED_POS;
  double **STORED_VEL_TMP = currP->STORED_VEL;
  int *INV_SEQUENCE_TMP = currP->INV_SEQUENCE;
  GtkWidget **sliderAry = currP->sliderArray;
  GtkWidget **sliderVelAry = currP->sliderVelArray;
  GtkWidget *tree_view = currP->treeview;
  guint32* timeout_seqeunce_rate_tmp = currP->timeout_seqeunce_rate;
  guint* timeout_seqeunce_id_tmp = currP->timeout_seqeunce_id;
  int *SEQUENCE_ITERATOR_TMP = currP->SEQUENCE_ITERATOR;

  int j = (*SEQUENCE_ITERATOR_TMP);
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  if (INV_SEQUENCE_TMP[j]!=-1)
    {
      ipos->setRefSpeeds(STORED_VEL_TMP[INV_SEQUENCE_TMP[j]]);
      ipos->positionMove(STORED_POS_TMP[INV_SEQUENCE_TMP[j]]);
      for (int k =0; k < NUMBER_OF_JOINTS; k++)
	{
	  gtk_range_set_value ((GtkRange *) (sliderAry[k]),    STORED_POS_TMP[INV_SEQUENCE_TMP[j]][k]);
	  gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[INV_SEQUENCE_TMP[j]][k]);
	}
      (*SEQUENCE_ITERATOR_TMP)++;
      *timeout_seqeunce_rate_tmp = (unsigned int) (TIMING_TMP[j]*1000);
      gtk_timeout_remove(*timeout_seqeunce_id_tmp);
      *timeout_seqeunce_id_tmp = gtk_timeout_add(*timeout_seqeunce_rate_tmp, (GtkFunction) sequence_iterator, currP);
    }
  else
    {
      //restart the sequence if finished
      *SEQUENCE_ITERATOR_TMP = 0;
      j = 0;
      ipos->setRefSpeeds(STORED_VEL_TMP[INV_SEQUENCE_TMP[j]]);
      ipos->positionMove(STORED_POS_TMP[INV_SEQUENCE_TMP[j]]);
      for (int k =0; k < NUMBER_OF_JOINTS; k++)
	{
	  gtk_range_set_value ((GtkRange *) (sliderAry[k]),    STORED_POS_TMP[INV_SEQUENCE_TMP[j]][k]);
	  gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[INV_SEQUENCE_TMP[j]][k]);
	}
      (*SEQUENCE_ITERATOR_TMP)++;
      *timeout_seqeunce_rate_tmp = (unsigned int) (TIMING_TMP[j]*1000);
      gtk_timeout_remove(*timeout_seqeunce_id_tmp);
      *timeout_seqeunce_id_tmp = gtk_timeout_add(*timeout_seqeunce_rate_tmp, (GtkFunction) sequence_iterator, currP);
    }

  return false;
}
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;
}
Ejemplo n.º 4
0
void ReachManager::RobotPositionControl(string partName, const Vector &jointAngles)
{
    IPositionControl *pos;
    IEncoders *encs;

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

    Vector command;
    command.resize(nbJoints[partName]);

    //first zero all joints
    command=0;
    //now set the shoulder to some value
    command[0]=jointAngles[0] * 180 / 3.14;
    command[1]=jointAngles[1] * 180 / 3.14;
    command[2]=jointAngles[2] * 180 / 3.14;
    command[3]=jointAngles[3] * 180 / 3.14;
    command[4]=jointAngles[4] * 180 / 3.14;
    command[5]=jointAngles[5] * 180 / 3.14;
    command[6]=jointAngles[6] * 180 / 3.14;

    cout << "Moving to : " << command.toString() << endl;

    pos->positionMove(command.data());

    return;
}
Ejemplo n.º 5
0
void partMover::go_click(GtkButton *button, partMover *currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  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 NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);
		
  //get the current row index
  int i = get_index_selection(currentPart);
  if (i != -1)
    {
      if (TIMING_TMP[i]>0)
	{
	  ipos->setRefSpeeds(STORED_VEL_TMP[i]);
	  ipos->positionMove(STORED_POS_TMP[i]);
	  for (int k =0; k < NUMBER_OF_JOINTS; k++)
	    {
	      gtk_range_set_value ((GtkRange *) (sliderAry[k]),  STORED_POS_TMP[i][k]);
	      gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[i][k]);
	    }
	}
    }
		
  return;
}
Ejemplo n.º 6
0
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;
}
Ejemplo n.º 7
0
    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_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;
}
Ejemplo n.º 9
0
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;
}
Ejemplo n.º 10
0
    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;
    }
Ejemplo n.º 11
0
void partMover::sequence_click(GtkButton *button, partMover* currentPart)
{

  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  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 j;
	
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);
	
  int invSequence[NUMBER_OF_STORED];

  for (j = 0; j < NUMBER_OF_STORED; j++)
    invSequence[j] = -1;

  for (j = 0; j < NUMBER_OF_STORED; j++)
    {
      if (SEQUENCE_TMP[j]>-1 && (SEQUENCE_TMP[j]<NUMBER_OF_STORED))
	invSequence[SEQUENCE_TMP[j]] = j;
    }
  for (j = 0; j < NUMBER_OF_STORED; j++)
    if (invSequence[j]!=-1)
      {
	if (TIMING_TMP[invSequence[j]] > 0)
	  {
	    ipos->setRefSpeeds(STORED_VEL_TMP[invSequence[j]]);
	    ipos->positionMove(STORED_POS_TMP[invSequence[j]]);
	    for (int k =0; k < NUMBER_OF_JOINTS; k++)
	      {
		gtk_range_set_value ((GtkRange *) (sliderAry[k]),    STORED_POS_TMP[invSequence[j]][k]);
		gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[invSequence[j]][k]);
	      }
	    Time::delay(TIMING_TMP[invSequence[j]]);
	  }
      }
    else
      break;
  return;
}
Ejemplo n.º 12
0
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;
}
Ejemplo n.º 13
0
    // the motion-done callback
    virtual void gazeEventCallback()
    {
        Vector ang;
        igaze->getAngles(ang);

        fprintf(stdout,"Actual gaze configuration: (%s) [deg]\n",
                ang.toString(3,3).c_str());

        fprintf(stdout,"Moving the torso; check if the gaze is compensated ...\n");

        // move the torso yaw
        double val;
        ienc->getEncoder(0,&val);
        ipos->positionMove(0,val>0.0?-30.0:30.0);

        t4=t;
        
        // detach the callback
        igaze->unregisterEvent(*this);

        // switch state
        state=STATE_STILL;
    }
Ejemplo n.º 14
0
	virtual bool threadInit(){
		if(!handleParams()){
			return false;
		}

		armPlan = new Port;
		armPred = new Port;
		armLocJ = new BufferedPort<Vector>;
		headLoc = new BufferedPort<Vector>;

		armPlan->open("/babbleTrack/plan:o");
		armPred->open("/babbleTrack/pred:i");
		armLocJ->open("/babbleTrack/arm:o");
		headLoc->open("/babbleTrack/head:o");

		gsl_rng_env_setup();
		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;
	}
Ejemplo n.º 15
0
	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");
		}
	}
Ejemplo n.º 16
0
    bool tune(const int i)
    {
        PidData &pid=pids[i];

        Property pGeneral;
        pGeneral.put("joint",i);
        string sGeneral="(general ";
        sGeneral+=pGeneral.toString().c_str();
        sGeneral+=')';

        Bottle bGeneral,bPlantEstimation,bStictionEstimation;
        bGeneral.fromString(sGeneral.c_str());
        bPlantEstimation.fromString("(plant_estimation (Ts 0.01) (Q 1.0) (R 1.0) (P0 100000.0) (tau 1.0) (K 1.0) (max_pwm 800.0))");
        bStictionEstimation.fromString("(stiction_estimation (Ts 0.01) (T 2.0) (vel_thres 5.0) (e_thres 1.0) (gamma (10.0 10.0)) (stiction (0.0 0.0)))");

        Bottle bConf=bGeneral;
        bConf.append(bPlantEstimation);
        bConf.append(bStictionEstimation);

        Property pOptions(bConf.toString().c_str());
        OnlineCompensatorDesign designer;
        if (!designer.configure(*driver,pOptions))
        {
            yError("designer configuration failed!");
            return false;
        }

        idlingCoupledJoints(i,true);

        Property pPlantEstimation;
        pPlantEstimation.put("max_time",20.0);
        pPlantEstimation.put("switch_timeout",2.0);
        designer.startPlantEstimation(pPlantEstimation);

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

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

        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;
    }
Ejemplo n.º 17
0
int main(int argc, char *argv[]) 
{
    // just list the devices if no argument given
    if (argc <= 2) {
        printf("You can call %s like this:\n", argv[0]);
        printf("   %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
        printf("For example:\n");
        printf("   %s --robot icub --part any --remote /controlboard\n", argv[0]);
        printf("Here are devices listed for your system:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    // get command line options
    Property options;
    options.fromCommand(argc, argv);
    if (!options.check("robot") || !options.check("part")) {
        printf("Missing either --robot or --part options\n");
        return 0;
    }

    Network yarp;
	Time::turboBoost();
    
    char name[1024];
    Value& v = options.find("robot");
    Value& part = options.find("part");

    Value *val;
    if (!options.check("device", val)) {
        options.put("device", "remote_controlboard");
    }
    if (!options.check("local", val)) {
        sprintf(name, "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
        options.put("local", name);
    }
    if (!options.check("remote", val)) {
        sprintf(name, "/%s/%s", v.asString().c_str(), part.asString().c_str());
        options.put("remote", name);
    }

	fprintf(stderr, "%s", options.toString().c_str());

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

    IPositionControl *pos;
    IVelocityControl *vel;
    IEncoders *enc;
    IPidControl *pid;
    IAmplifierControl *amp;
    IControlLimits *lim;

    bool ok;
    ok = dd.view(pos);
    ok &= dd.view(vel);
    ok &= dd.view(enc);
    ok &= dd.view(pid);
    ok &= dd.view(amp);
    ok &= dd.view(lim);

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

    int jnts = 0;
    pos->getAxes(&jnts);
    printf("Working with %d axes\n", jnts);
    double *tmp = new double[jnts];
    assert (tmp != NULL);

    printf("Device active...\n");
    while (dd.isValid()) {
        char s[1024];
        
        printf("-> ");
        char c = 0;
        int i = 0;
        while (c != '\n') {
            c = (char)fgetc(stdin);
            s[i++] = c;
        }
        s[i-1] = s[i] = 0;

        Bottle p;
        p.fromString(s);
        printf("Bottle: %s\n", p.toString().c_str());

        switch(p.get(0).asVocab()) {        
        case VOCAB_HELP:
            printf("\n\n");
            printf("Available commands:\n\n");

            printf("type [get] and one of the following:\n");
            printf("[%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str());
            printf("[%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
            printf("[%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
            printf("[%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
            printf("[%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str());
            printf("[%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str());
            printf("[%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str());
            printf("[%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
            printf("[%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
            printf("[%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());

            printf("\n");

            printf("type [set] and one of the following:\n");
            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;

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

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

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

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

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

                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: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
                    pos->setRefSpeeds(tmp);
                }
                break;

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

                case VOCAB_STOP: {
                    int j = p.get(2).asInt();
                    printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str());
                    pos->stop(j);
                }
                break;

                case VOCAB_STOPS: {
                    printf("%s: stopping all axes %d\n", Vocab::decode(VOCAB_STOPS).c_str());
                    pos->stop();
                }
                break;

                case VOCAB_ENCODER: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref);
                    enc->setEncoder(j, ref);                    
                }
                break; 

                case VOCAB_ENCODERS: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str());
                    enc->setEncoders(tmp);
                }
                break;

                case VOCAB_PID: {
                    Pid pd;
                    int j = p.get(2).asInt();
                    Bottle *l = p.get(3).asList();
                    pd.kp = l->get(0).asDouble();
                    pd.kd = l->get(1).asDouble();
                    pd.ki = l->get(2).asDouble();
                    pd.max_int = l->get(3).asDouble();
                    pd.max_output = l->get(4).asDouble();
                    pd.offset = l->get(5).asDouble();
                    pd.scale = l->get(6).asDouble();
                    printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j);
                    pid->setPid(j, pd);
                }
                break;

                case VOCAB_DISABLE: {
                    int j = p.get(2).asInt();
                    printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j);
                    pid->disablePid(j);
                    amp->disableAmp(j);
                }
                break;

                case VOCAB_ENABLE: {
                    int j = p.get(2).asInt();
                    printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j);
                    amp->enableAmp(j);
                    pid->enablePid(j);
                }
                break;

                case VOCAB_LIMITS: {
                    int j = p.get(2).asInt();
                    printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j);
                    Bottle *l = p.get(3).asList();
                    lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble());
                }
                break;
            }
            break;
        } /* switch get(0) */

    } /* while () */

ApplicationCleanQuit:
    dd.close();
    delete[] tmp;

    return 0;
}
Ejemplo n.º 18
0
void partMover::sequence_cycle(GtkButton *button,partMover* currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  int *SEQUENCE_TMP = currentPart->SEQUENCE;
  double *TIMING_TMP = currentPart->TIMING;
  double **STORED_POS_TMP = currentPart->STORED_POS;
  double **STORED_VEL_TMP = currentPart->STORED_VEL;
  int *INV_SEQUENCE_TMP = currentPart->INV_SEQUENCE;
  GtkWidget **sliderAry = currentPart->sliderArray;
  GtkWidget **sliderVelAry = currentPart->sliderVelArray;
  GtkWidget *tree_view = currentPart->treeview;
  guint32* timeout_seqeunce_rate_tmp = currentPart->timeout_seqeunce_rate;
  guint* timeout_seqeunce_id_tmp = currentPart->timeout_seqeunce_id;
  int *SEQUENCE_ITERATOR_TMP = currentPart->SEQUENCE_ITERATOR;
	
  int j, k;
  *timeout_seqeunce_rate_tmp = (unsigned int) (TIMING_TMP[0]*1000);
	
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  for (j = 0; j < NUMBER_OF_STORED; j++)
    INV_SEQUENCE_TMP[j] = -1;

  for (j = 0; j < NUMBER_OF_STORED; j++)
    {
      if (SEQUENCE_TMP[j]>-1 && (SEQUENCE_TMP[j]<NUMBER_OF_STORED))
	{
	  INV_SEQUENCE_TMP[SEQUENCE_TMP[j]] = j;
	}
    }

  //if possible execute the first movement
  //SEQUENCE_ITERATOR = 0;
  if (INV_SEQUENCE_TMP[0]!=-1 && TIMING_TMP[0] >0 )
    {
      *timeout_seqeunce_id_tmp = gtk_timeout_add(*timeout_seqeunce_rate_tmp, (GtkFunction)sequence_iterator, currentPart);
      ipos->setRefSpeeds(STORED_VEL_TMP[INV_SEQUENCE_TMP[0]]);
      ipos->positionMove(STORED_POS_TMP[INV_SEQUENCE_TMP[0]]);
      for (k =0; k < NUMBER_OF_JOINTS; k++)
	{
	  gtk_range_set_value ((GtkRange *) (sliderAry[k]),    STORED_POS_TMP[INV_SEQUENCE_TMP[0]][k]);
	  gtk_range_set_value ((GtkRange *) (sliderVelAry[k]), STORED_VEL_TMP[INV_SEQUENCE_TMP[0]][k]);
	}
      //point the SEQUENCE ITERATOR to the next movement
      *SEQUENCE_ITERATOR_TMP = 1;

      //deactivate all buttons
      for (k =0; k < NUMBER_OF_JOINTS; k++)
	{
	  gtk_widget_set_sensitive(sliderVelAry[k], false);
	  gtk_widget_set_sensitive(sliderAry[k], false);
	}

      //fprintf(stderr, "Disabling bottons\n");
      if (currentPart->button1 != NULL)
	gtk_widget_set_sensitive(currentPart->button1, false);
      if (currentPart->button2 != NULL)
	gtk_widget_set_sensitive(currentPart->button2, false);
      if (currentPart->button3 != NULL)
	gtk_widget_set_sensitive(currentPart->button3, false);
      if (currentPart->button4 != NULL)
	gtk_widget_set_sensitive(currentPart->button4, false);
      if (currentPart->button5 != NULL)
	gtk_widget_set_sensitive(currentPart->button5, false);
      if (currentPart->button7 != NULL)
	gtk_widget_set_sensitive(currentPart->button7, false);
      if (currentPart->button8 != NULL)
	gtk_widget_set_sensitive(currentPart->button8, false);

    }
  return;
}
Ejemplo n.º 19
0
int main(int argc, char *argv[]){

	Network yarp;
	Port armPlan;
	Port armPred;
	BufferedPort<Vector> objAngles;
	BufferedPort<Vector> armOut;
	armPlan.open("/learnedReach/plan");
	armPred.open("/learnedReach/pred");
	objAngles.open("/learnedReach/loc:i");
	armOut.open("/learnedReach/arm:o");
	bool fwCvOn = 0;
	fwCvOn = Network::connect("/learnedReach/plan","/fwdConv:i");
	fwCvOn *= Network::connect("/fwdConv:o","/learnedReach/pred");
	if (!fwCvOn){
		printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n");
		return -1;
	}

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

	if (!params.check("robot")){
		fprintf(stderr, "Please specify robot name\n");
		fprintf(stderr, "e.g. --robot icub\n");
		return -1;
	}
	std::string robotName = params.find("robot").asString().c_str();
	std::string remotePorts = "/";
	remotePorts += robotName;
	remotePorts += "/";
	if (params.check("arm")){
		remotePorts += params.find("arm").asString().c_str();
	}
	else{
		remotePorts += "left";
	}
	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());
	}
Ejemplo n.º 20
0
int main(int argc, char *argv[])
{
	Network yarp;
	
	// IMPORTANT
	// TO BE ABLE TO FIND KASPAR Devices
    #ifdef USE_KASPAR_MOD
	yarp::dev::DriverCollection dev;
	#endif

	printf("SDLJSLKdjfklSJ\n");
	Property config;
	
	//config.fromCommand(argc, argv);
	//config.put("device", "KasparServos");
	config.fromConfigFile("D:/roboskin/coding/roboskin/src/modules/kasparServos/KasparRoboSkin.cfg");
	//Property options;

	//options.put("device", "fsrTouchSensor");
	//options.put("device", "remote_controlboard");
	PolyDriver dd;
	dd.open(config);
	
	/*CAUTION::::: device here should be remote_controlboard, rather than dynamixelAX12FtdiDriver*/
	//http://wikiri.upc.es/index.php/YARP_devices
	//http://eris.liralab.it/wiki/YARP_Motor_Control
	//http://eris.liralab.it/yarpdoc/d1/d88/classyarp_1_1dev_1_1RemoteControlBoard.html
	//PolyDriver dd("(device remote_controlboard) (local /client) (remote /controlboard)");
	if (!dd.isValid())
	{
		printf("Device not available.\n");
	//	Network::fini();
		return 1;
	}

	IPositionControl *pos;
	ITorqueControl *tor;
	IEncoders *enc;
	//yarp::os::Thread *th;
	if(!dd.view(pos))
    {
        printf("Problem of open pos device\n");
        return 0;
	}
pos->setRefSpeed(9, 10);
	std::cout << pos->positionMove(9, 150) << std::endl;
	/*
	// Test for SSC32 servos
	pos->setRefSpeed(6, 10);
	std::cout << pos->positionMove(6, 0) << std::endl;
	//Time::delay(1);
	//pos->setRefSpeed(3, 0.1);
	std::cout << pos->positionMove(6, 0) << std::endl;
	//Time::delay(1);
	//pos->setRefSpeed(4, 0.1);
	pos->setRefSpeed(5, 30);
	pos->setRefSpeed(4, 80);
	pos->setRefSpeed(3, 80);
	
	std::cout << pos->positionMove(5, 0) << std::endl;
	std::cout << pos->positionMove(4, 0) << std::endl;
		std::cout << pos->positionMove(3, -40) << std::endl;
		Time::delay(1);
		std::cout << pos->positionMove(3, 40) << std::endl;
	*/
	///*
		if(!dd.view(tor))
    {
        printf("Problem of open tor device\n");
        return 0;
	}

		tor->setRefTorque(9, 1023);

		Time::delay(2);
		tor->setRefTorque(9, 0);

		Time::delay(2);
		tor->setRefTorque(9, 1023);

		Time::delay(2);
		//tor->setRefTorque(9, 1023);
		/*
	if(!dd.view(enc))
    {
        printf("Problem of open enc device\n");
        return 0;
	}

    int jnts = 5;
	double t = 0;
	double p = 0;
	pos->getAxes(&jnts);
	std::cout << jnts << std::endl;
	pos->setRefSpeed(4, 23);
	std::cout << pos->positionMove(4, 90) << std::endl;
	pos->setRefSpeed(6, 20);
	std::cout << pos->relativeMove(6, 10) << std::endl;
	enc->getEncoder(4, &p);
	std::cout << "Position for joint 4 is --> " << p << std::endl;
//	printf("axes: %d\n", jnts);

	Time::delay(1);
	pos->setRefSpeed(2, 23);
	//yarp::os::Searchable c;

	//    std::cout << pos->positionMove(2, 15) << std::endl;
	//Time::delay(1);
	pos->setRefSpeed(4, 9);
	//yarp::os::Searchable c;

	std::cout << pos->positionMove(4, 100) << std::endl;
	yarp::os::Time::delay(1);
	//sleep(1);
	//tor->getTorque(4, &t);
	//std::cout << t << std::endl;

	std::cout << pos->relativeMove(4, 20) << std::endl;
	//sleep(1);
	//pos->relativeMove(4, 40);
    //yarp::os::Time::delay(1);
	//sleep(1);
	//sleep(1);
	for (int ii = 0; ii < 10; ii++)
	{
        //tor->getTorque(4, &t);
	//	enc->getEncoderSpeed(4, &t);
	//	std::cout << "Torque for join t 4 is --> " << t << std::endl;
		//std::cout << pos->positionMove(4, 90 + ii*5) << std::endl;

		//		sleep(1);
		yarp::os::Time::delay(0.11);
		enc->getEncoder(4, &p);
		std::cout << "Position for joint 4 is --> " << p << std::endl;
enc->getEncoder(1, &p);
		std::cout << "Position for joint 1 is --> " << p << std::endl;
		enc->getEncoder(2, &p);
		std::cout << "Position for joint 2 is --> " << p << std::endl;
		enc->getEncoder(3, &p);
		std::cout << "Position for joint 3 is --> " << p << std::endl;
	}
	yarp::os::Time::delay(1);
//	std::cout << pos->positionMove(4, 90) << std::endl;

	    std::cout << pos->relativeMove(4, -20) << std::endl;

	//    std::cout << pos->positionMove(2, 30) << std::endl;
	//    std::cout << pos->positionMove(2, 50) << std::endl;

	Time::delay(1);
*/
	dd.close();
    
	return 0;
}
Ejemplo n.º 21
0
void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint,
                                          const double min, const double max)
{
    printMessage(1,"calibrating finger %s ...\n",finger.getName().c_str());
    double margin=0.1*(max-min);
    double _min=min+margin;
    double _max=max-margin;
    double tol_min=5.0;
    double tol_max=5.0;
    double *val=&_min;
    double *tol=&tol_min;
    double timeout=2.0*(_max-_min)/finger.getCalibVel();

    mutex.lock();
    IControlMode2    *imod; driver.view(imod);
    IEncoders        *ienc; driver.view(ienc);
    IPositionControl *ipos; driver.view(ipos);
    mutex.unlock();

    // workaround
    if ((finger.getName()=="ring") || (finger.getName()=="little"))
    {
        _min=30.0;
        _max=180.0;
        tol_min=20.0;
        tol_max=50.0;
    }

    Property reset("(reset)");
    Property feed("(feed)");
    Property train("(train)");

    finger.calibrate(reset);

    mutex.lock();
    imod->setControlMode(joint,VOCAB_CM_POSITION);
    ipos->setRefSpeed(joint,finger.getCalibVel());
    mutex.unlock();

    for (int i=0; i<5; i++)
    {
        mutex.lock();
        ipos->positionMove(joint,*val);
        mutex.unlock();

        bool done=false;
        double fbOld=1e9;
        double t0=Time::now();
        while (!done)
        {
            Time::delay(0.01);

            mutex.lock();
            double fb; ienc->getEncoder(joint,&fb);
            mutex.unlock();

            if (fabs(fb-fbOld)>0.5)
            {
                printMessage(2,"feeding finger %s\n",finger.getName().c_str());
                finger.calibrate(feed);
            }

            done=(fabs(*val-fb)<*tol)||(Time::now()-t0>timeout);
            fbOld=fb;
        }

        if (val==&_min)
        {
            val=&_max;
            tol=&tol_max;
        }
        else
        {
            val=&_min;
            tol=&tol_min;
        }
    }

    printMessage(1,"training finger %s ...\n",finger.getName().c_str());    
    finger.calibrate(train);
    printMessage(1,"finger %s trained!\n",finger.getName().c_str());
}
Ejemplo n.º 22
0
bool SpringyFingersModel::calibrate(const Property &options)
{
    if (configured)
    {
        IControlMode2    *imod; driver.view(imod);
        IControlLimits   *ilim; driver.view(ilim);
        IEncoders        *ienc; driver.view(ienc);
        IPositionControl *ipos; driver.view(ipos);

        int nAxes; ienc->getAxes(&nAxes);
        Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);

        printMessage(1,"steering the hand to a suitable starting configuration\n");
        for (int j=7; j<nAxes; j++)
        {
            imod->setControlMode(j,VOCAB_CM_POSITION);
            ilim->getLimits(j,&qmin[j],&qmax[j]);            

            ipos->getRefAcceleration(j,&acc[j]);
            ipos->getRefSpeed(j,&vel[j]);
            
            ipos->setRefAcceleration(j,1e9);
            ipos->setRefSpeed(j,60.0);
            ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]);   // thumb in opposition
        }

        printMessage(1,"proceeding with the calibration\n");
        Property &opt=const_cast<Property&>(options);
        string tag=opt.check("finger",Value("all")).asString().c_str();
        if (tag=="thumb")
        {
            calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
        }
        else if (tag=="index")
        {
            calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
        }
        else if (tag=="middle")
        {
            calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
        }
        else if (tag=="ring")
        {
            calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
        }
        else if (tag=="little")
        {
            calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
        }
        else if ((tag=="all") || (tag=="all_serial"))
        {
            calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
            calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
            calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
            calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
            calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
        }
        else if (tag=="all_parallel")
        {
            CalibThread thr[5];
            thr[0].setInfo(this,fingers[0],10,qmin[10],qmax[10]);
            thr[1].setInfo(this,fingers[1],12,qmin[12],qmax[12]);
            thr[2].setInfo(this,fingers[2],14,qmin[14],qmax[14]);
            thr[3].setInfo(this,fingers[3],15,qmin[15],qmax[15]);
            thr[4].setInfo(this,fingers[4],15,qmin[15],qmax[15]);

            thr[0].start(); thr[1].start(); thr[2].start();
            thr[3].start(); thr[4].start();

            bool done=false;
            while (!done)
            {
                done=true;
                for (int i=0; i<5; i++)
                {
                    done&=thr[i].isDone();
                    if (thr[i].isDone() && thr[i].isRunning())
                        thr[i].stop();
                }

                Time::delay(0.1);
            }
        }
        else
        {
            printMessage(1,"unknown finger request %s\n",tag.c_str());
            return false;
        }

        for (int j=7; j<nAxes; j++)
        {
            ipos->setRefAcceleration(j,acc[j]);
            ipos->setRefSpeed(j,vel[j]);
        }

        return true;
    }
    else
        return false;
}
Ejemplo n.º 23
0
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;
}
Ejemplo n.º 24
0
    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];
        }

        // don't blend together the two "for"
        // since we have to enforce the modes on the whole
        // prior to commanding the joints
        std::vector<int>    joints;
        std::vector<int>    modes;
        std::vector<double> speeds;
        std::vector<double> positions;

        for (size_t i=0; i<disp.length(); i++)
        {
            joints.push_back(offset+i);
            speeds.push_back(disp[i]/time);
            modes.push_back(VOCAB_CM_POSITION);
            positions.push_back(x->getCmd()[i]);
        }

        mode->setControlModes(disp.length(), joints.data(), modes.data());
        yarp::os::Time::delay(0.01);  // give time to update control modes value
        mode->getControlModes(disp.length(), joints.data(), modes.data());
        for (size_t i=0; i<disp.length(); i++)
        {
            if(modes[i] != VOCAB_CM_POSITION)
            {
                yError() << "Joint " << i << " not in position mode";
            }
        }
        pos->setRefSpeeds(disp.length(), joints.data(), speeds.data());
        pos->positionMove(disp.length(), joints.data(), positions.data());

        cout << "Script port: " << x->getCmd().toString() << endl;
    }
Ejemplo n.º 25
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();

			}
		}
	}
Ejemplo n.º 26
0
int main()
{
	Network::init();
    srand(time(NULL));
    
    Property options;
    options.put("robot", "icub"); // typically from the command line.
    options.put("device", "remote_controlboard");

    Value& robotname = options.find("robot");
    string s("/");
    s += robotname.asString();
    s += "/right_arm/babbling";
    options.put("local", s.c_str());
    s.clear();
    s += "/";
    s += robotname.asString();
    s += "/right_arm";
    options.put("remote", s.c_str());
    

    PolyDriver dd(options);
    if (!dd.isValid()) {
        cout << "Device not available.  Here are the known devices:\n"<< endl;
        cout << Drivers::factory().toString().c_str() << endl;;
        Network::fini();
        return 0;
    }

    IPositionControl *pos;
    IVelocityControl *vel;
    IEncoders *enc;
    IPidControl *pid;
    IAmplifierControl *amp;
    IControlLimits *lim;

    bool ok;
    ok = dd.view(pos);
    ok &= dd.view(vel);
    ok &= dd.view(enc);
    ok &= dd.view(pid);
    ok &= dd.view(amp);
    ok &= dd.view(lim);

	if (!ok) {
		cout << "Device not able to acquire views" << endl;
		Network::fini();
		dd.close();
		return 0;
	}


     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;
}
Ejemplo n.º 27
0
    bool configure(ResourceFinder &rf)
    {
        std::string robotname = rf.check("robot",yarp::os::Value("icubSim")).asString();
        Property options;
        options.put("robot", robotname); // typically from the command line.
        options.put("device", "remote_controlboard");

        string s("/");
        s += robotname;
        s += "/right_arm/keyBabbling";
        options.put("local", s.c_str());
        s.clear();
        s += "/";
        s += robotname;
        s += "/right_arm";
        options.put("remote", s.c_str());

        if (!dd.open(options)) {
            cout << "Device not available.  Here are the known devices:\n"<< endl;
            cout << Drivers::factory().toString().c_str() << endl;;
            return false;
        }
        
        portStreamer.open("/keyboardBabbling/stream:o");
        portFromCart.open("/keyboardBabbling/stream:i");
        portRpc.open("/keyboarBabbling/rpc");
        attach(portRpc);

        Property optionCart("(device cartesiancontrollerclient)");
        optionCart.put("remote", "/" + robotname +"/cartesianController/right_arm");
        optionCart.put("local","/keyboardBabbling/cartesian_client/right_arm");
        if (!driverCart.open(optionCart))
            return false;

        bool ok;
        ok = dd.view(pos);
        ok &= dd.view(vel);
        ok &= dd.view(pid);
        ok &= dd.view(amp);
        ok &= dd.view(armCtrlModeUsed);
        ok &= dd.view(armImpUsed);
        ok &= driverCart.view(armCart);
        ok &= dd.view(ilimRight);

        iCubFinger* fingerRight = new iCubFinger("right_index");
        deque<IControlLimits*> limRight;
        limRight.push_back(ilimRight);
        fingerRight->alignJointsBounds(limRight);

        if (!ok) {
            cout << "Device not able to acquire views" << endl;
            dd.close();
            return false;
        }

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

        vector<bool> mask;
        mask.resize(jnts);
        mask[0] = false;
        mask[1] = false;
        mask[2] = false;
        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;
    }
Ejemplo n.º 28
0
int main(int argc, char *argv[]) 
{
    // just list the devices if no argument given
    if (argc <= 2) {
        printf("You can call %s like this:\n", argv[0]);
        printf("   %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
        printf("For example:\n");
        printf("   %s --robot icub --local /talkto/james --remote /controlboard/rpc\n", argv[0]);
        printf("Here are devices listed for your system:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    // get command line options
    Property options;
    options.fromCommand(argc, argv);
    if (!options.check("robot") || !options.check("part")) {
        printf("Missing either --robot or --part options\n");
        return 0;
    }

    Network::init();
	Time::turboBoost();
    
    std::string name;
    Value& v = options.find("robot");
    Value& part = options.find("part");

    Value *val;
    if (!options.check("device", val)) {
        options.put("device", "remote_controlboard");
    }
    if (!options.check("local", val)) {
		name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str())+"/simpleclient";
        //sprintf(&name[0], "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
        options.put("local", name.c_str());
    }
    if (!options.check("remote", val)) {
        name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str());    
		//sprintf(&name[0], "/%s/%s", v.asString().c_str(), part.asString().c_str());
        options.put("remote", name.c_str());
    }

	fprintf(stderr, "%s", options.toString().c_str());

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

    IPositionControl *pos;
    IPositionDirect  *posDir;
    IVelocityControl *vel;
    IEncoders *enc;
    IPidControl *pid;
    IAmplifierControl *amp;
    IControlLimits *lim;
//    IControlMode *icm;
    IControlMode2 *iMode2;
    ITorqueControl *itorque;
    IOpenLoopControl *iopenloop;
	IImpedanceControl *iimp;
    IInteractionMode *iInteract;

    bool ok;
    ok = dd.view(pos);
    ok &= dd.view(vel);
    ok &= dd.view(enc);
    ok &= dd.view(pid);
    ok &= dd.view(amp);
    ok &= dd.view(lim);
//    ok &= dd.view(icm);
    ok &= dd.view(itorque);
    ok &= dd.view(iopenloop);
	ok &= dd.view(iimp);
    ok &= dd.view(posDir);
    ok &= dd.view(iMode2);
    ok &= dd.view(iInteract);

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

    pos->getAxes(&jnts);
    printf("Working with %d axes\n", jnts);
    double *tmp = new double[jnts];

    printf("Device active...\n");
    while (dd.isValid()) {
        std::string s;
        s.resize(1024);
        
        printf("-> ");
        char c = 0;
        int i = 0;
        while (c != '\n') {
            c = (char)fgetc(stdin);
            s[i++] = c;
        }
        s[i-1] = s[i] = 0;

        Bottle p;
        Bottle response;
        bool ok=false;
        bool rec=false;
        p.fromString(s.c_str());
        printf("Bottle: %s\n", p.toString().c_str());

        switch(p.get(0).asVocab()) {      
        case VOCAB_HELP:
            printf("\n\n");
            printf("Available commands:\n");
			printf("-------------------\n\n");

            printf("IOpenLoop:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IOPENLOOP).c_str());
            printf("	[set] [%s] <int> <float>\n",
                    Vocab::decode(VOCAB_OUTPUT).c_str());
            printf("	[get] [%s] <int>\n",
                    Vocab::decode(VOCAB_OUTPUT).c_str());
            printf("	[get] [%s]\n\n",
                    Vocab::decode(VOCAB_OUTPUTS).c_str());

            printf("IControlMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_ICONTROLMODE).c_str());
            printf("	[set] [%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s][%s]|[%s]\n",
                    Vocab::decode(VOCAB_CM_POSITION).c_str(),
                    Vocab::decode(VOCAB_CM_POSITION_DIRECT).c_str(),
                    Vocab::decode(VOCAB_CM_VELOCITY).c_str(),
                    Vocab::decode(VOCAB_CM_MIXED).c_str(),
                    Vocab::decode(VOCAB_CM_TORQUE).c_str(),
                    Vocab::decode(VOCAB_CM_OPENLOOP).c_str(),
                    Vocab::decode(VOCAB_CM_IDLE).c_str(),
                    Vocab::decode(VOCAB_CM_FORCE_IDLE).c_str(),
                    Vocab::decode(VOCAB_CM_IMPEDANCE_POS).c_str(),
                    Vocab::decode(VOCAB_CM_IMPEDANCE_VEL).c_str());
            
            printf("	[get] [%s] <int>\n\n",
                Vocab::decode(VOCAB_CM_CONTROL_MODE).c_str());

            printf("ITorqueControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_TORQUE).c_str());
            printf("	[get] [%s] <int> to read the measured torque for a single axis\n",                  Vocab::decode(VOCAB_TRQ).c_str());
            printf("	[get] [%s]  to read the measured torque for all axes\n",                      Vocab::decode(VOCAB_TRQS).c_str());
            printf("	[set] [%s] <int> <float> to set the reference torque for a single axis\n",          Vocab::decode(VOCAB_REF).c_str());
            printf("	[set] [%s] <float list> to set the reference torque for all axes\n",        Vocab::decode(VOCAB_REFS).c_str());
            printf("	[get] [%s] <int> to read the reference torque for a single axis\n",                  Vocab::decode(VOCAB_REF).c_str());
            printf("	[get] [%s] to read the reference torque for all axes\n\n",                      Vocab::decode(VOCAB_REFS).c_str());

			printf("IImpedanceControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IMPEDANCE).c_str());
            printf("	[set] [%s] <int> <float> <float> \n", 
                Vocab::decode(VOCAB_IMP_PARAM).c_str());
            printf("	[set] [%s] <int> <float>\n\n", 
                Vocab::decode(VOCAB_IMP_OFFSET).c_str());

            printf("	[get] [%s] <int>\n", 
                Vocab::decode(VOCAB_IMP_PARAM).c_str());
            printf("	[get] [%s] <int>\n\n", 
                Vocab::decode(VOCAB_IMP_OFFSET).c_str());

            printf("IInteractionMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_INTERFACE_INTERACTION_MODE).c_str());
            printf("	[set] [%s]|[%s] <int>\n",
                    Vocab::decode(VOCAB_IM_STIFF).c_str(),
                    Vocab::decode(VOCAB_IM_COMPLIANT).c_str());

            printf("	[get] [%s] <int>\n",
                Vocab::decode(VOCAB_INTERACTION_MODE).c_str());
            printf("	[get] [%s] \n\n",
                Vocab::decode(VOCAB_INTERACTION_MODES).c_str());

			printf("Standard Interfaces:\n");
            printf("type [get] and one of the following:\n");
            printf("	[%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str());
            printf("	[%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
            printf("	[%s] to read the PID values for all axes\n", Vocab::decode(VOCAB_PIDS).c_str());
            printf("	[%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
            printf("	[%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
            printf("	[%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str());
            printf("	[%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str());
            printf("	[%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str());
			printf("	[%s] <int> to read the reference position for a single axis\n", Vocab::decode(VOCAB_REFERENCE).c_str());
            printf("	[%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
			printf("	[%s] <int> to read the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
            printf("	[%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
			printf("	[%s] <int> to read the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
            printf("	[%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());

            printf("\n");

            printf("type [set] and one of the following:\n");
            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");
			printf("NOTES: - A list is a sequence of numbers in parenthesis, e.g. (10 2 1 10)\n");
			printf("       - Pids are expressed as a list of 7 numbers, type get pid <int> to see an example\n");
            printf("\n");
            break;

        case VOCAB_QUIT:
            goto ApplicationCleanQuit;
            break;

        case VOCAB_ICONTROLMODE:
            {
                handleControlModeMsg(iMode2, p, response, &rec, &ok);
                printf("%s\n", response.toString().c_str());
                break;
            }

        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);
                          c.addDouble(p[i].kd);
                          c.addDouble(p[i].ki);
                          c.addDouble(p[i].max_int);
                          c.addDouble(p[i].max_output);
                          c.addDouble(p[i].offset);
                          c.addDouble(p[i].scale);
                        }
                    printf("%s\n", b.toString().c_str());
                    delete[] p;
                }
                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;

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

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

                case VOCAB_OUTPUT: {
                    int j = p.get(2).asInt();
                    double v;
                    iopenloop->getOutput(j, &v);
                    printf("%s: ", Vocab::decode(VOCAB_OUTPUT).c_str());
                    printf("%.2f ", v);
                    printf("\n");
                }
                break;

				case VOCAB_REFERENCE: {
					double ref_pos;
					int j = p.get(2).asInt();
                    pid->getReference(j,&ref_pos);
                    printf ("%s: (", Vocab::decode(VOCAB_REFERENCE).c_str());
                    printf ("%.2f ", ref_pos);
                    printf (")\n");                    
                }
                break;

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

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

				case VOCAB_REF_SPEED: {
					double ref_speed;
					int j = p.get(2).asInt();
                    pos->getRefSpeed(j,&ref_speed);
                    printf ("%s: (", Vocab::decode(VOCAB_REF_SPEED).c_str());
                    printf ("%.2f ", ref_speed);
                    printf (")\n");                    
                }
                break;

				case VOCAB_REF_ACCELERATION: {
					double ref_acc;
					int j = p.get(2).asInt();
                    pos->getRefAcceleration(j,&ref_acc);
                    printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
                    printf ("%.2f ", ref_acc);
                    printf (")\n");                    
                }
                break;

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

                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: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
                    pos->setRefSpeeds(tmp);
                }
                break;

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

                case VOCAB_STOP: {
                    int j = p.get(2).asInt();
                    printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str(), j);
                    pos->stop(j);
                }
                break;

                case VOCAB_STOPS: {
                    printf("%s: stopping all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
                    pos->stop();
                }
                break;

                case VOCAB_ENCODER: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref);
                    enc->setEncoder(j, ref);                    
                }
                break; 

                case VOCAB_ENCODERS: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str());
                    enc->setEncoders(tmp);
                }
                break;

                case VOCAB_PID: {
                    Pid pd;
                    int j = p.get(2).asInt();
                    Bottle *l = p.get(3).asList();
                    if (l==0)
                        {
                            printf("Check you specify a 7 elements list, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
                        }
                    else
                        {
                            int elems=l->size();
                            if (elems>=3)
                                {
                                    pd.kp = l->get(0).asDouble();
                                    pd.kd = l->get(1).asDouble();
                                    pd.ki = l->get(2).asDouble();
                                    if (elems>=7)
                                        {
                                            pd.max_int = l->get(3).asDouble();
                                            pd.max_output = l->get(4).asDouble();
                                            pd.offset = l->get(5).asDouble();
                                            pd.scale = l->get(6).asDouble();
                                        }
                                    printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j);
                                    pid->setPid(j, pd);
                                }
                            else
                                {
                                    printf("Error, check you specify at least 7 elements, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
                                }
                        }
                }
                break;

                case VOCAB_DISABLE: {
                    int j = p.get(2).asInt();
                    printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j);
                    pid->disablePid(j);
                    amp->disableAmp(j);
                }
                break;

                case VOCAB_ENABLE: {
                    int j = p.get(2).asInt();
                    printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j);
                    amp->enableAmp(j);
                    pid->enablePid(j);
                }
                break;

                case VOCAB_LIMITS: {
                    int j = p.get(2).asInt();
                    printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j);
                    Bottle *l = p.get(3).asList();
                    lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble());
                }
                break;

                case VOCAB_OUTPUT: {
                    int j=p.get(2).asInt();
                    double v=p.get(3).asDouble();
                    iopenloop->setRefOutput(j,v);
                    printf("%s: setting output for axis %d to %f\n", Vocab::decode(VOCAB_OUTPUT).c_str(), j, v);            
                }
                break;
            }
            break;
        } /* switch get(0) */

    } /* while () */

ApplicationCleanQuit:
    dd.close();
    delete[] tmp;

    Network::fini();
    return 0;
}
Ejemplo n.º 29
0
int main() {
    Network yarp; // set up yarp
    BufferedPort<Bottle> targetPort;
    targetPort.open("/mover/target/in");
    Network::connect("/objectDetector/target","/mover/target/in");
    Network::connect("/tracker/target:o","/mover/target/in");
    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", "/mover/motor/client");
    options.put("remote", "/icubSim/head");
    PolyDriver robotHead(options);

    if (!robotHead.isValid()) 
    {
        printf("Cannot connect to robot head\n");
        return 1;
    }
    
    IPositionControl *pos;
    IVelocityControl *vel;
    IEncoders *enc;
    robotHead.view(pos);
    robotHead.view(vel);
    robotHead.view(enc);
    if (pos==NULL || vel==NULL || enc==NULL) 
    {
        printf("Cannot get interface to robot head\n");
        robotHead.close();
        return 1;
    }
    int jnts = 0;
    pos->getAxes(&jnts);
    Vector setpoints;
    setpoints.resize(jnts);
	for (int i=0; i<jnts; i++) 
            {
                setpoints[i] = 0.0;
            }
	pos->positionMove(setpoints.data());

    while (1) 
    { // repeat forever
        Bottle *target = targetPort.read(); // read a target
        
        if (target!=NULL) 
        { // check we actually got something
            printf("We got a vector containing");
            for (int i=0; i<target->size(); i++) 
            {
                printf(" %g", target->get(i).asDouble());
            }
            printf("\n");
            double x = target->get(0).asDouble();
            double y = target->get(1).asDouble();
            double conf = target->get(2).asDouble();
            x -= 320/2;
            y -= 240/2;
            double vx = x*0.1;
            double vy = -y*0.1;
            /* prepare command */
            for (int i=0; i<jnts; i++) 
            {
                setpoints[i] = 0;
            }
            if (conf>0.5) 
            {
                setpoints[0] = vy*5;
                setpoints[1] = 0.0;
                setpoints[2] = -vx*7;
                setpoints[3] = vy;
                setpoints[4] = vx;
                vel->velocityMove(setpoints.data());
            } 
            else 
            {
                setpoints[0] = 0.0;
                setpoints[1] = 0.0;
                setpoints[2] = 0.0;
                setpoints[3] = 0;
                setpoints[4] = 0;
                pos->positionMove(setpoints.data());
            }
            
        }
    }
    return 0;
}
Ejemplo n.º 30
0
int main(int argc, char *argv[]) {

    time_t timev;

    printf("WARNING: requires a running instance of RaveBot (i.e. testRaveBot or cartesianServer)\n");
    Network yarp;
    if (!Network::checkNetwork()) {
        printf("Please start a yarp name server first\n");
        return(-1);
    }
    Property options;
    options.put("device","remote_controlboard");
    options.put("remote","/teo/leftArm");
    options.put("local","/local");
    PolyDriver dd(options);
    if(!dd.isValid()) {
      printf("RaveBot device not available.\n");
	  dd.close();
      Network::fini();
      return 1;
    }

    IPositionControl *pos;
    bool ok = dd.view(pos);
    if (!ok) {
        printf("[warning] Problems acquiring robot interface\n");
        return false;
    } else printf("[success] testAsibot acquired robot interface\n");
    pos->setPositionMode();

    for(b=0; b<20; b++){
        a=0;
        if (a==0) {
            printf("MOVE TO POSITION 01\n");
            pos->positionMove(0, -30);
            pos->positionMove(1, 40);
            pos->positionMove(2, 0);
            pos->positionMove(3, -70);
            pos->positionMove(4, -40);
            pos->positionMove(5, 10);
            pos->positionMove(6, 0);

            Time::delay(5);
            a=1;
        }
        
        if (a==1) {
            printf("MOVE TO POSITION 02\n");
            pos->positionMove(0, -10);
            pos->positionMove(1, 30);
            pos->positionMove(2, 0);
            pos->positionMove(3, -90);
            pos->positionMove(4, -30);
            pos->positionMove(5, 10);
            pos->positionMove(6, 0);

            Time::delay(5);
            a=2;
        }
        
        if (a==2) {
            printf("MOVE TO POSITION 03\n");
            pos->positionMove(0, -30);
            pos->positionMove(1, -10);
            pos->positionMove(2, 0);
            pos->positionMove(3, -70);
            pos->positionMove(4, 10);
            pos->positionMove(5, 10);
            pos->positionMove(6, 0);

            Time::delay(5);
            a=0;
        }
    }


  /*  b=5;
    a=0;
    if(a==0&&b>=0){
    printf("test positionMove(1,-35)\n");
    pos->positionMove(1, b);
    b--;
    } */

    printf("Delaying 5 seconds...\n");
    Time::delay(5);

 /*   IEncoders *enc;
    ok = dd.view(enc);

    IVelocityControl *vel;
    ok = dd.view(vel);
    vel->setVelocityMode();
    printf("test velocityMove(0,10)\n");
    vel->velocityMove(0,10);

    printf("Delaying 5 seconds...\n");
    Time::delay(5); */

    return 0;
}