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;
}
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 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_click(GtkButton *button, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
IPositionControl *ipos = currentPart->pos;
IEncoders *iiencs = currentPart->iencs;
IAmplifierControl *iamp = currentPart->amp;
IPidControl *ipid = currentPart->pid;
IControlCalibration2 *ical = currentPart->cal;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
//fprintf(stderr, "Retrieving finder \n");
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "Retrieved finder: %p \n", fnd);
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_zero"));
//fprintf(stderr, "Finder retrieved %s\n", buffer2);
if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
{
//fprintf(stderr, "Home group was not empty \n");
bool ok = true;
Bottle xtmp;
xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double positionZero = xtmp.get(*joint+1).asDouble();
//fprintf(stderr, "%f\n", positionZero);
xtmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
//fprintf(stderr, "VALUE VEL is %d \n", fnd->findGroup(buffer2).find("VelocityZero").toString().c_str());
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double velocityZero = xtmp.get(*joint+1).asDouble();
//fprintf(stderr, "%f\n", velocityZero);
if(!ok)
dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group", buffer2, true);
else
{
ipos->setRefSpeed(*joint, velocityZero);
ipos->positionMove(*joint, positionZero);
}
}
else
{
// currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);
dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable", buffer2, true);
}
return;
}
void 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::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::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;
}
bool partMover::entry_update(partMover *currentPart)
{
GdkColor color_black;
GdkColor color_grey;
GdkColor color_yellow;
GdkColor color_green;
GdkColor color_green_blue;
GdkColor color_dark_green;
GdkColor color_red;
GdkColor color_fault_red;
GdkColor color_pink;
GdkColor color_indaco;
GdkColor color_white;
GdkColor color_blue;
color_pink.red=219*255;
color_pink.green=166*255;
color_pink.blue=171*255;
color_fault_red.red=255*255;
color_fault_red.green=10*255;
color_fault_red.blue=10*255;
color_black.red=10*255;
color_black.green=10*255;
color_black.blue=10*255;
color_red.red=255*255;
color_red.green=100*255;
color_red.blue=100*255;
color_grey.red=220*255;
color_grey.green=220*255;
color_grey.blue=220*255;
color_white.red=250*255;
color_white.green=250*255;
color_white.blue=250*255;
color_green.red=149*255;
color_green.green=221*255;
color_green.blue=186*255;
color_dark_green.red=(149-30)*255;
color_dark_green.green=(221-30)*255;
color_dark_green.blue=(186-30)*255;
color_blue.red=150*255;
color_blue.green=190*255;
color_blue.blue=255*255;
color_green_blue.red=(149+150)/2*255;
color_green_blue.green=(221+190)/2*255;
color_green_blue.blue=(186+255)/2*255;
color_indaco.red=220*255;
color_indaco.green=190*255;
color_indaco.blue=220*255;
color_yellow.red=249*255;
color_yellow.green=236*255;
color_yellow.blue=141*255;
GdkColor* pColor= &color_grey;
static int slowSwitcher = 0;
IControlMode *ictrl = currentPart->ctrlmode2;
IInteractionMode *iint = currentPart->iinteract;
IPositionControl *ipos = currentPart->pos;
IVelocityControl *ivel = currentPart->iVel;
IPositionDirect *iDir = currentPart->iDir;
IEncoders *iiencs = currentPart->iencs;
ITorqueControl *itrq = currentPart->trq;
IAmplifierControl *iamp = currentPart->amp;
GtkEntry * *pos_entry = (GtkEntry **) currentPart->currPosArray;
GtkEntry **trq_entry = (GtkEntry **) currentPart->currTrqArray;
GtkEntry **speed_entry = (GtkEntry **) currentPart->currSpeedArray;
GtkEntry **inEntry = (GtkEntry **) currentPart->inPosArray;
GtkWidget **colorback = (GtkWidget **) currentPart->frameColorBack;
GtkWidget **sliderAry = currentPart->sliderArray;
bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
char buffer[40] = {'i', 'n', 'i', 't'};
char frame_title [255];
double positions[MAX_NUMBER_OF_JOINTS];
double torques[MAX_NUMBER_OF_JOINTS];
double speeds[MAX_NUMBER_OF_JOINTS];
double max_torques[MAX_NUMBER_OF_JOINTS];
double min_torques[MAX_NUMBER_OF_JOINTS];
static int controlModes[MAX_NUMBER_OF_JOINTS];
static int controlModesOld[MAX_NUMBER_OF_JOINTS];
static yarp::dev::InteractionModeEnum interactionModes[MAX_NUMBER_OF_JOINTS];
static yarp::dev::InteractionModeEnum interactionModesOld[MAX_NUMBER_OF_JOINTS];
int k;
int NUMBER_OF_JOINTS=0;
bool done = false;
bool ret = false;
ipos->getAxes(&NUMBER_OF_JOINTS);
if (NUMBER_OF_JOINTS == 0)
{
fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" );
Time::delay(0.1);
pColor=&color_grey;
strcpy(frame_title,"DISCONNECTED");
for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++)
{
if (currentPart->framesArray[k]!=0)
{
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
}
}
return true;
}
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
max_torques[k]=0;
min_torques[k]=0;
torques[k]=0;
}
if (!iiencs->getEncoders(positions))
return true;
itrq->getTorques(torques);
iiencs->getEncoderSpeeds(speeds);
//update all joints positions
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
sprintf(buffer, "%.1f", positions[k]);
gtk_entry_set_text((GtkEntry*) pos_entry[k], buffer);
sprintf(buffer, "%.3f", torques[k]);
gtk_entry_set_text((GtkEntry*) trq_entry[k], buffer);
sprintf(buffer, "%.1f", speeds[k]);
gtk_entry_set_text((GtkEntry*) speed_entry[k], buffer);
}
//update all joint sliders
for (k = 0; k < NUMBER_OF_JOINTS; k++)
if(POS_UPDATE[k])
gtk_range_set_value((GtkRange*)sliderAry[k], positions[k]);
// *** update the checkMotionDone box section ***
// (only one at a time in order to save badwidth)
k = slowSwitcher%NUMBER_OF_JOINTS;
slowSwitcher++;
#if DEBUG_GUI
gtk_entry_set_text((GtkEntry*) inEntry[k], "off");
#else
ipos->checkMotionDone(k, &done);
if (!done)
gtk_entry_set_text((GtkEntry*) inEntry[k], " ");
else
gtk_entry_set_text((GtkEntry*) inEntry[k], "@");
#endif
// *** update the controlMode section ***
// the new icubinterface does not increase the bandwidth consumption
// ret = true; useless guys!
ret=ictrl->getControlModes(controlModes);
if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" );
ret=iint->getInteractionModes(interactionModes);
if (ret==false) fprintf(stderr,"iint->getInteractionlMode failed\n" );
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue;
controlModesOld[k]=controlModes[k];
sprintf(frame_title,"Joint %d ",k );
switch (controlModes[k])
{
case VOCAB_CM_IDLE:
pColor=&color_yellow;
strcat(frame_title," (IDLE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_POSITION:
pColor=&color_green;
strcat(frame_title," (POSITION)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_POSITION_DIRECT:
pColor=&color_dark_green;
strcat(frame_title," (POSITION_DIRECT)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"---");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_MIXED:
pColor=&color_green_blue;
strcat(frame_title," (MIXED_MODE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_VELOCITY:
pColor=&color_blue;
strcat(frame_title," (VELOCITY)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_TORQUE:
pColor=&color_pink;
strcat(frame_title," (TORQUE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_IMPEDANCE_POS:
pColor=&color_indaco;
strcat(frame_title," (IMPEDANCE POS)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_IMPEDANCE_VEL:
pColor=&color_indaco;
strcat(frame_title," (IMPEDANCE VEL)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_OPENLOOP:
pColor=&color_white;
strcat(frame_title," (OPENLOOP)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_HW_FAULT:
pColor=&color_fault_red;
strcat(frame_title," (HARDWARE_FAULT)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"---");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"---");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_CALIBRATING:
pColor=&color_grey;
strcat(frame_title," (CALIBRATING)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_CALIB_DONE:
pColor=&color_grey;
strcat(frame_title," (CALIB DONE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_NOT_CONFIGURED:
pColor=&color_grey;
strcat(frame_title," (NOT CONFIGURED)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_CONFIGURED:
pColor=&color_grey;
strcat(frame_title," (CONFIGURED)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
default:
case VOCAB_CM_UNKNOWN:
pColor=&color_grey;
strcat(frame_title," (UNKNOWN)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
}
}
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
if (currentPart->first_time==false && interactionModes[k] == interactionModesOld[k]) continue;
interactionModesOld[k]=interactionModes[k];
switch (interactionModes[k])
{
case VOCAB_IM_STIFF:
gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_green);
break;
case VOCAB_IM_COMPLIANT:
gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_fault_red);
break;
default:
case VOCAB_CM_UNKNOWN:
gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_white);
break;
}
}
currentPart->first_time =false;
return true;
}
int main(int argc, char *argv[])
{
Network yarp;
Property params;
params.fromCommand(argc, argv);
if (!params.check("robot"))
{
fprintf(stderr, "Please specify the name of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
return -1;
}
std::string robotName=params.find("robot").asString().c_str();
std::string remotePorts="/";
remotePorts+=robotName;
remotePorts+="/left_arm";
std::string localPorts="/test/client";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str()); //local port names
options.put("remote", remotePorts.c_str()); //where we connect to
// create a device
PolyDriver robotDevice(options);
if (!robotDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
IPositionControl *pos;
IEncoders *encs;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 0;
}
int nj=0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
int i;
for (i = 0; i < nj; i++) {
tmp[i] = 50.0;
}
pos->setRefAccelerations(tmp.data());
for (i = 0; i < nj; i++) {
tmp[i] = 4.0;
pos->setRefSpeed(i, tmp[i]);
}
//pos->setRefSpeeds(tmp.data()))
//fisrst zero all joints
//
command=0;
//now set the shoulder to some value
command[0]=-26.0;
command[1]=20.0;
command[2]=0.0;
command[3]=49.0;
command[4]=0.0;
pos->positionMove(command.data());
bool done=false;
while(!done) {
Time::delay(0.5);
pos->checkMotionDone(&done);
}
printf("\niCub @ HOME. Press any key...");
mygetch();
int times=0;
while(true)
{
times++;
if (times%2)
{
printf("\n\nSet pos1: ");
//command[0]=-50;
command[1]=64.0;
//command[2]=-10;
//command[3]=50;
//command[4]=0;
}
else
{
printf("\n\nSet pos2: ");
//command[0]=-20;
command[1]=20.0;
//command[2]=-10;
//command[3]=30;
//command[4]=0;
}
pos->positionMove(command.data());
printf("waiting");
bool done3=false;
while(!done3) {
Time::delay(1.0);
pos->checkMotionDone(&done3);
printf(".");
}
printf("ok!\n");
mygetch();
}
robotDevice.close();
return 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;
}
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;
}
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;
}
/*
* Load from file the current list of positions
*/
void partMover::load_from_file(char* filenameIn, 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;
GtkWidget *tree_view = currentPart->treeview;
int j, k, extensionLength, filenameLength;
char buffer[800];
char filenameExtension[800];
Property p;
bool fileExists = p.fromConfigFile(filenameIn);
strcpy(filenameExtension, ".pos");
strcat(filenameExtension, currentPart->partLabel);
extensionLength = strlen(filenameExtension);
filenameLength = strlen(filenameIn);
if ((filenameLength - extensionLength) > 0)
fileExists &= (strcmp(filenameIn +
(sizeof(char))*(filenameLength - extensionLength),
filenameExtension) == 0 );
else
fileExists=false;
if (fileExists)
{
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
for (j = 0; j < NUMBER_OF_STORED; j++)
{
sprintf(buffer, "POSITION%d", j);
Bottle& xtmp = p.findGroup(buffer).findGroup("jointPositions");
if (xtmp.size() == NUMBER_OF_JOINTS+1 && j < NUMBER_OF_STORED - 1)
{
for (k = 0; k < NUMBER_OF_JOINTS; k++)
STORED_POS_TMP[j][k] = xtmp.get(k+1).asDouble();
xtmp = p.findGroup(buffer).findGroup("jointVelocities");
for (k = 0; k < NUMBER_OF_JOINTS; k++)
STORED_VEL_TMP[j][k] = xtmp.get(k+1).asDouble();
xtmp = p.findGroup(buffer).findGroup("timing");
TIMING_TMP[j] = xtmp.get(1).asDouble();
SEQUENCE_TMP[j] = j;
}
else
{
for (k = 0; k < NUMBER_OF_JOINTS; k++)
STORED_POS_TMP[j][k] = 0.0;
SEQUENCE_TMP[j] = -1;
if(j==0)
{
dialog_message(GTK_MESSAGE_ERROR,
(char *) "Couldn't load a valid position file.",
(char *) "Check the format of the supplied file.", true);
return;
}
if(j == NUMBER_OF_STORED - 1 && xtmp.size() == NUMBER_OF_JOINTS+1)
{
dialog_message(GTK_MESSAGE_ERROR,
(char *) "Truncating the current sequence which is too long. You need to recompile with a greater value of NUMBER_OF_STORED",
(char *) "Unfortunately maximum sequence length is not set at runtime", true);
}
}
}
if(GTK_IS_TREE_VIEW (tree_view))
{
gtk_tree_view_set_model (GTK_TREE_VIEW (tree_view), refresh_position_list_model(currentPart));
gtk_widget_draw(GTK_WIDGET(tree_view), NULL);
}
}
else
dialog_message(GTK_MESSAGE_ERROR,
(char *) "Wrong format (check estensions) of the file associated with:",
currentPart->partLabel, true);
return;
}
/*
* Enable all PID and refresh position sliders
*/
void partMover::calib_all(GtkButton *button, partMover* currentPart)
{
//ask for confirmation
if (!dialog_question("Do you really want to recalibrate the whole part?"))
{
return;
}
IPositionControl *ipos = currentPart->pos;
IControlCalibration2 *ical = currentPart->cal;
int joint;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "opening file \n");
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_calib"));
if (!fnd->findGroup(buffer2).isNull())
{
bool ok = true;
Bottle p1 = fnd->findGroup(buffer2).findGroup("Calibration1");
ok = ok && (p1.size() == NUMBER_OF_JOINTS+1);
Bottle p2 = fnd->findGroup(buffer2).findGroup("Calibration2");
ok = ok && (p2.size() == NUMBER_OF_JOINTS+1);
Bottle p3 = fnd->findGroup(buffer2).findGroup("Calibration3");
ok = ok && (p3.size() == NUMBER_OF_JOINTS+1);
Bottle ty = fnd->findGroup(buffer2).findGroup("CalibrationType");
ok = ok && (ty.size() == NUMBER_OF_JOINTS+1);
if (ok)
{
for (joint=0; joint < NUMBER_OF_JOINTS; joint++)
{
double param1 = p1.get(joint+1).asDouble();
//fprintf(stderr, "%f ", param1);
double param2 = p2.get(joint+1).asDouble();
//fprintf(stderr, "%f ", param2);
double param3 = p3.get(joint+1).asDouble();
//fprintf(stderr, "%f ", param3);
unsigned char type = (unsigned char) ty.get(joint+1).asDouble();
//fprintf(stderr, "%d ", type);
ical->calibrate2(joint, type, param1, param2, param3);
}
}
else
dialog_message(GTK_MESSAGE_ERROR,(char *) "Check number of entries in the file", buffer1, true);
}
else
{
dialog_message(GTK_MESSAGE_ERROR,(char *) "No calib file found. Define a suitable file called:", buffer1, true);
}
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());
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());
}
bool partMover::entry_update(partMover *currentPart)
{
GdkColor color_grey;
GdkColor color_yellow;
GdkColor color_green;
GdkColor color_red;
GdkColor color_pink;
GdkColor color_indaco;
GdkColor color_white;
GdkColor color_blue;
color_pink.red=219*255;
color_pink.green=166*255;
color_pink.blue=171*255;
color_red.red=255*255;
color_red.green=100*255;
color_red.blue=100*255;
color_grey.red=220*255;
color_grey.green=220*255;
color_grey.blue=220*255;
color_white.red=250*255;
color_white.green=250*255;
color_white.blue=250*255;
color_green.red=149*255;
color_green.green=221*255;
color_green.blue=186*255;
color_blue.red=150*255;
color_blue.green=190*255;
color_blue.blue=255*255;
color_indaco.red=220*255;
color_indaco.green=190*255;
color_indaco.blue=220*255;
color_yellow.red=249*255;
color_yellow.green=236*255;
color_yellow.blue=141*255;
GdkColor* pColor= &color_grey;
static int slowSwitcher = 0;
IControlMode *ictrl = currentPart->ctrlmode;
IPositionControl *ipos = currentPart->pos;
IVelocityControl *ivel = currentPart->iVel;
IEncoders *iiencs = currentPart->iencs;
ITorqueControl *itrq = currentPart->trq;
IAmplifierControl *iamp = currentPart->amp;
GtkEntry * *pos_entry = (GtkEntry **) currentPart->currPosArray;
GtkEntry **trq_entry = (GtkEntry **) currentPart->currTrqArray;
GtkEntry **speed_entry = (GtkEntry **) currentPart->currSpeedArray;
GtkEntry **inEntry = (GtkEntry **) currentPart->inPosArray;
GtkWidget **colorback = (GtkWidget **) currentPart->frameColorBack;
GtkWidget **sliderAry = currentPart->sliderArray;
bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;
char buffer[40] = {'i', 'n', 'i', 't'};
char frame_title [255];
double positions[MAX_NUMBER_OF_JOINTS];
double torques[MAX_NUMBER_OF_JOINTS];
double speeds[MAX_NUMBER_OF_JOINTS];
double max_torques[MAX_NUMBER_OF_JOINTS];
double min_torques[MAX_NUMBER_OF_JOINTS];
static int controlModes[MAX_NUMBER_OF_JOINTS];
static int controlModesOld[MAX_NUMBER_OF_JOINTS];
int k;
int NUMBER_OF_JOINTS=0;
bool done = false;
bool ret = false;
ipos->getAxes(&NUMBER_OF_JOINTS);
if (NUMBER_OF_JOINTS == 0)
{
fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" );
Time::delay(0.1);
pColor=&color_grey;
strcpy(frame_title,"DISCONNECTED");
for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++)
{
if (currentPart->framesArray[k]!=0)
{
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
}
}
return true;
}
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
max_torques[k]=0;
min_torques[k]=0;
torques[k]=0;
}
if (!iiencs->getEncoders(positions))
return true;
itrq->getTorques(torques);
iiencs->getEncoderSpeeds(speeds);
//update all joints positions
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
sprintf(buffer, "%.1f", positions[k]);
gtk_entry_set_text((GtkEntry*) pos_entry[k], buffer);
sprintf(buffer, "%.3f", torques[k]);
gtk_entry_set_text((GtkEntry*) trq_entry[k], buffer);
sprintf(buffer, "%.1f", speeds[k]);
gtk_entry_set_text((GtkEntry*) speed_entry[k], buffer);
}
//update all joint sliders
for (k = 0; k < NUMBER_OF_JOINTS; k++)
if(POS_UPDATE[k])
gtk_range_set_value((GtkRange*)sliderAry[k], positions[k]);
// *** update the checkMotionDone box section ***
// (only one at a time in order to save badwidth)
k = slowSwitcher%NUMBER_OF_JOINTS;
slowSwitcher++;
#if DEBUG_GUI
gtk_entry_set_text((GtkEntry*) inEntry[k], "off");
#else
ipos->checkMotionDone(k, &done);
if (!done)
gtk_entry_set_text((GtkEntry*) inEntry[k], " ");
else
gtk_entry_set_text((GtkEntry*) inEntry[k], "@");
#endif
// *** update the controlMode section ***
// the new icubinterface does not increase the bandwidth consumption
// ret = true; useless guys!
ret=ictrl->getControlModes(controlModes);
if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" );
for (k = 0; k < NUMBER_OF_JOINTS; k++)
{
if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue;
controlModesOld[k]=controlModes[k];
sprintf(frame_title,"Joint %d ",k );
switch (controlModes[k])
{
case VOCAB_CM_IDLE:
pColor=&color_yellow;
strcat(frame_title," (IDLE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_POSITION:
pColor=&color_green;
strcat(frame_title," (POSITION)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_VELOCITY:
pColor=&color_blue;
strcat(frame_title," (VELOCITY)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_TORQUE:
pColor=&color_pink;
strcat(frame_title," (TORQUE)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:");
gtk_frame_set_label (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:");
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_IMPEDANCE_POS:
pColor=&color_indaco;
strcat(frame_title," (IMPEDANCE POS)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_IMPEDANCE_VEL:
pColor=&color_indaco;
strcat(frame_title," (IMPEDANCE VEL)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
case VOCAB_CM_OPENLOOP:
pColor=&color_white;
strcat(frame_title," (OPENLOOP)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
default:
case VOCAB_CM_UNKNOWN:
pColor=&color_grey;
//strcat(frame_title," (UNKNOWN)");
gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
break;
}
// pColor=&color_blue;
// gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
int curr_amp_status=0;
int amp_status[60]; //fix this!!!
for (int i=0; i<60; i++) amp_status[i]=0; //fix this!!!
iamp->getAmpStatus(amp_status); //fix this!!!
curr_amp_status=amp_status[k]; //fix this!!!
#if 0
if ((amp_status[k] & 0xFF)!=0)
{
//fprintf(stderr, "FAULT DETECTED: %x\n", curr_amp_status);
//pColor=&color_red;
//strcat(frame_title," (FAULT)");
//gtk_frame_set_label (GTK_FRAME(currentPart->framesArray[k]),frame_title);
//gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
}
#endif
}
currentPart->first_time =false;
return true;
}
void partMover::sequence_cycle_time(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);
//fprintf(stderr, "Getting the number of joitns %d\n", 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_time, currentPart);
fixed_time_move(STORED_POS_TMP[INV_SEQUENCE_TMP[0]],
TIMING_TMP[0],
currentPart);
//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);
}
//fprintf(stderr, "Iterator was initialized \n");
return;
}
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;
}
int main(int argc, char const *argv[]) {
Network yarp;
Node node("/icub_sim/state_publisher");
Property params;
params.fromCommand(argc, argv);
if (!params.check("robot"))
{
fprintf(stderr, "Please specify the name of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
return 1;
}
std::string robotName=params.find("robot").asString().c_str();
std::string remotePorts="/";
remotePorts+=robotName;
std::string remoteRArmPorts = remotePorts + "/right_arm";
std::string remoteHeadPorts = remotePorts + "/head";
std::string remoteTorsoPorts = remotePorts + "/torso";
std::string remoteLArmPorts = remotePorts + "/left_arm";
Property options_head;
options_head.put("device", "remote_controlboard");
options_head.put("local", "/head/client"); //local port names
options_head.put("remote", remoteHeadPorts.c_str());
// create a device
PolyDriver headDevice(options_head);
if (!headDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
Property options_torso;
options_torso.put("device", "remote_controlboard");
options_torso.put("local", "/torso/client"); //local port names
options_torso.put("remote", remoteTorsoPorts.c_str());
// create a device
PolyDriver torsoDevice(options_torso);
if (!torsoDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
Property options_right_arm;
options_right_arm.put("device", "remote_controlboard");
options_right_arm.put("local", "/right_arm/client"); //local port names
options_right_arm.put("remote", remoteRArmPorts.c_str());
// create a device
PolyDriver rArmDevice(options_right_arm);
if (!rArmDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
Property options_left_arm;
options_left_arm.put("device", "remote_controlboard");
options_left_arm.put("local", "/left_arm/client"); //local port names
options_left_arm.put("remote", remoteLArmPorts.c_str());
// create a device
PolyDriver lArmDevice(options_left_arm);
if (!lArmDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
IPositionControl *posHead;
IPositionControl *posTorso;
IPositionControl *posRArm;
IPositionControl *posLArm;
IEncoders *encsHead;
IEncoders *encsTorso;
IEncoders *encsRArm;
IEncoders *encsLArm;
torsoDevice.view(posTorso);
torsoDevice.view(encsTorso);
rArmDevice.view(posRArm);
rArmDevice.view(encsRArm);
lArmDevice.view(posLArm);
lArmDevice.view(encsLArm);
headDevice.view(posHead);
headDevice.view(encsHead);
int nj = 0;
posHead->getAxes(&nj);
Vector encodersHead;
encodersHead.resize(nj);
nj = 0;
posTorso->getAxes(&nj);
Vector encodersTorso;
encodersTorso.resize(nj);
nj = 0;
posRArm->getAxes(&nj);
Vector encodersRArm;
encodersRArm.resize(nj);
nj = 0;
posLArm->getAxes(&nj);
Vector encodersLArm;
encodersLArm.resize(nj);
yarp::os::Publisher<sensor_msgs_JointState> joint_pub;
if (!joint_pub.topic("/joint_states")) {
std::cerr<< "Failed to create publisher to /chatter\n";
return -1;
}
float degtorad = 0.0174532925;
sensor_msgs_JointState joint_states;
struct timespec currentTime;
joint_states.name.resize(70);
joint_states.position.resize(70);
joint_states.velocity.resize(70);
joint_states.name[0] ="j1";
joint_states.position[0] = 0.0;
joint_states.name[1] ="j2";
joint_states.position[1] = 0.0;
joint_states.name[2] ="j3";
joint_states.position[2] = 0.0;
joint_states.name[3] ="j4";
joint_states.position[3] = 0.0;
joint_states.name[4] ="j5";
joint_states.position[4] = 0.0;
joint_states.name[5] ="j6";
joint_states.position[5] = 0.0;
joint_states.name[6] ="j7";
joint_states.position[6] = 0.0;
joint_states.name[7] ="j8";
joint_states.position[7] = 0.0;
joint_states.name[8] ="j7s";
joint_states.position[8] = 0.0;
joint_states.name[9] ="j8s";
joint_states.position[9] = 0.0;
joint_states.name[10] ="raj1";
joint_states.position[10] = 0.0;
joint_states.name[11] ="raj2";
joint_states.position[11] = 0.0;
joint_states.name[12] ="raj3";
joint_states.position[12] = 0.0;
joint_states.name[13] ="raj4";
joint_states.position[13] = 0.0;
joint_states.name[14] ="raj5";
joint_states.position[14] = 0.0;
joint_states.name[15] ="raj6";
joint_states.position[15] = 0.0;
joint_states.name[16] ="laj1";
joint_states.position[16] = 0.0;
joint_states.name[17] ="laj2";
joint_states.position[17] = 0.0;
joint_states.name[18] ="laj3";
joint_states.position[18] = 0.0;
joint_states.name[19] ="laj4";
joint_states.position[19] = 0.0;
joint_states.name[20] ="laj5";
joint_states.position[20] = 0.0;
joint_states.name[21] ="laj6";
joint_states.position[21] = 0.0;
joint_states.name[22] ="rlaj1";
joint_states.position[22] = 0.0;
joint_states.name[23] ="rlaj2";
joint_states.position[23] = 0.0;
joint_states.name[24] ="rlaj3";
joint_states.position[24] = 0.0;
joint_states.name[25] ="rlaj4";
joint_states.position[25] = 0.0;
joint_states.name[26] ="rlaj5";
joint_states.position[26] = 0.0;
joint_states.name[27] ="rlaj6";
joint_states.position[27] = 0.0;
joint_states.name[28] ="llaj1";
joint_states.position[28] = 0.0;
joint_states.name[29] ="llaj2";
joint_states.position[29] = 0.0;
joint_states.name[30] ="llaj3";
joint_states.position[30] = 0.0;
joint_states.name[31] ="llaj4";
joint_states.position[31] = 0.0;
joint_states.name[32] ="llaj5";
joint_states.position[32] = 0.0;
joint_states.name[33] ="llaj6";
joint_states.position[33] = 0.0;
joint_states.name[34] ="right_wrist_yaw";
joint_states.position[34] = 0.0;
joint_states.name[35] ="tj2";
joint_states.position[35] = 0.0;
joint_states.name[36] ="tj4";
joint_states.position[36] = 0.0;
joint_states.name[37] ="tj5";
joint_states.position[37] = 0.0;
joint_states.name[38] ="tj6";
joint_states.position[38] = 0.0;
joint_states.name[39] ="ij3";
joint_states.position[39] = 0.0;
joint_states.name[40] ="ij4";
joint_states.position[40] = 0.0;
joint_states.name[41] ="ij5";
joint_states.position[41] = 0.0;
joint_states.name[42] ="mj3";
joint_states.position[42] = 0.0;
joint_states.name[43] ="mj4";
joint_states.position[43] = 0.0;
joint_states.name[44] ="mj5";
joint_states.position[44] = 0.0;
joint_states.name[45] ="rij3";
joint_states.position[45] = 0.0;
joint_states.name[46] ="rij4";
joint_states.position[46] = 0.0;
joint_states.name[47] ="rij5";
joint_states.position[47] = 0.0;
joint_states.name[48] ="raj4";
joint_states.position[48] = 0.0;
joint_states.name[49] ="raj5";
joint_states.position[49] = 0.0;
joint_states.name[50] ="lij3";
joint_states.position[50] = 0.0;
joint_states.name[51] ="lij4";
joint_states.position[51] = 0.0;
joint_states.name[52] ="lij5";
joint_states.position[52] = 0.0;
joint_states.name[53] ="left_wrist_yaw";
joint_states.position[53] = 0.0;
joint_states.name[54] ="ltj2";
joint_states.position[54] = 0.0;
joint_states.name[55] ="ltj4";
joint_states.position[55] = 0.0;
joint_states.name[56] ="ltj5";
joint_states.position[56] = 0.0;
joint_states.name[57] ="ltj6";
joint_states.position[57] = 0.0;
joint_states.name[58] ="laij3";
joint_states.position[58] = 0.0;
joint_states.name[59] ="laij4";
joint_states.position[59] = 0.0;
joint_states.name[60] ="laij5";
joint_states.position[60] = 0.0;
joint_states.name[61] ="lmj3";
joint_states.position[61] = 0.0;
joint_states.name[62] ="lmj4";
joint_states.position[62] = 0.0;
joint_states.name[63] ="lmj5";
joint_states.position[63] = 0.0;
joint_states.name[64] ="lrij3";
joint_states.position[64] = 0.0;
joint_states.name[65] ="lrij4";
joint_states.position[65] = 0.0;
joint_states.name[66] ="lrij5";
joint_states.position[66] = 0.0;
joint_states.name[67] ="llij3";
joint_states.position[67] = 0.0;
joint_states.name[68] ="llij4";
joint_states.position[68] = 0.0;
joint_states.name[69] ="llij5";
joint_states.position[69] = 0.0;
joint_pub.write(joint_states);
while (1) {
encsTorso->getEncoders(encodersTorso.data());
encsRArm->getEncoders(encodersRArm.data());
encsLArm->getEncoders(encodersLArm.data());
encsHead->getEncoders(encodersHead.data());
clock_gettime(CLOCK_REALTIME, ¤tTime);
joint_states.header.stamp.sec = currentTime.tv_sec;
joint_states.header.stamp.nsec = currentTime.tv_nsec;
// update torso positions
joint_states.position[0] = encodersTorso[2] * degtorad;
joint_states.position[1] = encodersTorso[1] * degtorad;
joint_states.position[2] = encodersTorso[0] * degtorad;
// update head positions
joint_states.position[3] = encodersHead[0] * degtorad;
joint_states.position[4] = encodersHead[1] * degtorad;
joint_states.position[5] = encodersHead[2] * degtorad;
// update right arm positions
joint_states.position[10] = encodersRArm[0] * degtorad;
joint_states.position[11] = encodersRArm[1] * degtorad;
joint_states.position[12] = encodersRArm[2] * degtorad;
joint_states.position[13] = encodersRArm[3] * degtorad;
joint_states.position[14] = encodersRArm[4] * degtorad;
joint_states.position[15] = encodersRArm[5] * degtorad;
// update left arm positions
joint_states.position[16] = encodersLArm[0] * degtorad;
joint_states.position[17] = encodersLArm[1] * degtorad;
joint_states.position[18] = encodersLArm[2] * degtorad;
joint_states.position[19] = encodersLArm[3] * degtorad;
joint_states.position[20] = encodersLArm[4] * degtorad;
joint_states.position[21] = encodersLArm[5] * degtorad;
joint_pub.write(joint_states);
Time::delay(0.01);
}
return 0;
}
void partMover::calib_click(GtkButton *button, gtkClassData* currentClassData)
{
//ask for confirmation
if (!dialog_question("Do you really want to recalibrate the joint?"))
{
return;
}
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);
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "opening file \n");
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_calib"));
if (!fnd->findGroup(buffer2).isNull())
{
bool ok = true;
Bottle xtmp;
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("CalibrationType");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
unsigned char type = (unsigned char) xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%d ", type);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration1");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param1 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f ", param1);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration2");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param2 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f ", param2);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration3");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param3 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f \n", param3);
if(!ok)
dialog_message(GTK_MESSAGE_ERROR,(char *)"Check number of calib entries in the group", buffer2, true);
else
ical->calibrate2(*joint, type, param1, param2, param3);
}
else
dialog_message(GTK_MESSAGE_ERROR,(char *)"The supplied file does not conatain a group named:", buffer2, true);
return;
}
int main(int argc, char *argv[])
{
Network yarp;
int maxSpeed;
Property params;
params.fromCommand(argc, argv);
if (!params.check("robot"))
{
fprintf(stderr, "Please specify the name of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
return -1;
}
if (!params.check("repetitions"))
{
fprintf(stderr, "Please specify number of repetitions\n");
fprintf(stderr, "--repetitions num (e.g. 10)\n");
return -1;
}
if (!params.check("speed"))
{
fprintf(stderr, "Speed not specified using default\n");
fprintf(stderr, "--speed num (e.g. 2)\n");
maxSpeed = 10.0;
}
else
{
maxSpeed = params.find("speed").asInt();
}
// sanity check on argument value
if(maxSpeed <0 || maxSpeed>50)
{
maxSpeed = 10;
}
std::string robotName=params.find("robot").asString().c_str();
std::string remotePorts="/";
remotePorts+=robotName;
remotePorts+="/head";
int numTimes = params.find("repetitions").asInt();
std::string localPorts="/headMovement_koroibot/client";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str()); //local port names
options.put("remote", remotePorts.c_str()); //where we connect to
// create a device
PolyDriver robotDevice(options);
if (!robotDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
IPositionControl *pos;
IEncoders *encs;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 0;
}
int nj=0;
pos->getAxes(&nj);
Vector encoders;
Vector command;
Vector tmp;
encoders.resize(nj);
tmp.resize(nj);
command.resize(nj);
int i;
for (i = 0; i < nj; i++) {
tmp[i] = 50.0;
}
pos->setRefAccelerations(tmp.data());
for (i = 0; i < nj; i++) {
tmp[i] = 10.0;
pos->setRefSpeed(i, tmp[i]);
}
//first read all encoders
printf("waiting for encoders");
while(!encs->getEncoders(encoders.data()))
{
Time::delay(0.1);
printf(".");
}
printf("\n;");
int ctr =0;
bool done = false;
while(ctr<numTimes)
{
printf("Starting headMovement\n");
command=encoders;
if(ctr%2 == 0)
command[2]=HEAD_YAW_MAX;
else
command[2]=HEAD_YAW_MIN;
done = false;
pos->positionMove(command.data());
while(!done)
{
pos->checkMotionDone(&done);
Time::delay(0.1);
}
ctr++;
}
command[2] = 0;
pos->positionMove(command.data());
while(!done)
{
pos->checkMotionDone(&done);
Time::delay(0.1);
}
robotDevice.close();
return 0;
}
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;
}
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;
}
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;
}
/*
* Save to file the current list of positions
*/
void partMover::save_to_file(char* filenameIn, partMover* currentPart)
{
char filename[800];
char filenamePart[800];
FILE* outputFile;
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 j, k;
char buffer[800];
int invSequence[NUMBER_OF_STORED];
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
//be sure that "." will be used in place of "," for decimals
char* loc = setlocale(LC_NUMERIC, NULL);
setlocale(LC_NUMERIC, "C");
int len = strlen (filenameIn);
int len2=0;
for (len2=0; len2<len; len2++)
if (filenameIn[len2]=='.') break;
strncpy(filename, filenameIn, len2);
filename[len2]=0;
strcat(filename, ".pos");
strcat(filename, currentPart->partLabel);
strcpy(filenamePart, filename);
fprintf(stderr, "Saving file %s \n", filenamePart);
outputFile = fopen(filenamePart,"w");
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)
{
sprintf(buffer, "[POSITION%d] \n", j);
fprintf(outputFile, "%s", buffer);
//Sequence positions
fprintf(outputFile, "jointPositions ");
for (k = 0; k < NUMBER_OF_JOINTS; k++)
fprintf(outputFile, "%.2f ", STORED_POS_TMP[ invSequence[j] ][k]);
fprintf(outputFile, "\n");
//Sequence Velocities
fprintf(outputFile, "jointVelocities ");
for (k = 0; k < NUMBER_OF_JOINTS; k++)
fprintf(outputFile, "%.2f ", STORED_VEL_TMP[ invSequence[j] ][k]);
fprintf(outputFile, "\n");
//Timing
fprintf(outputFile, "timing ");
fprintf(outputFile, "%.2f ", TIMING_TMP[invSequence[j]]);
fprintf(outputFile, "\n");
}
else
break;
fclose(outputFile);
fprintf(stderr, "File saved and closed\n");
//restore local "."/"," policy
setlocale(LC_NUMERIC, loc);
}
int main(int argc, char *argv[]){
Network yarp;
//Port<Bottle> armPlan;
//Port<Bottle> armPred;
Port armPlan;
Port armPred;
armPlan.open("/randArm/plan");
armPred.open("/randArm/pred");
bool fwCvOn = 0;
fwCvOn = Network::connect("/randArm/plan","/fwdConv:i");
fwCvOn *= Network::connect("/fwdConv:o","/randArm/pred");
if (!fwCvOn){
printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o");
return 1;
}
const gsl_rng_type *T;
gsl_rng *r;
gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc(T);
Property params;
params.fromCommand(argc,argv);
if (!params.check("robot")){
fprintf(stderr, "Please specify robot name");
fprintf(stderr, "e.g. --robot icub");
return -1;
}
std::string robotName = params.find("robot").asString().c_str();
std::string remotePorts = "/";
remotePorts += robotName;
remotePorts += "/";
if (params.check("side")){
remotePorts += params.find("side").asString().c_str();
}
else{
remotePorts += "left";
}
remotePorts += "_arm";
std::string localPorts = "/randArm/cmd";
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(nj);
for (int i = 0; i < nj; i++) {
tmp[i] = 25.0;
}
pos->setRefAccelerations(tmp.data());
for (int i = 0; i < nj; i++) {
tmp[i] = 5.0;
pos->setRefSpeed(i, tmp[i]);
}
command = 0;
//set the arm joints to "middle" values
command[0] = -45;
command[1] = 45;
command[2] = 0;
command[3] = 45;
pos->positionMove(command.data());
bool done = false;
while (!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
while (true){
tmp = command;
command[0] += 15*(2*gsl_rng_uniform(r)-1);
command[1] += 15*(2*gsl_rng_uniform(r)-1);
command[2] += 15*(2*gsl_rng_uniform(r)-1);
command[3] += 15*(2*gsl_rng_uniform(r)-1);
printf("%.1lf %.1lf %.1lf %.1lf\n", command[0], command[1], command[2], command[3]);
//above 0 doesn't seem to be safe for joint 0
if (command[0] > 0 || command[0] < -90){
command[0] = tmp[0];
}
if (command[1] > 160 || command[1] < -0){
command[1] = tmp[1];
}
if (command[2] > 100 || command[2] < -35){
command[2] = tmp[2];
}
if (command[3] > 100 || command[3] < 10){
command[3] = tmp[3];
}
//use fwd kin to find end effector position
Bottle plan, pred;
for (int i = 0; i < nj; i++){
plan.add(command[i]);
}
armPlan.write(plan);
armPred.read(pred);
for (int i = 0; i < 3; i++){
commandCart[i] = pred.get(i).asDouble();
}
double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
// safety radius back to 30 cm
if (rad > 0.3){
pos->positionMove(command.data());
done = false;
while(!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
}
else{
printf("Self collision detected!\n");
}
}
robotDevice.close();
gsl_rng_free(r);
return 0;
}
int main() {
Network yarp; // set up yarp
BufferedPort<Vector> targetPort;
targetPort.open("/tutorial/target/in");
Network::connect("/tutorial/target/out","/tutorial/target/in");
Property options;
options.put("device", "remote_controlboard");
options.put("local", "/tutorial/motor/client");
options.put("remote", "/icub/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);
int cnt=0;bool temp;
while (1) { // repeat forever
Vector *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)[i]);
}
printf("\n");
double x = (*target)[0];
double y = (*target)[1];
double conf = (*target)[2];
x -= 320/2;
y -= 240/2;
double vx = x*0.1;
double vy = -y*0.1;
// prepare command
/*if(cnt>2)
temp =false;
else if(cnt<-2)
temp =true;
if (temp ==true)
cnt++;
else if (temp==false)
cnt--;*/
for (int i=0; i<jnts; i++) {
setpoints[i] = 0.0;//(double)cnt;
}
//setpoints[0] = (double) cnt;
if (conf>0.5) {
setpoints[3] = vy;
setpoints[4] = vx;
} else {
setpoints[3] = 0;
setpoints[4] = 0;
}
for (int i=0; i<jnts; i++) {
printf("%f ", setpoints[i]);
}
printf("\n");
vel->velocityMove(setpoints.data());
//pos->positionMove(setpoints.data());
//getchar();
}
}
return 0;
}
