C++ IPositionControl类代码示例

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

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

void testMotor(PolyDriver& driver) {
    IPositionControl *pos;
    if (driver.view(pos)) {
        int ct = 0;
        pos->getAxes(&ct);
        printf("  number of axes is: %d\n", ct);
    } else {
        printf("  could not find IPositionControl interface\n");
    }
}

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

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

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


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

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

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

void partMover::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;
}

bool partMover::sequence_iterator_time(partMover* currP)
{
  //fprintf(stderr, "calling sequence iterator time \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)
    {
      fixed_time_move(STORED_POS_TMP[INV_SEQUENCE_TMP[j]],
			    TIMING_TMP[j],
			    currP);

      (*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_time, currP);
    }
  else
    {
      //restart the sequence if finished
      *SEQUENCE_ITERATOR_TMP = 0;
      j = 0;
      fixed_time_move(STORED_POS_TMP[INV_SEQUENCE_TMP[j]],
			    TIMING_TMP[j],
			    currP);
      

      (*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_time, currP);
    }

  return false;
}

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

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

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

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

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

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

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

void partMover::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;
}

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

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

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

    IPositionControl *pos;
    IEncoders *encs;

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

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


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

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

void partMover::sequence_stop(GtkButton *button,partMover* currP)
{
  //fprintf(stderr, "calling sequence time stop\n");
  guint* timeout_seqeunce_id_tmp = currP->timeout_seqeunce_id;
  GtkWidget **sliderAry = currP->sliderArray;
  GtkWidget **sliderVelAry = currP->sliderVelArray;
  IPositionControl *ipos = currP->pos;

  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);

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

  //fprintf(stderr, "Enabling bottons...");
  if (currP->button1 != NULL)
    //fprintf(stderr, "botton is sensitive...");
    gtk_widget_set_sensitive(currP->button1, true);
    //fprintf(stderr, "disabled...");     
  if (currP->button2 != NULL)
    gtk_widget_set_sensitive(currP->button2, true);
  if (currP->button3 != NULL)
    gtk_widget_set_sensitive(currP->button3, true);
  if (currP->button4 != NULL)
    gtk_widget_set_sensitive(currP->button4, true);
  if (currP->button5 != NULL)
    gtk_widget_set_sensitive(currP->button5, true);
  if (currP->button6 != NULL)
    gtk_widget_set_sensitive(currP->button6, true);
  if (currP->button7 != NULL)
    gtk_widget_set_sensitive(currP->button7, true);
  if (currP->button8 != NULL)
    gtk_widget_set_sensitive(currP->button8, true);
  //fprintf(stderr, "done!\n");

  gtk_timeout_remove(*timeout_seqeunce_id_tmp);
  return;
}

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

void partMover::idle_all(GtkButton *button, partMover* currentPart)
{
  IPositionControl *ipos = currentPart->pos;
  IEncoders *iiencs = currentPart->iencs;
  IAmplifierControl *iamp = currentPart->amp;
  IPidControl *ipid = currentPart->pid;
  GtkWidget **sliderAry = currentPart->sliderArray;
	 
  double posJoint;
  int joint;
  int NUMBER_OF_JOINTS;
  ipos->getAxes(&NUMBER_OF_JOINTS);
  
  for (joint=0; joint < NUMBER_OF_JOINTS; joint++)
  {
    iiencs->getEncoder(joint, &posJoint);
    iamp->disableAmp(joint);
    ipid->disablePid(joint);
    gtk_range_set_value ((GtkRange *) (sliderAry[joint]), posJoint);
  }
  return;
}

void partMover::sequence_time(GtkButton *button, partMover* currentPart)
{

  IPositionControl *ipos = currentPart->pos;
  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 invSequence[NUMBER_OF_STORED];
  int NUMBER_OF_JOINTS;
  int j;
	
  ipos->getAxes(&NUMBER_OF_JOINTS);	


  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)
	  { 	    
	    fixed_time_move(STORED_POS_TMP[invSequence[j]],
			    TIMING_TMP[invSequence[j]],
			    currentPart);
	    Time::delay(TIMING_TMP[invSequence[j]]);
	  }
      }
    else
      break;
  return;
}

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