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;
}
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;
}
Localizer::Localizer(exchangeData *_commData, const unsigned int _period) :
RateThread(_period), commData(_commData), period(_period)
{
iCubHeadCenter eyeC(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");
// remove constraints on the links
// we use the chains for logging purpose
eyeL->setAllConstraints(false);
eyeR->setAllConstraints(false);
// release links
eyeL->releaseLink(0); eyeC.releaseLink(0); eyeR->releaseLink(0);
eyeL->releaseLink(1); eyeC.releaseLink(1); eyeR->releaseLink(1);
eyeL->releaseLink(2); eyeC.releaseLink(2); eyeR->releaseLink(2);
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
}
// get the absolute reference frame of the head
Vector q(eyeC.getDOF(),0.0);
eyeCAbsFrame=eyeC.getH(q);
// ... and its inverse
invEyeCAbsFrame=SE3inv(eyeCAbsFrame);
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
bool ret;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_LEFT",&PrjL,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_LEFT",&Prj,true))
{
delete PrjL;
PrjL=Prj;
}
}
if (ret)
{
cxl=(*PrjL)(0,2);
cyl=(*PrjL)(1,2);
invPrjL=new Matrix(pinv(PrjL->transposed()).transposed());
}
else
PrjL=invPrjL=NULL;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_RIGHT",&PrjR,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_RIGHT",&Prj,true))
{
delete PrjR;
PrjR=Prj;
}
}
if (ret)
{
cxr=(*PrjR)(0,2);
cyr=(*PrjR)(1,2);
invPrjR=new Matrix(pinv(PrjR->transposed()).transposed());
}
else
PrjR=invPrjR=NULL;
Vector Kp(1,0.001), Ki(1,0.001), Kd(1,0.0);
Vector Wp(1,1.0), Wi(1,1.0), Wd(1,1.0);
Vector N(1,10.0), Tt(1,1.0);
Matrix satLim(1,2);
satLim(0,0)=0.05;
satLim(0,1)=10.0;
pid=new parallelPID(0.05,Kp,Ki,Kd,Wp,Wi,Wd,N,Tt,satLim);
Vector z0(1,0.5);
pid->reset(z0);
dominantEye="left";
port_xd=NULL;
}
