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