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