Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead,
const double eyeTiltMin, const double eyeTiltMax)
{
IEncoders *encs;
IControlLimits *lims;
double min, max;
int nJointsTorso=1;
if (drvTorso!=NULL)
{
drvTorso->view(encs);
drvTorso->view(lims);
encs->getAxes(&nJointsTorso);
for (int i=0; i<nJointsTorso; i++)
{
lims->getLimits(i,&min,&max);
(*chain)[nJointsTorso-1-i].setMin(CTRL_DEG2RAD*min); // reversed order
(*chain)[nJointsTorso-1-i].setMax(CTRL_DEG2RAD*max);
}
}
drvHead->view(encs);
drvHead->view(lims);
int nJointsHead;
encs->getAxes(&nJointsHead);
Matrix lim(nJointsHead,2);
for (int i=0; i<nJointsHead; i++)
{
lims->getLimits(i,&min,&max);
// limit eye's tilt due to eyelids
if (i==2)
{
min=std::max(min,eyeTiltMin);
max=std::min(max,eyeTiltMax);
}
lim(i,0)=CTRL_DEG2RAD*min;
lim(i,1)=CTRL_DEG2RAD*max;
// just one eye's got only 5 dofs
if (i<nJointsHead-1)
{
(*chain)[nJointsTorso+i].setMin(lim(i,0));
(*chain)[nJointsTorso+i].setMax(lim(i,1));
}
}
return lim;
}
void _send(const ActionItem *x)
{
if (!connected)
{
cerr<<"Error: not connected to control board skipping"<<endl;
return;
}
int size=x->getCmd().size();
int offset=x->getOffset();
double time=x->getTime();
int nJoints=0;
enc->getAxes(&nJoints);
if ((offset+size)>nJoints)
{
cerr<<"Error: detected possible overflow, skipping"<<endl;
cerr<<"For debug --> joints: "<<nJoints<< " off: "<<offset<<" cmd length: "<<size<<endl;
return;
}
Vector disp(size);
if (time==0)
{
return;
}
for (size_t i=0; i<disp.length(); i++)
{
double q;
if (!enc->getEncoder(offset+i,&q))
{
cerr<<"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
return;
}
disp[i]=x->getCmd()[i]-q;
if (disp[i]<0.0)
disp[i]=-disp[i];
}
for (size_t i=0; i<disp.length(); i++)
{
pos->setRefSpeed(offset+i,disp[i]/time);
pos->positionMove(offset+i,x->getCmd()[i]);
}
cout << "Script port: " << const_cast<Vector &>(x->getCmd()).toString() << endl;
}
bool threadInit()
{
//initialize here variables
printf("ControlThread:starting\n");
Property options;
options.put("device", "remote_controlboard");
options.put("local", "/local/head");
//substitute icubSim with icub for use with the real robot
options.put("remote", "/icubSim/head");
dd.open(options);
dd.view(iencs);
dd.view(ivel);
if ( (!iencs) || (!ivel) )
return false;
int joints;
iencs->getAxes(&joints);
encoders.resize(joints);
commands.resize(joints);
commands=10000;
ivel->setRefAccelerations(commands.data());
count=0;
return true;
}
void partMover::run_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;
GtkWidget **sliderAry = currentPart->sliderArray;
double posJoint;
while (!iiencs->getEncoder(*joint, &posJoint))
Time::delay(0.001);
iamp->enableAmp(*joint);
ipid->enablePid(*joint);
gtk_range_set_value ((GtkRange *) (sliderAry[*joint]), posJoint);
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 run()
{
//do the work
iencs->getEncoders(encoders.data());
count++;
if (count%2)
commands=5;
else
commands=-5;
ivel->velocityMove(commands.data());
printf(".");
}
// the motion-done callback
virtual void gazeEventCallback()
{
Vector ang;
igaze->getAngles(ang);
fprintf(stdout,"Actual gaze configuration: (%s) [deg]\n",
ang.toString(3,3).c_str());
fprintf(stdout,"Moving the torso; check if the gaze is compensated ...\n");
// move the torso yaw
double val;
ienc->getEncoder(0,&val);
ipos->positionMove(0,val>0.0?-30.0:30.0);
t4=t;
// detach the callback
igaze->unregisterEvent(*this);
// switch state
state=STATE_STILL;
}
bool updateModule()
{
if (calibrate)
{
Property options;
options.put("finger",fingerName.c_str());
model->calibrate(options);
calibrate=false;
ipos->setRefAcceleration(joint,1e9);
if ((fingerName=="ring")||(fingerName=="little"))
ipos->setRefSpeed(joint,60.0);
else
ipos->setRefSpeed(joint,30.0);
ipos->positionMove(joint,*val);
}
else
{
if (Node *finger=model->getNode(fingerName))
{
Value data; finger->getSensorsData(data);
Value out; finger->getOutput(out);
fprintf(stdout,"%s sensors data = %s; output = %s\n",
finger->getName().c_str(),data.toString().c_str(),out.toString().c_str());
}
double fb; ienc->getEncoder(joint,&fb);
if (fabs(*val-fb)<5.0)
{
val==&min?val=&max:val=&min;
ipos->positionMove(joint,*val);
}
}
return true;
}
bool updateModule()
{
LockGuard guard(mutex);
if (Vector *in=dataIn.read(false))
Controller_U_reference=(*in)[0];
ienc->getEncoder(joint,&Controller_U_plant_output);
// Step the model
double t0=Time::now();
// Step the model
Controller_step(Controller_M, Controller_U_reference,
Controller_U_compensator_state, Controller_U_plant_output,
&Controller_Y_controller_output,
&Controller_Y_controller_reference,
&Controller_Y_plant_reference, &Controller_Y_error_statistics,
&Controller_Y_enable_compensation);
double t1=Time::now();
ivel->velocityMove(joint,Controller_Y_controller_output);
Vector &out=dataOut.prepare();
out.resize(7);
out[0]=Controller_U_reference;
out[1]=Controller_Y_plant_reference;
out[2]=Controller_U_plant_output;
out[3]=Controller_Y_controller_reference;
out[4]=Controller_Y_controller_output;
out[5]=Controller_Y_error_statistics;
out[6]=Controller_Y_enable_compensation;
dataOut.write();
yInfo("time elapsed = %g [us]",(t1-t0)*1e6);
return true;
}
void _send(const ActionItem *x)
{
if (!connected)
{
cerr<<"Error: not connected to control board skipping"<<endl;
return;
}
int size=x->getCmd().size();
int offset=x->getOffset();
double time=x->getTime();
int nJoints=0;
enc->getAxes(&nJoints);
if ((offset+size)>nJoints)
{
cerr<<"Error: detected possible overflow, skipping"<<endl;
cerr<<"For debug --> joints: "<<nJoints<< " off: "<<offset<<" cmd length: "<<size<<endl;
return;
}
Vector disp(size);
if (time==0)
{
return;
}
for (size_t i=0; i<disp.length(); i++)
{
double q;
if (!enc->getEncoder(offset+i,&q))
{
cerr<<"Error: encoders timed out, cannot rely on encoder feedback, aborted"<<endl;
return;
}
disp[i]=x->getCmd()[i]-q;
if (disp[i]<0.0)
disp[i]=-disp[i];
}
// don't blend together the two "for"
// since we have to enforce the modes on the whole
// prior to commanding the joints
std::vector<int> joints;
std::vector<int> modes;
std::vector<double> speeds;
std::vector<double> positions;
for (size_t i=0; i<disp.length(); i++)
{
joints.push_back(offset+i);
speeds.push_back(disp[i]/time);
modes.push_back(VOCAB_CM_POSITION);
positions.push_back(x->getCmd()[i]);
}
mode->setControlModes(disp.length(), joints.data(), modes.data());
yarp::os::Time::delay(0.01); // give time to update control modes value
mode->getControlModes(disp.length(), joints.data(), modes.data());
for (size_t i=0; i<disp.length(); i++)
{
if(modes[i] != VOCAB_CM_POSITION)
{
yError() << "Joint " << i << " not in position mode";
}
}
pos->setRefSpeeds(disp.length(), joints.data(), speeds.data());
pos->positionMove(disp.length(), joints.data(), positions.data());
cout << "Script port: " << x->getCmd().toString() << endl;
}
virtual void run(){
tmp = command;
(*command)[0] = -60*(gsl_rng_uniform(r));
(*command)[1] = 100*(gsl_rng_uniform(r));
(*command)[2] = -35 + 95*(gsl_rng_uniform(r));
(*command)[3] = 10 + 90*(gsl_rng_uniform(r));
printf("%.1lf %.1lf %.1lf %.1lf\n", (*command)[0], (*command)[1], (*command)[2], (*command)[3]);
//above 0 doesn't seem to be safe for joint 0
if ((*command)[0] > 0 || (*command)[0] < -60){
(*command)[0] = (*tmp)[0];
}
if ((*command)[1] > 100 || (*command)[1] < -0){
(*command)[1] = (*tmp)[1];
}
if ((*command)[2] > 60 || (*command)[2] < -35){
(*command)[2] = (*tmp)[2];
}
if ((*command)[3] > 100 || (*command)[3] < 10){
(*command)[3] = (*tmp)[3];
}
//use fwd kin to find end effector position
Bottle plan, pred;
for (int i = 0; i < nj; i++){
plan.add((*command)[i]);
}
armPlan->write(plan);
armPred->read(pred);
Vector commandCart(3);
for (int i = 0; i < 3; i++){
commandCart[i] = pred.get(i).asDouble();
}
printf("Cartesian safety check\n");
double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
// safety radius back to 30 cm
if (rad > 0.3){
pos->positionMove(command->data());
bool done = false;
while(!done){
pos->checkMotionDone(&done);
Time::delay(0.1);
}
printf("Moved arm to new location\n");
Vector &armJ = armLocJ->prepare();
Vector encoders(nj);
enc->getEncoders(encoders.data());
armJ = encoders;
Vector noisyArm(3);
for(int i = 0; i < 3; i++){
//noisyArm[i] = commandCart[i] + 0.01*(2*gsl_rng_uniform(r)-1);
//sanity check
noisyArm[i] = commandCart[i] + 0.005*(2*gsl_rng_uniform(r)-1);
}
//insert here:
//read off peak saliences
//fixate there
//calculate cartesian value, compare to real cart. value of arm
printf("Looking at arm\n");
igaze->lookAtFixationPoint(noisyArm);
done = false;
while(!done){
igaze->checkMotionDone(&done);
Time::delay(0.5);
}
//igaze->waitMotionDone(0.1,30);
printf("Saw arm\n");
Vector &headAng = headLoc->prepare();
igaze->getAngles(headAng);
Bottle tStamp;
tStamp.clear();
tStamp.add(Time::now());
headLoc->setEnvelope(tStamp);
headLoc->write();
armLocJ->write();
headLoc->unprepare();
armLocJ->unprepare();
}
else{
printf("Self collision detected!\n");
}
}
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;
}
Controller::Controller(PolyDriver *_drvTorso, PolyDriver *_drvHead, exchangeData *_commData,
const bool _neckPosCtrlOn, const double _neckTime, const double _eyesTime,
const double _minAbsVel, const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), neckPosCtrlOn(_neckPosCtrlOn), neckTime(_neckTime),
eyesTime(_eyesTime), minAbsVel(_minAbsVel), period(_period),
Ts(_period/1000.0), printAccTime(0.0)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
// release links
neck->releaseLink(0); eyeL->releaseLink(0); eyeR->releaseLink(0);
neck->releaseLink(1); eyeL->releaseLink(1); eyeR->releaseLink(1);
neck->releaseLink(2); eyeL->releaseLink(2); eyeR->releaseLink(2);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
drvHead->view(modHead);
drvHead->view(posHead);
drvHead->view(velHead);
// if requested check if position control is available
if (neckPosCtrlOn)
{
neckPosCtrlOn=drvHead->view(posNeck);
yInfo("### neck control - requested POSITION mode: IPositionDirect [%s] => %s mode selected",
neckPosCtrlOn?"available":"not available",neckPosCtrlOn?"POSITION":"VELOCITY");
}
else
yInfo("### neck control - requested VELOCITY mode => VELOCITY mode selected");
// joints bounds alignment
lim=alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltMin,commData->eyeTiltMax);
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
// exclude acceleration constraints by fixing
// thresholds at high values
Vector a_robHead(nJointsHead,1e9);
velHead->setRefAccelerations(a_robHead.data());
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// find minimum allowed vergence
startupMinVer=lim(nJointsHead-1,0);
findMinimumAllowedVergence();
// reinforce vergence min bound
lim(nJointsHead-1,0)=commData->get_minAllowedVergence();
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
fbNeck=fbHead.subVector(0,2);
fbEyes=fbHead.subVector(3,5);
qdNeck.resize(3,0.0); qdEyes.resize(3,0.0);
vNeck.resize(3,0.0); vEyes.resize(3,0.0);
// Set the task execution time
setTeyes(eyesTime);
setTneck(neckTime);
mjCtrlNeck=new minJerkVelCtrlForIdealPlant(Ts,fbNeck.length());
mjCtrlEyes=new minJerkVelCtrlForIdealPlant(Ts,fbEyes.length());
IntState=new Integrator(Ts,fbHead,lim);
IntPlan=new Integrator(Ts,fbNeck,lim.submatrix(0,2,0,1));
v.resize(nJointsHead,0.0);
neckJoints.resize(3);
eyesJoints.resize(3);
neckJoints[0]=0;
neckJoints[1]=1;
neckJoints[2]=2;
eyesJoints[0]=3;
eyesJoints[1]=4;
eyesJoints[2]=5;
qd=fbHead;
q0deg=CTRL_RAD2DEG*qd;
qddeg=q0deg;
vdeg =CTRL_RAD2DEG*v;
port_xd=NULL;
ctrlActiveRisingEdgeTime=0.0;
saccadeStartTime=0.0;
unplugCtrlEyes=false;
ctrlInhibited=false;
}
EyePinvRefGen::EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead,
ExchangeData *_commData, Controller *_ctrl,
const Vector &_counterRotGain, const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), ctrl(_ctrl), period(_period),
Ts(_period/1000.0), counterRotGain(_counterRotGain)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
imu=new iCubInertialSensor(commData->head_version>1.0?"v2":"v1");
// remove constraints on the links: logging purpose
imu->setAllConstraints(false);
// block neck dofs
eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
// joints bounds alignment
lim=alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltLim);
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// just eye part is required
lim=lim.submatrix(3,5,0,1);
// reinforce vergence min bound
lim(2,0)=commData->minAllowedVergence;
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
// save original eyes tilt and pan bounds
orig_eye_tilt_min=(*chainEyeL)[nJointsTorso+3].getMin();
orig_eye_tilt_max=(*chainEyeL)[nJointsTorso+3].getMax();
orig_eye_pan_min=(*chainEyeL)[nJointsTorso+4].getMin();
orig_eye_pan_max=(*chainEyeL)[nJointsTorso+4].getMax();
orig_lim=lim;
// Instantiate integrator
qd.resize(3);
qd[0]=fbHead[3];
qd[1]=fbHead[4];
qd[2]=fbHead[5];
I=new Integrator(Ts,qd,lim);
fp.resize(3,0.0);
eyesJ.resize(3,3);
eyesJ.zero();
genOn=false;
saccadeUnderWayOld=false;
}
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;
}
virtual void run() {
while (isStopping() != true) {
/* poll the click ports containers to see if we have left/right ready to go */
bfL.lock(); bfR.lock();
if (bfL.size() == 2 && bfR.size() == 2) {
printf("got a hit!\n");
/* if they are, raise the flag that action is beginning, save current joint configuration */
Bottle susmsg;
susmsg.addInt(1);
susPort->write(susmsg);
//get the current joint configuration for the torso, head, and arm
tang.clear(); tang.resize(3);
tEnc->getEncoders(tang.data());
hang.clear(); hang.resize(6);
hEnc->getEncoders(hang.data());
aang.clear(); aang.resize(16);
aEnc->getEncoders(aang.data());
/* get the xyz location of the gaze point */
Vector bvL(2); Vector bvR(2);
bvL[0] = bfL.get(0).asDouble(); bvL[1] = bfL.get(1).asDouble();
bvR[0] = bfR.get(0).asDouble(); bvR[1] = bfR.get(1).asDouble();
objPos.clear(); objPos.resize(3);
gaze->triangulate3DPoint(bvL,bvR,objPos);
/* servo the head to gaze at that point */
//gaze->lookAtStereoPixels(bvL,bvR);
gaze->lookAtFixationPoint(objPos);
gaze->waitMotionDone(1.0,10.0);
gaze->stopControl();
printf("object position estimated as: %f, %f, %f\n", objPos[0], objPos[1], objPos[2]);
printf("is this ok?\n");
string posResp = Network::readString().c_str();
if (posResp == "yes" || posResp == "y") {
/* move to hover the hand over the XY position of the target: [X, Y, Z=0.2], with palm upright */
objPos[2] = 0.1;
Vector axa(4);
axa.zero();
if (armInUse) {
axa[2] = 1.0; axa[3] = M_PI;
} else {
axa[1] = 1.0; axa[3] = M_PI;
}
carm->goToPoseSync(objPos,axa);
carm->waitMotionDone(1.0,10.0);
Time::delay(2.0);
//curl fingers and thumb slightly to hold object
Vector armCur(16);
aEnc->getEncoders(armCur.data());
armCur[8] = 3; armCur[10] = 25;
armCur[11] = 25; armCur[12] = 25;
armCur[13] = 25; armCur[14] = 25;
armCur[15] = 55;
aPos->positionMove(armCur.data());
Time::delay(2.0);
/* wait for terminal signal from user that object has been moved to the hand */
bool validTarg = false;
printf("object position reached, place in hand and enter target xy position\n");
while (!validTarg) {
string objResp = Network::readString().c_str();
/* ask the user to enter in an XY target location, or confirm use of previous one */
Bottle btarPos(objResp.c_str());
if (btarPos.size() < 2) {
//if user enters no target position, try to use last entered position
if (targetPos.length() != 3) {
printf("no previous target position available, please re-enter:\n");
} else {
validTarg = true;
}
} else {
targetPos.clear(); targetPos.resize(3);
targetPos[0] = btarPos.get(0).asDouble();
targetPos[1] = btarPos.get(1).asDouble();
targetPos[2] = 0.1;
validTarg = true;
}
}
/* move the arm to above the target location */
axa.zero();
if (armInUse) {
axa[2] = 1.0; axa[3] = M_PI;
} else {
axa[1] = 1.0; axa[3] = M_PI;
}
carm->goToPoseSync(targetPos,axa);
//carm->goToPosition(targetPos);
carm->waitMotionDone(1.0,10.0);
Time::delay(2.0);
/* wait for user signal that the object has been removed */
printf("object has been moved to target location. please remove object and hit enter\n");
string tarResp = Network::readString().c_str();
}
/* return to saved motor configuration, clear click buffers, lower flag signaling action done */
printf("gaze done, attempting reset\n");
tPos->positionMove(tang.data());
hPos->positionMove(hang.data());
aPos->positionMove(aang.data());
bfL.clear(); bfR.clear();
bfL.unlock(); bfR.unlock();
susmsg.clear();
susmsg.addInt(0);
susPort->write(susmsg);
}
else {
bfL.unlock(); bfR.unlock();
}
}
}
bool tune(const int i)
{
PidData &pid=pids[i];
Property pGeneral;
pGeneral.put("joint",i);
string sGeneral="(general ";
sGeneral+=pGeneral.toString().c_str();
sGeneral+=')';
Bottle bGeneral,bPlantEstimation,bStictionEstimation;
bGeneral.fromString(sGeneral.c_str());
bPlantEstimation.fromString("(plant_estimation (Ts 0.01) (Q 1.0) (R 1.0) (P0 100000.0) (tau 1.0) (K 1.0) (max_pwm 800.0))");
bStictionEstimation.fromString("(stiction_estimation (Ts 0.01) (T 2.0) (vel_thres 5.0) (e_thres 1.0) (gamma (10.0 10.0)) (stiction (0.0 0.0)))");
Bottle bConf=bGeneral;
bConf.append(bPlantEstimation);
bConf.append(bStictionEstimation);
Property pOptions(bConf.toString().c_str());
OnlineCompensatorDesign designer;
if (!designer.configure(*driver,pOptions))
{
yError("designer configuration failed!");
return false;
}
idlingCoupledJoints(i,true);
Property pPlantEstimation;
pPlantEstimation.put("max_time",20.0);
pPlantEstimation.put("switch_timeout",2.0);
designer.startPlantEstimation(pPlantEstimation);
yInfo("Estimating plant for joint %d: max duration = %g seconds",
i,pPlantEstimation.find("max_time").asDouble());
double t0=Time::now();
while (!designer.isDone())
{
yInfo("elapsed %d [s]",(int)(Time::now()-t0));
Time::delay(1.0);
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
}
Property pResults;
designer.getResults(pResults);
double tau=pResults.find("tau_mean").asDouble();
double K=pResults.find("K_mean").asDouble();
yInfo("plant = %g/s * 1/(1+s*%g)",K,tau);
Property pControllerRequirements,pController;
pControllerRequirements.put("tau",tau);
pControllerRequirements.put("K",K);
pControllerRequirements.put("f_c",0.75);
if (i!=15)
{
pControllerRequirements.put("T_dr",1.0);
pControllerRequirements.put("type","PI");
}
else
pControllerRequirements.put("type","P");
designer.tuneController(pControllerRequirements,pController);
yInfo("tuning results: %s",pController.toString().c_str());
double Kp=pController.find("Kp").asDouble();
double Ki=pController.find("Ki").asDouble();
pid.scale=4.0;
int scale=(int)pid.scale; int shift=1<<scale;
double fwKp=floor(Kp*pid.encs_ratio*shift);
double fwKi=floor(Ki*pid.encs_ratio*shift/1000.0);
pid.Kp=yarp::math::sign(pid.Kp*fwKp)>0.0?fwKp:-fwKp;
pid.Ki=yarp::math::sign(pid.Ki*fwKi)>0.0?fwKi:-fwKi;
pid.Kd=0.0;
yInfo("Kp (FW) = %g; Ki (FW) = %g; Kd (FW) = %g; shift factor = %d",pid.Kp,pid.Ki,pid.Kd,scale);
Property pStictionEstimation;
pStictionEstimation.put("max_time",60.0);
pStictionEstimation.put("Kp",Kp);
pStictionEstimation.put("Ki",0.0);
pStictionEstimation.put("Kd",0.0);
designer.startStictionEstimation(pStictionEstimation);
yInfo("Estimating stiction for joint %d: max duration = %g seconds",
i,pStictionEstimation.find("max_time").asDouble());
t0=Time::now();
while (!designer.isDone())
{
yInfo("elapsed %d [s]",(int)(Time::now()-t0));
Time::delay(1.0);
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
}
designer.getResults(pResults);
pid.st_up=floor(pResults.find("stiction").asList()->get(0).asDouble());
pid.st_down=floor(pResults.find("stiction").asList()->get(1).asDouble());
yInfo("Stiction values: up = %g; down = %g",pid.st_up,pid.st_down);
IControlMode2 *imod;
IPositionControl *ipos;
IEncoders *ienc;
driver->view(imod);
driver->view(ipos);
driver->view(ienc);
imod->setControlMode(i,VOCAB_CM_POSITION);
ipos->setRefSpeed(i,50.0);
ipos->positionMove(i,0.0);
yInfo("Driving the joint back to rest... ");
t0=Time::now();
while (Time::now()-t0<5.0)
{
double enc;
ienc->getEncoder(i,&enc);
if (fabs(enc)<1.0)
break;
if (interrupting)
{
idlingCoupledJoints(i,false);
return false;
}
Time::delay(0.2);
}
yInfo("done!");
idlingCoupledJoints(i,false);
return true;
}
bool SpringyFingersModel::fromProperty(const Property &options)
{
Property &opt=const_cast<Property&>(options);
if (!opt.check("name") || !opt.check("type"))
{
printMessage(1,"missing mandatory options \"name\" and/or \"type\"\n");
return false;
}
if (configured)
close();
name=opt.find("name").asString().c_str();
type=opt.find("type").asString().c_str();
robot=opt.check("robot",Value("icub")).asString().c_str();
carrier=opt.check("carrier",Value("udp")).asString().c_str();
verbosity=opt.check("verbosity",Value(0)).asInt();
string part_motor=string(type+"_arm");
string part_analog=string(type+"_hand");
Property prop;
prop.put("device","remote_controlboard");
prop.put("remote",("/"+robot+"/"+part_motor).c_str());
prop.put("local",("/"+name+"/"+part_motor).c_str());
if (!driver.open(prop))
return false;
port->open(("/"+name+"/"+part_analog+"/analog:i").c_str());
string analogPortName(("/"+robot+"/"+part_analog+"/analog:o").c_str());
if (!Network::connect(analogPortName.c_str(),port->getName().c_str(),carrier.c_str()))
{
printMessage(1,"unable to connect to %s\n",analogPortName.c_str());
close();
return false;
}
IEncoders *ienc; driver.view(ienc);
int nAxes; ienc->getAxes(&nAxes);
printMessage(1,"configuring interface-based sensors ...\n");
Property propGen;
propGen.put("name","In_0");
propGen.put("size",nAxes);
Property propThumb=propGen; propThumb.put( "index",10);
Property propIndex=propGen; propIndex.put( "index",12);
Property propMiddle=propGen; propMiddle.put("index",14);
Property propRing=propGen; propRing.put( "index",15);
Property propLittle=propGen; propLittle.put("index",15);
bool sensors_ok=true;
void *pIF=static_cast<void*>(ienc);
sensors_ok&=sensIF[0].configure(pIF,propThumb);
sensors_ok&=sensIF[1].configure(pIF,propIndex);
sensors_ok&=sensIF[2].configure(pIF,propMiddle);
sensors_ok&=sensIF[3].configure(pIF,propRing);
sensors_ok&=sensIF[4].configure(pIF,propLittle);
printMessage(1,"configuring port-based sensors ...\n");
Property thumb_mp( "(name Out_0) (index 1)" );
Property thumb_ip( "(name Out_1) (index 2)" );
Property index_mp( "(name Out_0) (index 4)" );
Property index_ip( "(name Out_1) (index 5)" );
Property middle_mp( "(name Out_0) (index 7)" );
Property middle_ip( "(name Out_1) (index 8)" );
Property ring_mp( "(name Out_0) (index 9)" );
Property ring_pip( "(name Out_1) (index 10)");
Property ring_dip( "(name Out_2) (index 11)");
Property little_mp( "(name Out_0) (index 12)");
Property little_pip("(name Out_1) (index 13)");
Property little_dip("(name Out_2) (index 14)");
void *pPort=static_cast<void*>(port);
sensors_ok&=sensPort[0].configure(pPort,thumb_mp);
sensors_ok&=sensPort[1].configure(pPort,thumb_ip);
sensors_ok&=sensPort[2].configure(pPort,index_mp);
sensors_ok&=sensPort[3].configure(pPort,index_ip);
sensors_ok&=sensPort[4].configure(pPort,middle_mp);
sensors_ok&=sensPort[5].configure(pPort,middle_ip);
sensors_ok&=sensPort[6].configure(pPort,ring_mp);
sensors_ok&=sensPort[7].configure(pPort,ring_pip);
sensors_ok&=sensPort[8].configure(pPort,ring_dip);
sensors_ok&=sensPort[9].configure(pPort,little_mp);
sensors_ok&=sensPort[10].configure(pPort,little_pip);
sensors_ok&=sensPort[11].configure(pPort,little_dip);
if (!sensors_ok)
{
printMessage(1,"some errors occured\n");
close();
return false;
}
printMessage(1,"configuring fingers ...\n");
Property thumb(opt.findGroup("thumb").toString().c_str());
Property index(opt.findGroup("index").toString().c_str());
Property middle(opt.findGroup("middle").toString().c_str());
Property ring(opt.findGroup("ring").toString().c_str());
Property little(opt.findGroup("little").toString().c_str());
bool fingers_ok=true;
fingers_ok&=fingers[0].fromProperty(thumb);
fingers_ok&=fingers[1].fromProperty(index);
fingers_ok&=fingers[2].fromProperty(middle);
fingers_ok&=fingers[3].fromProperty(ring);
fingers_ok&=fingers[4].fromProperty(little);
if (!fingers_ok)
{
printMessage(1,"some errors occured\n");
close();
return false;
}
printMessage(1,"attaching sensors to fingers ...\n");
fingers[0].attachSensor(sensIF[0]);
fingers[0].attachSensor(sensPort[0]);
fingers[0].attachSensor(sensPort[1]);
fingers[1].attachSensor(sensIF[1]);
fingers[1].attachSensor(sensPort[2]);
fingers[1].attachSensor(sensPort[3]);
fingers[2].attachSensor(sensIF[2]);
fingers[2].attachSensor(sensPort[4]);
fingers[2].attachSensor(sensPort[5]);
fingers[3].attachSensor(sensIF[3]);
fingers[3].attachSensor(sensPort[6]);
fingers[3].attachSensor(sensPort[7]);
fingers[3].attachSensor(sensPort[8]);
fingers[4].attachSensor(sensIF[4]);
fingers[4].attachSensor(sensPort[9]);
fingers[4].attachSensor(sensPort[10]);
fingers[4].attachSensor(sensPort[11]);
attachNode(fingers[0]);
attachNode(fingers[1]);
attachNode(fingers[2]);
attachNode(fingers[3]);
attachNode(fingers[4]);
printMessage(1,"configuration complete\n");
return configured=true;
}
virtual bool configure(ResourceFinder &rf)
{
// get command line options
if (!rf.check("robot") || !rf.check("part"))
{
printf("Missing either --robot or --part options. Quitting!\n");
return false;
}
std::string dumpername;
// get dumepr name
if (rf.check("name"))
{
dumpername = rf.find("name").asString();
dumpername += "/";
}
else
{
dumpername = "/controlBoardDumper/";
}
Value& robot = rf.find("robot");
Value& part = rf.find("part");
configFileRobotPart = "config_";
configFileRobotPart = configFileRobotPart + robot.asString();
configFileRobotPart = configFileRobotPart + "_";
configFileRobotPart = configFileRobotPart + part.asString();
configFileRobotPart = configFileRobotPart + ".ini";
//get the joints to be dumped
getRate(rf, rate);
if (getNumberUsedJoints(rf, nJoints) == 0)
return false;
thetaMap = new int[nJoints];
if (getUsedJointsMap(rf, nJoints, thetaMap) == 0)
return false;
//get the type of data to be dumped
if (getNumberDataToDump(rf, nData) == 0)
return false;
dataToDump = new std::string[nData];
if (getDataToDump(rf, dataToDump, nData, &useDebugClient) == 0)
return false;
// Printing configuration to the user
yInfo("Running with the following configuration:\n");
yInfo("Selected rate is: %d\n", rate);
yInfo("Data selected to be dumped are:\n");
for (int i = 0; i < nData; i++)
yInfo("\t%s \n", dataToDump[i].c_str());
//open remote control board
ddBoardOptions.put("device", "remote_controlboard");
ddDebugOptions.put("device", "debugInterfaceClient");
std::string localPortName = name;
std::string localDebugPortName = name;
localPortName = localPortName + dumpername;
localDebugPortName = localPortName + "debug/";
//localPortName = localPortName + robot.asString();
localPortName = localPortName + part.asString();
localDebugPortName = localDebugPortName + part.asString();
ddBoardOptions.put("local", localPortName);
ddDebugOptions.put("local", localDebugPortName);
std::string remotePortName = "/";
std::string remoteDebugPortName;
remotePortName = remotePortName + robot.asString();
remotePortName = remotePortName + "/";
remotePortName = remotePortName + part.asString();
ddBoardOptions.put("remote", remotePortName);
remoteDebugPortName = remotePortName + "/debug";
ddDebugOptions.put("remote", remoteDebugPortName);
// create a device
ddBoard.open(ddBoardOptions);
if (!ddBoard.isValid()) {
printf("Device not available.\n");
return false;
}
if (useDebugClient )
{
ddDebug.open(ddDebugOptions);
if (!ddDebug.isValid())
{
yError("\n-----------------------------------------\n");
yError("Debug Interface is mandatory to run this module with the '--dataToDumpAll' option or to dump any of the getRotorxxx data.\n");
yError("Please Verify the following 2 conditions are satisfied:\n\n");
yError("1) Check 'debugInterfaceClient' is available using 'yarpdev --list' command\n");
// yError("%s", Drivers::factory().toString().c_str());
std::string deviceList, myDev;
deviceList.clear();
deviceList.append(Drivers::factory().toString());
myDev = "debugInterfaceClient";
if(deviceList.find(myDev) != std::string::npos)
yError("\t--> Seems OK\n");
else
yError("\t--> Seems NOT OK. The device was not found, please activate the compilation using the corrisponding CMake flag.\n");
yError("\n2) Check if the robot has the 'debugInterfaceWrapper' device up and running. \n You should see from 'yarp name list' output, a port called\n");
yError("\t/robotName/part_name/debug/rpc:i\n If not, fix the robot configuration files to instantiate the 'debugInterfaceWrapper' device.\n");
yError("\nQuickFix: If you set the --dataToDumpAll and do not need the advanced debug feature (getRotorxxx) just remove this option. See help for more information.\n");
yError("------------- END ERROR MESSAGE ---------------\n\n");
return false;
}
}
bool logToFile = false;
if (rf.check("logToFile")) logToFile = true;
portPrefix= dumpername + part.asString() + "/";
//boardDumperThread *myDumper = new boardDumperThread(&dd, rate, portPrefix, dataToDump[0]);
//myDumper->setThetaMap(thetaMap, nJoints);
myDumper = new boardDumperThread[nData];
for (int i = 0; i < nData; i++)
{
if (dataToDump[i] == "getEncoders" )
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getEncs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetEncs.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetEncs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetEncs);
}
}
else if (dataToDump[i] == "getEncoderSpeeds")
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getSpeeds thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetSpeeds.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
myGetSpeeds.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetSpeeds);
}
}
else if (dataToDump[i] == "getEncoderAccelerations")
{
if (ddBoard.view(ienc))
{
yInfo("Initializing a getAccs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetAccs.setInterface(ienc);
if (ddBoard.view(istmp))
{
yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetAccs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetAccs);
}
}
else if (dataToDump[i] == "getPosPidReferences")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPidRefs.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getPosPidReferences::The time stamp initalization interfaces was successfull! \n");
myGetPidRefs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPidRefs);
}
}
else if (dataToDump[i] == "getTrqPidReferences")
{
if (ddBoard.view(itrq))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqRefs.setInterface(itrq);
if (ddBoard.view(istmp))
{
yInfo("getTrqPidReferences::The time stamp initalization interfaces was successfull! \n");
myGetTrqRefs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqRefs);
}
}
else if (dataToDump[i] == "getControlModes")
{
if (ddBoard.view(icmod))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetControlModes.setInterface(icmod, nJoints);
if (ddBoard.view(istmp))
{
yInfo("getControlModes::The time stamp initalization interfaces was successfull! \n");
myGetControlModes.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetControlModes);
}
}
else if (dataToDump[i] == "getInteractionModes")
{
if (ddBoard.view(iimod))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetInteractionModes.setInterface(iimod, nJoints);
if (ddBoard.view(istmp))
{
yInfo("getInteractionModes::The time stamp initalization interfaces was successfull! \n");
myGetInteractionModes.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetInteractionModes);
}
}
else if (dataToDump[i] == "getPositionErrors")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getErrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetPosErrs.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getPositionErrors::The time stamp initalization interfaces was successfull! \n");
myGetPosErrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetPosErrs);
}
}
else if (dataToDump[i] == "getOutputs")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getOuts thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetOuts.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getOutputs::The time stamp initalization interfaces was successfull! \n");
myGetOuts.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetOuts);
}
}
else if (dataToDump[i] == "getCurrents")
{
if (ddBoard.view(iamp))
{
yInfo("Initializing a getCurrs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetCurrs.setInterface(iamp);
if (ddBoard.view(istmp))
{
yInfo("getCurrents::The time stamp initalization interfaces was successfull! \n");
myGetCurrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetCurrs);
}
}
else if (dataToDump[i] == "getTorques")
{
if (ddBoard.view(itrq))
{
yInfo("Initializing a getTorques thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqs.setInterface(itrq);
if (ddBoard.view(istmp))
{
yInfo("getTorques::The time stamp initalization interfaces was successfull! \n");
myGetTrqs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqs);
}
}
else if (dataToDump[i] == "getTorqueErrors")
{
if (ddBoard.view(ipid))
{
yInfo("Initializing a getTorqueErrors thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTrqErrs.setInterface(ipid);
if (ddBoard.view(istmp))
{
yInfo("getTorqueErrors::The time stamp initalization interfaces was successfull! \n");
myGetTrqErrs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTrqErrs);
}
}
else if (dataToDump[i] == "getTemperatures")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getTemps thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetTemps.setInterface(imot);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetTemps.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetTemps);
}
}
else if (dataToDump[i] == "getMotorEncoders")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getEncs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorEncs.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncoders::The time stamp initalization interfaces was successfull! \n");
myGetMotorEncs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorEncs);
}
}
else if (dataToDump[i] == "getMotorEncoderSpeeds")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getSpeeds thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorSpeeds.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncodersSpeed::The time stamp initalization interfaces was successfull! \n");
myGetMotorSpeeds.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorSpeeds);
}
}
else if (dataToDump[i] == "getMotorEncoderAccelerations")
{
if (ddBoard.view(imotenc))
{
yInfo("Initializing a getAccs thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotorAccs.setInterface(imotenc);
if (ddBoard.view(istmp))
{
yInfo("getEncoderAccelerations::The time stamp initalization interfaces was successfull! \n");
myGetMotorAccs.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotorAccs);
}
}
else if (dataToDump[i] == "getMotorsPwm")
{
if (ddBoard.view(iamp))
{
yInfo("Initializing a getMotPwm thread\n");
myDumper[i].setDevice(&ddBoard, &ddDebug, rate, portPrefix, dataToDump[i], logToFile);
myDumper[i].setThetaMap(thetaMap, nJoints);
myGetMotPwm.setInterface(iamp);
if(ienc == 0)
{
if(!ddBoard.view(ienc))
return false;
}
ienc->getAxes(&myGetMotPwm.n_joint_part);
if (ddBoard.view(istmp))
{
yInfo("getMotorsPwm::The time stamp initalization interfaces was successfull! \n");
myGetMotPwm.setStamp(istmp);
}
else
yError("Problems getting the time stamp interfaces \n");
myDumper[i].setGetter(&myGetMotPwm);
}
}
}
Time::delay(1);
for (int i = 0; i < nData; i++)
myDumper[i].start();
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;
}
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;
}
Solver::Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commData,
EyePinvRefGen *_eyesRefGen, Localizer *_loc, Controller *_ctrl,
const unsigned int _period) :
RateThread(_period), drvTorso(_drvTorso), drvHead(_drvHead),
commData(_commData), eyesRefGen(_eyesRefGen), loc(_loc),
ctrl(_ctrl), period(_period), Ts(_period/1000.0)
{
// Instantiate objects
neck=new iCubHeadCenter(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
imu=new iCubInertialSensor(commData->head_version>1.0?"v2":"v1");
// remove constraints on the links: logging purpose
imu->setAllConstraints(false);
// block neck dofs
eyeL->blockLink(3,0.0); eyeR->blockLink(3,0.0);
eyeL->blockLink(4,0.0); eyeR->blockLink(4,0.0);
eyeL->blockLink(5,0.0); eyeR->blockLink(5,0.0);
// Get the chain objects
chainNeck=neck->asChain();
chainEyeL=eyeL->asChain();
chainEyeR=eyeR->asChain();
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain());
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain());
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain());
}
// read number of joints
if (drvTorso!=NULL)
{
IEncoders *encTorso; drvTorso->view(encTorso);
encTorso->getAxes(&nJointsTorso);
}
else
nJointsTorso=3;
IEncoders *encHead; drvHead->view(encHead);
encHead->getAxes(&nJointsHead);
// joints bounds alignment
alignJointsBounds(chainNeck,drvTorso,drvHead,commData->eyeTiltLim);
// read starting position
fbTorso.resize(nJointsTorso,0.0);
fbHead.resize(nJointsHead,0.0);
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
copyJointsBounds(chainNeck,chainEyeL);
copyJointsBounds(chainEyeL,chainEyeR);
// store neck pitch/roll/yaw bounds
neckPitchMin=(*chainNeck)[3].getMin();
neckPitchMax=(*chainNeck)[3].getMax();
neckRollMin =(*chainNeck)[4].getMin();
neckRollMax =(*chainNeck)[4].getMax();
neckYawMin =(*chainNeck)[5].getMin();
neckYawMax =(*chainNeck)[5].getMax();
neckPos.resize(3);
gazePos.resize(3);
updateAngles();
updateTorsoBlockedJoints(chainNeck,fbTorso);
chainNeck->setAng(3,fbHead[0]);
chainNeck->setAng(4,fbHead[1]);
chainNeck->setAng(5,fbHead[2]);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
updateNeckBlockedJoints(chainEyeL,fbHead);
updateNeckBlockedJoints(chainEyeR,fbHead);
Vector eyePos(2);
eyePos[0]=gazePos[0];
eyePos[1]=gazePos[1]+gazePos[2]/2.0;
chainEyeL->setAng(eyePos);
eyePos[1]=gazePos[1]-gazePos[2]/2.0;
chainEyeR->setAng(eyePos);
fbTorsoOld=fbTorso;
neckAngleUserTolerance=0.0;
}
bool updateModule()
{
ImageOf<PixelRgb> *imgIn=imgInPort.read(true);
if (imgIn==NULL)
return false;
ImageOf<PixelRgb> &imgOut=imgOutPort.prepare();
imgOut=*imgIn;
cv::Mat img=cv::cvarrToMat(imgOut.getIplImage());
Vector xa,oa;
iarm->getPose(xa,oa);
Matrix Ha=axis2dcm(oa);
xa.push_back(1.0);
Ha.setCol(3,xa);
Vector pc;
igaze->get2DPixel(camSel,xa,pc);
cv::Point point_c((int)pc[0],(int)pc[1]);
cv::circle(img,point_c,4,cv::Scalar(0,255,0),4);
Vector analogs,encs(nEncs),joints;
if (ianalog!=NULL)
ianalog->read(analogs);
iencs->getEncoders(encs.data());
for (int i=0; i<3; i++)
{
if (ianalog!=NULL)
finger[i].getChainJoints(encs,analogs,joints);
else
finger[i].getChainJoints(encs,joints);
finger[i].setAng(CTRL_DEG2RAD*joints);
}
for (int fng=0; fng<3; fng++)
{
deque<cv::Point> point_f;
for (int i=-1; i<(int)finger[fng].getN(); i++)
{
Vector fc;
igaze->get2DPixel(camSel,Ha*(i<0?finger[fng].getH0().getCol(3):
finger[fng].getH(i,true).getCol(3)),fc);
point_f.push_front(cv::Point((int)fc[0],(int)fc[1]));
cv::circle(img,point_f.front(),3,cv::Scalar(0,0,255),4);
if (i>=0)
{
cv::line(img,point_f.front(),point_f.back(),cv::Scalar(255,255,255),2);
point_f.pop_back();
}
else
cv::line(img,point_c,point_f.front(),cv::Scalar(255,0,0),2);
}
}
imgOutPort.writeStrict();
return true;
}
int main(int argc, char **argv)
{
Network yarp;
printf("Going to stress rpc connections to the robot\n");
printf("Run as --id unique-id\n");
printf("Optionally:\n");
printf("--part robot-part\n");
printf("--prot protocol\n");
printf("--time dt (seconds)\n");
printf("--robot name \n");
Property parameters;
parameters.fromCommand(argc, argv);
ConstString part=parameters.find("part").asString();
int id=parameters.find("id").asInt();
double time=0;
if (parameters.check("time"))
{
time=parameters.find("time").asDouble();
}
else
time=-1;
ConstString protocol;
if (parameters.check("prot"))
{
protocol=parameters.find("prot").asString();
}
else
protocol="tcp";
ConstString rname;
if (parameters.check("robot"))
{
rname=parameters.find("robot").asString();
}
else
rname="controlboard";
PolyDriver dd;
Property p;
Random::seed((unsigned int)Time::now());
string remote=string("/")+rname.c_str();
if (part!="")
{
remote+=string("/");
remote+=part;
}
string local=string("/")+string(rname.c_str());
if (part!="")
{
local+=string("/");
local+=part;
}
local+=string("/stress");
stringstream lStream;
lStream << id;
local += lStream.str();
p.put("device", "remote_controlboard");
p.put("local", local.c_str());
p.put("remote", remote.c_str());
p.put("carrier", protocol.c_str());
dd.open(p);
if (!dd.isValid())
{
fprintf(stderr, "Error, could not open controlboard\n");
return -1;
}
IEncoders *ienc;
IPositionControl *ipos;
IAmplifierControl *iamp;
IPidControl *ipid;
IControlLimits *ilim;
IControlCalibration2 *ical;
dd.view(ienc);
dd.view(ipos);
dd.view(iamp);
dd.view(ipid);
dd.view(ilim);
dd.view(ical);
int c=100;
int nj;
Vector encoders;
ienc->getAxes(&nj);
encoders.resize(nj);
int count=0;
bool done=false;
double startT=Time::now();
double now=0;
while((!done) || (time==-1))
{
count++;
double v;
int jj=0;
for(jj=0; jj<nj; jj++)
{
// ienc->getEncoder(jj, encoders.data()+jj);
// iamp->enableAmp(jj);
// ilim->setLimits(jj, 0, 0);
// double max;
// double min;
// ilim->getLimits(jj, &min, &max);
fprintf(stderr, ".");
// Pid pid;
// ipid->getPid(jj, &pid);
bool done;
ipos->checkMotionDone(jj, &done);
fprintf(stderr, "#\n");
}
fprintf(stderr, "%u\n", count);
Time::delay(0.1);
now=Time::now();
if ((now-startT)>time)
done=true;
}
printf("bye bye from %d\n", id);
return 0;
}
bool configure(ResourceFinder &rf)
{
string robot=rf.check("robot",Value("icub")).asString();
string arm=rf.check("arm",Value("left")).asString();
string eye=rf.check("eye",Value("left")).asString();
bool analog=(rf.check("analog",Value(robot=="icub"?"on":"off")).asString()=="on");
if ((arm!="left") && (arm!="right"))
{
yError()<<"Invalid arm requested";
return false;
}
if ((eye!="left") && (eye!="right"))
{
yError()<<"Invalid eye requested";
return false;
}
// open drivers
if (analog)
{
Property optionAnalog("(device analogsensorclient)");
optionAnalog.put("remote","/"+robot+"/"+arm+"_hand/analog:o");
optionAnalog.put("local","/show-fingers/analog");
if (!drvAnalog.open(optionAnalog))
{
yError()<<"Analog sensor not available";
terminate();
return false;
}
}
Property optionArm("(device remote_controlboard)");
optionArm.put("remote","/"+robot+"/"+arm+"_arm");
optionArm.put("local","/show-fingers/joints");
if (!drvArm.open(optionArm))
{
yError()<<"Joints arm controller not available";
terminate();
return false;
}
Property optionCart("(device cartesiancontrollerclient)");
optionCart.put("remote","/"+robot+"/cartesianController/"+arm+"_arm");
optionCart.put("local","/show-fingers/cartesian");
if (!drvCart.open(optionCart))
{
yError()<<"Cartesian arm controller not available";
terminate();
return false;
}
Property optionGaze("(device gazecontrollerclient)");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local","/show-fingers/gaze");
if (!drvGaze.open(optionGaze))
{
yError()<<"Gaze controller not available";
terminate();
return false;
}
if (analog)
drvAnalog.view(ianalog);
else
ianalog=NULL;
IControlLimits *ilim;
drvArm.view(iencs);
drvArm.view(ilim);
drvCart.view(iarm);
drvGaze.view(igaze);
iencs->getAxes(&nEncs);
finger[0]=iCubFinger(arm+"_thumb");
finger[1]=iCubFinger(arm+"_index");
finger[2]=iCubFinger(arm+"_middle");
deque<IControlLimits*> lim;
lim.push_back(ilim);
for (int i=0; i<3; i++)
finger[i].alignJointsBounds(lim);
yInfo()<<"Using arm="<<arm<<" and eye="<<eye;
imgInPort.open("/show-fingers/img:i");
imgOutPort.open("/show-fingers/img:o");
camSel=(eye=="left")?0:1;
return true;
}
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;
}
bool SpringyFingersModel::calibrate(const Property &options)
{
if (configured)
{
IControlMode2 *imod; driver.view(imod);
IControlLimits *ilim; driver.view(ilim);
IEncoders *ienc; driver.view(ienc);
IPositionControl *ipos; driver.view(ipos);
int nAxes; ienc->getAxes(&nAxes);
Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);
printMessage(1,"steering the hand to a suitable starting configuration\n");
for (int j=7; j<nAxes; j++)
{
imod->setControlMode(j,VOCAB_CM_POSITION);
ilim->getLimits(j,&qmin[j],&qmax[j]);
ipos->getRefAcceleration(j,&acc[j]);
ipos->getRefSpeed(j,&vel[j]);
ipos->setRefAcceleration(j,1e9);
ipos->setRefSpeed(j,60.0);
ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]); // thumb in opposition
}
printMessage(1,"proceeding with the calibration\n");
Property &opt=const_cast<Property&>(options);
string tag=opt.check("finger",Value("all")).asString().c_str();
if (tag=="thumb")
{
calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
}
else if (tag=="index")
{
calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
}
else if (tag=="middle")
{
calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
}
else if (tag=="ring")
{
calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
}
else if (tag=="little")
{
calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
}
else if ((tag=="all") || (tag=="all_serial"))
{
calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
}
else if (tag=="all_parallel")
{
CalibThread thr[5];
thr[0].setInfo(this,fingers[0],10,qmin[10],qmax[10]);
thr[1].setInfo(this,fingers[1],12,qmin[12],qmax[12]);
thr[2].setInfo(this,fingers[2],14,qmin[14],qmax[14]);
thr[3].setInfo(this,fingers[3],15,qmin[15],qmax[15]);
thr[4].setInfo(this,fingers[4],15,qmin[15],qmax[15]);
thr[0].start(); thr[1].start(); thr[2].start();
thr[3].start(); thr[4].start();
bool done=false;
while (!done)
{
done=true;
for (int i=0; i<5; i++)
{
done&=thr[i].isDone();
if (thr[i].isDone() && thr[i].isRunning())
thr[i].stop();
}
Time::delay(0.1);
}
}
else
{
printMessage(1,"unknown finger request %s\n",tag.c_str());
return false;
}
for (int j=7; j<nAxes; j++)
{
ipos->setRefAcceleration(j,acc[j]);
ipos->setRefSpeed(j,vel[j]);
}
return true;
}
else
return false;
}
int main(int argc, char *argv[])
{
Network yarp;
Property params;
params.fromCommand(argc, argv);
if (!params.check("robot"))
{
fprintf(stderr, "Please specify the name of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
fprintf(stderr, "--part name (right_arm or left_arm)\n");
fprintf(stderr, "--durationInS duration (in seconds)\n");
fprintf(stderr, "--sampleInMS sample time (in milliseconds)\n");
fprintf(stderr, "--refTimeInMS Reference time of minimum jerk trajectory generator (in milliseconds)\n");
return 1;
}
if( !params.check("sampleInMS") ||
!params.check("durationInS") ||
!params.check("refTimeInMS") )
{
fprintf(stderr, "Necessary params not passed\n");
return EXIT_FAILURE;
}
std::string robotName=params.find("robot").asString().c_str();
std::string partName=params.find("part").asString().c_str();
double durationInS = params.find("durationInS").asDouble();
double samplesInMS = params.find("sampleInMS").asDouble();
double refTimeInMS = params.find("refTimeInMS").asDouble();
std::cout << "robotName : " << robotName << std::endl;
std::cout << "partName : " << partName << std::endl;
std::cout << "durationInS : " << durationInS << std::endl;
std::cout << "samplesInMS : " << samplesInMS << std::endl;
std::cout << "refTimeInMS : " << refTimeInMS << std::endl;
std::string remotePorts="/";
remotePorts+=robotName;
remotePorts+="/"+partName;
std::string localPorts="/randomShoulderMovements/client";
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPorts.c_str()); //local port names
options.put("remote", remotePorts.c_str()); //where we connect to
// create a device
PolyDriver robotDevice(options);
if (!robotDevice.isValid()) {
printf("Device not available. Here are the known devices:\n");
printf("%s", Drivers::factory().toString().c_str());
return 0;
}
IPositionDirect *pos;
IEncoders *encs;
IControlLimits * lims;
IControlMode2 * ictrl;
bool ok;
ok = robotDevice.view(pos);
ok = ok && robotDevice.view(encs);
ok = ok && robotDevice.view(lims);
ok = ok && robotDevice.view(ictrl);
if (!ok) {
printf("Problems acquiring interfaces\n");
return 0;
}
int nj=0;
pos->getAxes(&nj);
Vector encodersInDeg(nj);
Vector desPosInDeg(3);
Vector commandInDeg(3);
Vector minShoulderInDeg(3);
Vector maxShoulderInDeg(3);
Vector lowerBoundConstraint;
Vector upperBoundConstraint;
Matrix constraintMatrix;
for(int i = 0; i < 3; i++ )
{
lims->getLimits(i,&(minShoulderInDeg[i]),&(maxShoulderInDeg[i]));
}
bool encodersRead = encs->getEncoders(encodersInDeg.data());
while( !encodersRead )
{
encodersRead = encs->getEncoders(encodersInDeg.data());
}
getShoulderConstraint(lowerBoundConstraint,upperBoundConstraint,constraintMatrix);
// Create the minimum jerk filter
iCub::ctrl::minJerkTrajGen filter(3,samplesInMS,refTimeInMS);
filter.init(encodersInDeg.subVector(0,2));
std::cout << "Filter initial value " << encodersInDeg.subVector(0,2).toString() << std::endl;
std::vector<int> indexToControl;
indexToControl.push_back(0);
indexToControl.push_back(1);
indexToControl.push_back(2);
// Set control modes
ictrl->setControlMode(0,VOCAB_CM_POSITION_DIRECT);
ictrl->setControlMode(1,VOCAB_CM_POSITION_DIRECT);
ictrl->setControlMode(2,VOCAB_CM_POSITION_DIRECT);
double samplesInS = samplesInMS/1000.0;
int samples = (int)(durationInS/samplesInS);
int samplesForNewPosition = (int)(refTimeInMS/samplesInMS);
for(int i=0; i < samples; i++ )
{
if( i % samplesForNewPosition == 0 )
{
// Generate a new desired shoulder position
generateRandomShoulderPosition(desPosInDeg,minShoulderInDeg,maxShoulderInDeg,
lowerBoundConstraint,upperBoundConstraint,constraintMatrix);
std::cout << "New position generated" << std::endl;
std::cout << "Position generated " << desPosInDeg.toString() << std::endl;
std::cout << "Remaining time " << (samples-i)*(samplesInS) << std::endl;
}
filter.computeNextValues(desPosInDeg);
commandInDeg = filter.getPos();
std::cout << "Position set " << commandInDeg.toString() << std::endl;
pos->setPositions(3,indexToControl.data(),commandInDeg.data());
yarp::os::Time::delay(samplesInS);
}
robotDevice.close();
return 0;
}
bool configure(ResourceFinder &rf)
{
printf(" Starting Configure\n");
this->rf=&rf;
//****************** PARAMETERS ******************
robot = rf.check("robot",Value("icub")).asString().c_str();
name = rf.check("name",Value("demoINNOROBO")).asString().c_str();
arm = rf.check("arm",Value("right_arm")).asString().c_str();
verbosity = rf.check("verbosity",Value(0)).asInt();
rate = rf.check("rate",Value(0.5)).asDouble();
if (arm!="right_arm" && arm!="left_arm")
{
printMessage(0,"ERROR: arm was set to %s, putting it to right_arm\n",arm.c_str());
arm="right_arm";
}
printMessage(1,"Parameters correctly acquired\n");
printMessage(1,"Robot: %s \tName: %s \tArm: %s\n",robot.c_str(),name.c_str(),arm.c_str());
printMessage(1,"Verbosity: %i \t Rate: %g \n",verbosity,rate);
//****************** DETECTOR ******************
certainty = rf.check("certainty", Value(10.0)).asInt();
strCascade = rf.check("cascade", Value("haarcascade_frontalface_alt.xml")).asString().c_str();
detectorL=new Detector();
detectorL->open(certainty,strCascade);
detectorR=new Detector();
detectorR->open(certainty,strCascade);
printMessage(1,"Detectors opened\n");
//****************** DRIVERS ******************
Property optionArm("(device remote_controlboard)");
optionArm.put("remote",("/"+robot+"/"+arm).c_str());
optionArm.put("local",("/"+name+"/"+arm).c_str());
if (!drvArm.open(optionArm))
{
printf("Position controller not available!\n");
return false;
}
Property optionCart("(device cartesiancontrollerclient)");
optionCart.put("remote",("/"+robot+"/cartesianController/"+arm).c_str());
optionCart.put("local",("/"+name+"/cart/").c_str());
if (!drvCart.open(optionCart))
{
printf("Cartesian controller not available!\n");
// return false;
}
Property optionGaze("(device gazecontrollerclient)");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optionGaze))
{
printf("Gaze controller not available!\n");
// return false;
}
// quitting conditions
bool andArm=drvArm.isValid()==drvCart.isValid()==drvGaze.isValid();
if (!andArm)
{
printMessage(0,"Something wrong occured while configuring drivers... quitting!\n");
return false;
}
drvArm.view(iencs);
drvArm.view(iposs);
drvArm.view(ictrl);
drvArm.view(iimp);
drvCart.view(iarm);
drvGaze.view(igaze);
iencs->getAxes(&nEncs);
iimp->setImpedance(0, 0.4, 0.03);
iimp->setImpedance(1, 0.35, 0.03);
iimp->setImpedance(2, 0.35, 0.03);
iimp->setImpedance(3, 0.2, 0.02);
iimp->setImpedance(4, 0.2, 0.00);
ictrl -> setImpedancePositionMode(0);
ictrl -> setImpedancePositionMode(1);
ictrl -> setImpedancePositionMode(2);
ictrl -> setImpedancePositionMode(3);
ictrl -> setImpedancePositionMode(2);
ictrl -> setImpedancePositionMode(4);
igaze -> storeContext(&contextGaze);
igaze -> setSaccadesStatus(false);
igaze -> setNeckTrajTime(0.75);
igaze -> setEyesTrajTime(0.5);
iarm -> storeContext(&contextCart);
printMessage(1,"Drivers opened\n");
//****************** PORTS ******************
imagePortInR -> open(("/"+name+"/imageR:i").c_str());
imagePortInL -> open(("/"+name+"/imageL:i").c_str());
imagePortOutR.open(("/"+name+"/imageR:o").c_str());
imagePortOutL.open(("/"+name+"/imageL:o").c_str());
portOutInfo.open(("/"+name+"/info:o").c_str());
if (robot=="icub")
{
Network::connect("/icub/camcalib/left/out",("/"+name+"/imageL:i").c_str());
Network::connect("/icub/camcalib/right/out",("/"+name+"/imageR:i").c_str());
}
else
{
Network::connect("/icubSim/cam/left",("/"+name+"/imageL:i").c_str());
Network::connect("/icubSim/cam/right",("/"+name+"/imageR:i").c_str());
}
Network::connect(("/"+name+"/imageL:o").c_str(),"/demoINN/left");
Network::connect(("/"+name+"/imageR:o").c_str(),"/demoINN/right");
Network::connect(("/"+name+"/info:o").c_str(),"/iSpeak");
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
printMessage(0,"Configure Finished!\n");
return true;
}
void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint,
const double min, const double max)
{
printMessage(1,"calibrating finger %s ...\n",finger.getName().c_str());
double margin=0.1*(max-min);
double _min=min+margin;
double _max=max-margin;
double tol_min=5.0;
double tol_max=5.0;
double *val=&_min;
double *tol=&tol_min;
double timeout=2.0*(_max-_min)/finger.getCalibVel();
mutex.lock();
IControlMode2 *imod; driver.view(imod);
IEncoders *ienc; driver.view(ienc);
IPositionControl *ipos; driver.view(ipos);
mutex.unlock();
// workaround
if ((finger.getName()=="ring") || (finger.getName()=="little"))
{
_min=30.0;
_max=180.0;
tol_min=20.0;
tol_max=50.0;
}
Property reset("(reset)");
Property feed("(feed)");
Property train("(train)");
finger.calibrate(reset);
mutex.lock();
imod->setControlMode(joint,VOCAB_CM_POSITION);
ipos->setRefSpeed(joint,finger.getCalibVel());
mutex.unlock();
for (int i=0; i<5; i++)
{
mutex.lock();
ipos->positionMove(joint,*val);
mutex.unlock();
bool done=false;
double fbOld=1e9;
double t0=Time::now();
while (!done)
{
Time::delay(0.01);
mutex.lock();
double fb; ienc->getEncoder(joint,&fb);
mutex.unlock();
if (fabs(fb-fbOld)>0.5)
{
printMessage(2,"feeding finger %s\n",finger.getName().c_str());
finger.calibrate(feed);
}
done=(fabs(*val-fb)<*tol)||(Time::now()-t0>timeout);
fbOld=fb;
}
if (val==&_min)
{
val=&_max;
tol=&tol_max;
}
else
{
val=&_min;
tol=&tol_min;
}
}
printMessage(1,"training finger %s ...\n",finger.getName().c_str());
finger.calibrate(train);
printMessage(1,"finger %s trained!\n",finger.getName().c_str());
}
