bool doubleTouchThread::selectTask()
{
switch(cntctSkinPart)
{
case SKIN_LEFT_HAND:
curTaskType = "LHtoR";
break;
case SKIN_LEFT_FOREARM:
curTaskType = "LtoR";
break;
case SKIN_RIGHT_HAND:
curTaskType = "RHtoL";
break;
case SKIN_RIGHT_FOREARM:
curTaskType = "RtoL";
break;
}
slv = new doubleTouch_Solver(curTaskType);
gue = new doubleTouch_Variables(slv->probl->getNVars()); // guess
sol = new doubleTouch_Variables(slv->probl->getNVars()); // solution
solution.resize(slv->probl->getNVars(),0.0);
nDOF = solution.size();
if (curTaskType == "LtoR" || curTaskType == "RtoL")
{
gue->joints[1] = -armPossHome[3]; gue->joints[3] = -armPossHome[1];
gue->joints[4] = -armPossHome[0]; gue->joints[5] = -armPossHome[0];
gue->joints[6] = armPossHome[1]; gue->joints[8] = armPossHome[3];
}
else if (curTaskType == "LHtoR" || curTaskType == "RHtoL")
{
gue->joints[1+2] = -armPossHome[3]; gue->joints[3+2] = -armPossHome[1];
gue->joints[4+2] = -armPossHome[0]; gue->joints[5+2] = armPossHome[0];
gue->joints[6+2] = armPossHome[1]; gue->joints[8+2] = armPossHome[3];
}
sol->clone(*gue);
slv->probl->limb.setAng(gue->joints);
testLimb = new iCubCustomLimb(curTaskType);
if (curTaskType=="LHtoR" || curTaskType=="LtoR")
{
iencsM = iencsR;
imodeM = imodeR;
iposM = iposR;
encsM = encsR;
ilimM = ilimR;
jntsM = jntsR;
armM = armR;
iencsS = iencsL;
imodeS = imodeL;
iposS = iposL;
encsS = encsL;
ilimS = ilimL;
jntsS = jntsL;
armS = armL;
}
else if (curTaskType=="RHtoL" || curTaskType=="RtoL")
{
iencsM = iencsL;
imodeM = imodeL;
iposM = iposL;
encsM = encsL;
ilimM = ilimL;
jntsM = jntsL;
armM = armL;
iencsS = iencsR;
imodeS = imodeR;
iposS = iposR;
encsS = encsR;
ilimS = ilimR;
jntsS = jntsR;
armS = armR;
}
else
{
yError("[doubleTouch] current task type is none of the admissible values!");
return false;
}
if (!alignJointsBounds())
{
yError("[doubleTouch] alignJointsBounds failed!!!\n");
return false;
}
Vector joints;
iencsM->getEncoders(encsM->data());
slv->probl->index.getChainJoints(*encsM,joints);
Matrix HIndex=slv->probl->index.getH(joints*iCub::ctrl::CTRL_DEG2RAD);
slv->probl->limb.setHN(HIndex);
testLimb->setHN(HIndex);
printMessage(1,"Index type: %s \t HIndex:\n%s\n", slv->probl->index.getType().c_str(),
HIndex.toString(3,3).c_str());
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;
}
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 doubleTouchThread::threadInit()
{
cntctRdr -> open(("/"+name+"/contacts:i").c_str());
Property OptGaze;
OptGaze.put("device","gazecontrollerclient");
OptGaze.put("remote","/iKinGazeCtrl");
OptGaze.put("local",("/"+name+"/gaze").c_str());
Property OptR;
OptR.put("robot", robot.c_str());
OptR.put("part", "right_arm");
OptR.put("device", "remote_controlboard");
OptR.put("remote",("/"+robot+"/right_arm").c_str());
OptR.put("local", ("/"+name+"/posCtrl/right_arm").c_str());
Property OptL;
OptL.put("robot", robot.c_str());
OptL.put("part", "left_arm");
OptL.put("device", "remote_controlboard");
OptL.put("remote",("/"+robot+"/left_arm").c_str());
OptL.put("local", ("/"+name+"/posCtrl/left_arm").c_str());
if ((!ddG.open(OptGaze)) || (!ddG.view(igaze))){
printMessage(0,"Error: could not open the Gaze Controller!\n");
return false;
}
igaze -> storeContext(&contextGaze);
igaze -> setNeckTrajTime(1.5);
igaze -> setEyesTrajTime(0.5);
igaze -> setSaccadesStatus(false);
if (!ddR.open(OptR))
{
printMessage(0,"ERROR: could not open right_arm PolyDriver!\n");
return false;
}
// open the view
bool ok = 1;
if (ddR.isValid())
{
ok = ok && ddR.view(iencsR);
ok = ok && ddR.view(iposR);
ok = ok && ddR.view(ictrlR);
ok = ok && ddR.view(ilimR);
}
if (!ok)
{
printMessage(0,"\nERROR: Problems acquiring right_arm interfaces!!!!\n");
return false;
}
iencsR->getAxes(&jntsR);
encsR = new Vector(jntsR,0.0);
if (!ddL.open(OptL))
{
printMessage(0,"ERROR: could not open left_arm PolyDriver!\n");
return false;
}
// open the view
ok = 1;
if (ddL.isValid())
{
ok = ok && ddL.view(iencsL);
ok = ok && ddL.view(iposL);
ok = ok && ddL.view(ictrlL);
ok = ok && ddL.view(ilimL);
}
if (!ok)
{
printMessage(0,"\nERROR: Problems acquiring left_arm interfaces!!!!\n");
return false;
}
iencsL->getAxes(&jntsL);
encsL = new Vector(jntsL,0.0);
alignJointsBounds();
Vector joints;
iencsR->getEncoders(encsR->data());
probl->index.getChainJoints(*encsR,joints);
Matrix indexH=probl->index.getH(joints*CTRL_DEG2RAD);
printf("indexH:\n%s\n", indexH.toString().c_str());
probl->limb.setHN(indexH);
testLimb.setHN(indexH);
return true;
}
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;
}
