//********************************************
bool close()
{
yInfo("[demoAvoidance] Closing module..");
dataPort.close();
if (useLeftArm)
{
if (driverCartL.isValid())
{
data["left"].iarm->stopControl();
data["left"].iarm->restoreContext(contextL);
driverCartL.close();
}
if (driverJointL.isValid())
{
IInteractionMode *imode;
driverJointL.view(imode);
for (int j=0; j<5; j++)
imode->setInteractionMode(j,VOCAB_IM_STIFF);
driverJointL.close();
}
}
if (useRightArm)
{
if (driverCartR.isValid())
{
data["right"].iarm->stopControl();
data["right"].iarm->restoreContext(contextR);
driverCartR.close();
}
if (driverJointR.isValid())
{
IInteractionMode *imode;
driverJointR.view(imode);
for (int j=0; j<5; j++)
imode->setInteractionMode(j,VOCAB_IM_STIFF);
driverJointR.close();
}
}
return true;
}
//********************************************
bool close()
{
dataPort.close();
if (driverCartL.isValid())
{
data["left"].iarm->stopControl();
data["left"].iarm->restoreContext(contextL);
driverCartL.close();
}
if (driverCartR.isValid())
{
data["right"].iarm->stopControl();
data["right"].iarm->restoreContext(contextR);
driverCartR.close();
}
if (driverJointL.isValid())
{
IInteractionMode *imode;
driverJointL.view(imode);
for (int j=0; j<5; j++)
imode->setInteractionMode(j,VOCAB_IM_STIFF);
driverJointL.close();
}
if (driverJointR.isValid())
{
IInteractionMode *imode;
driverJointR.view(imode);
for (int j=0; j<5; j++)
imode->setInteractionMode(j,VOCAB_IM_STIFF);
driverJointR.close();
}
return true;
}
bool close()
{
// Terminate model
Controller_terminate(Controller_M);
imod->setControlMode(joint,VOCAB_CM_POSITION);
if (compliant)
iint->setInteractionMode(joint,VOCAB_IM_STIFF);
rpc.close();
dataOut.close();
dataIn.close();
drv.close();
return true;
}
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;
}
//********************************************
bool configure(ResourceFinder &rf)
{
if (rf.check("noLeftArm") && rf.check("noRightArm"))
{
yError("[demoAvoidance] no arm has been selected. Closing.");
return false;
}
useLeftArm = false;
useRightArm = false;
string name=rf.check("name",Value("avoidance")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
motionGain = -1.0;
if (rf.check("catching"))
{
motionGain = 1.0;
}
if (motionGain!=-1.0)
{
yWarning("[demoAvoidance] motionGain set to catching");
}
bool autoConnect=rf.check("autoConnect");
if (autoConnect)
{
yWarning("[demoAvoidance] Autoconnect mode set to ON");
}
bool stiff=rf.check("stiff");
if (stiff)
{
yInfo("[demoAvoidance] Stiff Mode enabled.");
}
Matrix R(4,4);
R(0,0)=-1.0; R(2,1)=-1.0; R(1,2)=-1.0; R(3,3)=1.0;
if (!rf.check("noLeftArm"))
{
useLeftArm=true;
data["left"]=Data();
data["left"].home_x[0]=-0.30;
data["left"].home_x[1]=-0.20;
data["left"].home_x[2]=+0.05;
data["left"].home_o=dcm2axis(R);
Property optionCartL;
optionCartL.put("device","cartesiancontrollerclient");
optionCartL.put("remote","/"+robot+"/cartesianController/left_arm");
optionCartL.put("local",("/"+name+"/cart/left_arm").c_str());
if (!driverCartL.open(optionCartL))
{
close();
return false;
}
Property optionJointL;
optionJointL.put("device","remote_controlboard");
optionJointL.put("remote","/"+robot+"/left_arm");
optionJointL.put("local",("/"+name+"/joint/left_arm").c_str());
if (!driverJointL.open(optionJointL))
{
close();
return false;
}
driverCartL.view(data["left"].iarm);
data["left"].iarm->storeContext(&contextL);
Vector dof;
data["left"].iarm->getDOF(dof);
dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0;
data["left"].iarm->setDOF(dof,dof);
data["left"].iarm->setTrajTime(0.9);
data["left"].iarm->goToPoseSync(data["left"].home_x,data["left"].home_o);
data["left"].iarm->waitMotionDone();
IInteractionMode *imode; driverJointL.view(imode);
IImpedanceControl *iimp; driverJointL.view(iimp);
if (!stiff)
{
imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03);
imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03);
imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03);
imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01);
imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0);
}
}
if (!rf.check("noRightArm"))
{
useRightArm = true;
data["right"]=Data();
data["right"].home_x[0]=-0.30;
data["right"].home_x[1]=+0.20;
data["right"].home_x[2]=+0.05;
data["right"].home_o=dcm2axis(R);
Property optionCartR;
optionCartR.put("device","cartesiancontrollerclient");
optionCartR.put("remote","/"+robot+"/cartesianController/right_arm");
optionCartR.put("local",("/"+name+"/cart/right_arm").c_str());
if (!driverCartR.open(optionCartR))
{
close();
return false;
}
Property optionJointR;
optionJointR.put("device","remote_controlboard");
optionJointR.put("remote","/"+robot+"/right_arm");
optionJointR.put("local",("/"+name+"/joint/right_arm").c_str());
if (!driverJointR.open(optionJointR))
{
close();
return false;
}
driverCartR.view(data["right"].iarm);
data["right"].iarm->storeContext(&contextR);
Vector dof;
data["right"].iarm->getDOF(dof);
dof=0.0; dof[3]=dof[4]=dof[5]=dof[6]=1.0;
data["right"].iarm->setDOF(dof,dof);
data["right"].iarm->setTrajTime(0.9);
data["right"].iarm->goToPoseSync(data["right"].home_x,data["right"].home_o);
data["right"].iarm->waitMotionDone();
IInteractionMode *imode; driverJointR.view(imode);
IImpedanceControl *iimp; driverJointR.view(iimp);
if (!stiff)
{
imode->setInteractionMode(0,VOCAB_IM_COMPLIANT); iimp->setImpedance(0,0.4,0.03);
imode->setInteractionMode(1,VOCAB_IM_COMPLIANT); iimp->setImpedance(1,0.4,0.03);
imode->setInteractionMode(2,VOCAB_IM_COMPLIANT); iimp->setImpedance(2,0.4,0.03);
imode->setInteractionMode(3,VOCAB_IM_COMPLIANT); iimp->setImpedance(3,0.2,0.01);
imode->setInteractionMode(4,VOCAB_IM_COMPLIANT); iimp->setImpedance(4,0.2,0.0);
}
}
dataPort.open(("/"+name+"/data:i").c_str());
dataPort.setReader(*this);
if (autoConnect)
{
Network::connect("/visuoTactileRF/skin_events:o",("/"+name+"/data:i").c_str());
}
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
return true;
}
bool configure(ResourceFinder &rf)
{
Time::turboBoost();
string name=rf.check("name",Value("yarpMinJerk")).asString().c_str();
string robot=rf.check("robot",Value("icubSim")).asString().c_str();
string part=rf.check("part",Value("left_arm")).asString().c_str();
joint=rf.check("joint",Value(3)).asInt();
compliant=rf.check("compliant");
Property option;
option.put("device","remote_controlboard");
option.put("remote",("/"+robot+"/"+part).c_str());
option.put("local",("/"+name+"/"+part).c_str());
if (!drv.open(option))
return false;
drv.view(imod);
drv.view(iint);
drv.view(ienc);
drv.view(ivel);
imod->setControlMode(joint,VOCAB_CM_VELOCITY);
if (compliant)
iint->setInteractionMode(joint,VOCAB_IM_COMPLIANT);
IControlLimits *ilim; drv.view(ilim);
double min_joint,max_joint;
ilim->getLimits(joint,&min_joint,&max_joint);
double enc;
while (!ienc->getEncoder(joint,&enc))
Time::delay(0.1);
Controller_P.Plant_IC=enc;
Controller_P.Plant_Max=max_joint;
Controller_P.Plant_Min=min_joint;
Controller_P.AutoCompensator_ThresHystMax=0.5;
Controller_P.AutoCompensator_ThresHystMin=0.1;
yInfo("enc=%g in [%g, %g] deg",enc,min_joint,max_joint);
// Pack model data into RTM
Controller_M->ModelData.defaultParam = &Controller_P;
Controller_M->ModelData.blockIO = &Controller_B;
Controller_M->ModelData.dwork = &Controller_DW;
// Initialize model
Controller_initialize(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);
Controller_U_reference=enc;
Controller_U_compensator_state=CompensatorState::Off;
Controller_U_plant_output=enc;
dataIn.open(("/"+name+"/data:i").c_str());
dataOut.open(("/"+name+"/data:o").c_str());
rpc.open(("/"+name+"/rpc").c_str());
attach(rpc);
return true;
}
