void doubleTouchThread::goToTaxel(string s="standard")
{
cntctH0 = findH0(cntctSkin);
if (s == "waypoint")
{
printMessage(0,"H0: \n%s\n",cntctH0.toString().c_str());
Matrix safetyPoint = eye(4);
safetyPoint(0,3) = 0.02; // let's move 2 cm from the cover
cntctH0 = cntctH0 * safetyPoint;
}
probl->limb.setH0(SE3inv(cntctH0));
testLimb.setH0(SE3inv(cntctH0));
Vector sol;
if (s == "waypoint")
{
probl->limb.setAng(g->joints);
slv->setInitialGuess(*g);
slv->solve(*s0);
s0->print();
sol = CTRL_RAD2DEG * s0->joints;
}
else
{
probl->limb.setAng(s0->joints);
slv->setInitialGuess(*s0);
slv->solve(*s1);
/* s1->print();*/
sol = CTRL_RAD2DEG * s1->joints;
}
iposR -> positionMove(0,sol[5]);
iposR -> positionMove(1,sol[6]);
iposR -> positionMove(2,sol[7]);
iposR -> positionMove(3,sol[8]);
iposR -> positionMove(4,sol[9]);
iposR -> positionMove(5,sol[10]);
iposR -> positionMove(6,sol[11]);
delay(3);
iposL -> positionMove(4,-sol[0]);
iposL -> positionMove(3,-sol[1]);
iposL -> positionMove(2,-sol[2]);
iposL -> positionMove(1,-sol[3]);
iposL -> positionMove(0,-sol[4]);
testLimb.setAng(s1->joints);
}
void doubleTouchThread::solveIK()
{
printMessage(2,"H0: \n%s\n",cntctH0.toString(3,3).c_str());
slv->probl->limb.setH0(SE3inv(cntctH0));
testLimb->setH0(SE3inv(cntctH0));
slv->probl->limb.setAng(sol->joints);
slv->setInitialGuess(*sol);
slv->solve(*sol);
// sol->print();
solution=iCub::ctrl::CTRL_RAD2DEG * sol->joints;
testLimb->setAng(sol->joints);
}
void Controller::findMinimumAllowedVergence()
{
iKinChain cl(*chainEyeL), cr(*chainEyeR);
Vector zeros(cl.getDOF(),0.0);
cl.setAng(zeros); cr.setAng(zeros);
double minVer=startupMinVer;
double maxVer=lim(nJointsHead-1,1);
for (double ver=minVer; ver<maxVer; ver+=0.5*CTRL_DEG2RAD)
{
cl(cl.getDOF()-1).setAng(ver/2.0);
cr(cr.getDOF()-1).setAng(-ver/2.0);
Vector fp(4);
fp[3]=1.0; // impose homogeneous coordinates
if (CartesianHelper::computeFixationPointData(cl,cr,fp))
{
// if the component along eye's z-axis is positive
// then this means that the fixation point is ok,
// being in front of the robot
Vector fpe=SE3inv(cl.getH())*fp;
if (fpe[2]>0.0)
{
minVer=ver;
break;
}
}
}
yInfo("### computed minimum allowed vergence = %g [deg]",minVer*CTRL_RAD2DEG);
commData->get_minAllowedVergence()=minVer;
}
void singlePlanner::init()
{
//**** visualizing targets and collision points in simulator ***************************
if (running_mode=="single")
{
string port2icubsim = "/" + name + "/" + controlPoint + "/sim:o";
//cout<<port2icubsim<<endl;
if (!portToSimWorld.open(port2icubsim.c_str())) {
yError("Unable to open port << port2icubsim << endl");
}
std::string port2world = "/icubSim/world";
yarp::os::Network::connect(port2icubsim, port2world.c_str());
if (controlPoint != "local-Half-Elbow")
{
cmd.clear();
cmd.addString("world");
cmd.addString("del");
cmd.addString("all");
portToSimWorld.write(cmd);
}
}
T_world_root = zeros(4,4);
T_world_root(0,1)=-1;
T_world_root(1,2)=1; T_world_root(1,3)=0.5976;
T_world_root(2,0)=-1; T_world_root(2,3)=-0.026;
T_world_root(3,3)=1;
T_root_world=SE3inv(T_world_root);
}
Vector Controller::computeNeckVelFromdxFP(const Vector &fp, const Vector &dfp)
{
// convert fp from root to the neck reference frame
Vector fpR=fp;
fpR.push_back(1.0);
Vector fpE=SE3inv(chainNeck->getH())*fpR;
// compute the Jacobian of the head joints alone
// (by adding the new fixation point beforehand)
Matrix HN=eye(4);
HN(0,3)=fpE[0];
HN(1,3)=fpE[1];
HN(2,3)=fpE[2];
mutexChain.lock();
chainNeck->setHN(HN);
Matrix JN=chainNeck->GeoJacobian();
chainNeck->setHN(eye(4,4));
mutexChain.unlock();
// take only the last three rows of the Jacobian
// belonging to the head joints
Matrix JNp=JN.submatrix(3,5,3,5);
return pinv(JNp)*dfp.subVector(3,5);
}
double Solver::neckTargetRotAngle(const Vector &xd)
{
Matrix H=commData->get_fpFrame();
Vector xdh=xd; xdh.push_back(1.0);
xdh=SE3inv(H)*xdh; xdh[3]=0.0;
return (CTRL_RAD2DEG*acos(xdh[2]/norm(xdh)));
}
bool Localizer::projectPoint(const string &type, const Vector &x, Vector &px)
{
if (x.length()<3)
{
yError("Not enough values given for the point!");
return false;
}
bool isLeft=(type=="left");
Matrix *Prj=(isLeft?PrjL:PrjR);
iCubEye *eye=(isLeft?eyeL:eyeR);
if (Prj)
{
Vector torso=commData->get_torso();
Vector head=commData->get_q();
Vector q(8);
q[0]=torso[0];
q[1]=torso[1];
q[2]=torso[2];
q[3]=head[0];
q[4]=head[1];
q[5]=head[2];
q[6]=head[3];
if (isLeft)
q[7]=head[4]+head[5]/2.0;
else
q[7]=head[4]-head[5]/2.0;
Vector xo=x;
if (xo.length()<4)
xo.push_back(1.0); // impose homogeneous coordinates
// find position wrt the camera frame
mutex.lock();
Vector xe=SE3inv(eye->getH(q))*xo;
mutex.unlock();
// find the 2D projection
px=*Prj*xe;
px=px/px[2];
px.pop_back();
return true;
}
else
{
yError("Unspecified projection matrix for %s camera!",type.c_str());
return false;
}
}
void EyeTableProjection::setBaseTransformation(const ::yarp::sig::Vector& torso3d,
const ::yarp::sig::Vector& head6d) {
Vector v(8);
// order according to http://wiki.icub.org/wiki/ICubHeadKinematics
v[0] = torso3d[2]; // pitch
v[1] = torso3d[1]; // roll
v[2] = torso3d[0]; // yaw
v[3] = head6d[0]; // neck pitch
v[4] = head6d[1]; // neck roll
v[5] = head6d[2]; // neck yaw
v[6] = head6d[3]; // tilt of the eye's
v[7] = camera == "left" ? head6d[4] + head6d[5] / 2 : head6d[4] - head6d[5] / 2;
// eye -> robot
Matrix T = SE3inv(eye.getH(v));
SimpleHomography::setBaseTransformation(T);
}
Vector Solver::computeTargetUserTolerance(const Vector &xd)
{
Matrix H=commData->get_fpFrame();
Vector z(3,0.0); z[2]=1.0;
Vector xdh=xd; xdh.push_back(1.0);
xdh=SE3inv(H)*xdh;
Vector xdh3=xdh; xdh3.pop_back();
Vector rot=cross(xdh3,z);
double r=norm(rot);
if (r<IKIN_ALMOST_ZERO)
return xd;
rot=rot/r;
rot.push_back(neckAngleUserTolerance*CTRL_DEG2RAD);
rot=H*(axis2dcm(rot)*xdh);
rot.pop_back();
return rot;
}
void EyePinvRefGen::run()
{
if (genOn)
{
LockGuard guard(mutex);
double timeStamp;
// read encoders
getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&timeStamp);
updateTorsoBlockedJoints(chainNeck,fbTorso);
updateTorsoBlockedJoints(chainEyeL,fbTorso);
updateTorsoBlockedJoints(chainEyeR,fbTorso);
// get current target
Vector xd=commData->port_xd->get_xd();
// update neck chain
chainNeck->setAng(nJointsTorso+0,fbHead[0]);
chainNeck->setAng(nJointsTorso+1,fbHead[1]);
chainNeck->setAng(nJointsTorso+2,fbHead[2]);
// ask for saccades (if possible)
if (commData->saccadesOn && (saccadesRxTargets!=commData->port_xd->get_rx()) &&
!commData->saccadeUnderway && (Time::now()-saccadesClock>commData->saccadesInhibitionPeriod))
{
Vector fph=xd; fph.push_back(1.0);
fph=SE3inv(chainNeck->getH())*fph; fph[3]=0.0;
double rot=CTRL_RAD2DEG*acos(fph[2]/norm(fph)); fph[3]=1.0;
// estimate geometrically the target tilt and pan of the eyes
Vector ang(3,0.0);
ang[0]=-atan2(fph[1],fabs(fph[2]));
ang[1]=atan2(fph[0],fph[2]);
// enforce joints bounds
ang[0]=sat(ang[0],lim(0,0),lim(0,1));
ang[1]=sat(ang[1],lim(1,0),lim(1,1));
// favor the smooth-pursuit in case saccades are small
if (rot>commData->saccadesActivationAngle)
{
// init vergence
ang[2]=fbHead[5];
// get rid of eyes tilt
Vector axis(4);
axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; axis[3]=-ang[0];
fph=axis2dcm(axis)*fph;
// go on iff the point is in front of us
if (fph[2]>0.0)
{
double L,R;
// estimate geometrically the target vergence Vg=L-R
if (fph[0]>=eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=M_PI/2.0-atan2(fph[2],fph[0]-eyesHalfBaseline);
}
else if (fph[0]>-eyesHalfBaseline)
{
L=M_PI/2.0-atan2(fph[2],fph[0]+eyesHalfBaseline);
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
else
{
L=-(M_PI/2.0-atan2(fph[2],-fph[0]-eyesHalfBaseline));
R=-(M_PI/2.0-atan2(fph[2],eyesHalfBaseline-fph[0]));
}
ang[2]=L-R;
}
// enforce joints bounds
ang[2]=sat(ang[2],lim(2,0),lim(2,1));
commData->set_qd(3,ang[0]);
commData->set_qd(4,ang[1]);
commData->set_qd(5,ang[2]);
Vector vel(3,SACCADES_VEL);
ctrl->doSaccade(ang,vel);
saccadesClock=Time::now();
}
}
// update eyes chains for convergence purpose
updateNeckBlockedJoints(chainEyeL,fbHead); updateNeckBlockedJoints(chainEyeR,fbHead);
chainEyeL->setAng(nJointsTorso+3,qd[0]); chainEyeR->setAng(nJointsTorso+3,qd[0]);
chainEyeL->setAng(nJointsTorso+4,qd[1]+qd[2]/2.0); chainEyeR->setAng(nJointsTorso+4,qd[1]-qd[2]/2.0);
// converge on target
if (CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
{
Vector v=EYEPINVREFGEN_GAIN*(pinv(eyesJ)*(xd-fp));
// update eyes chains in actual configuration for velocity compensation
chainEyeL->setAng(nJointsTorso+3,fbHead[3]); chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0); chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);
// compensate neck rotation at eyes level
if ((commData->eyesBoundVer>=0.0) || !CartesianHelper::computeFixationPointData(*chainEyeL,*chainEyeR,fp,eyesJ))
commData->set_counterv(zeros(qd.length()));
else
commData->set_counterv(getEyesCounterVelocity(eyesJ,fp));
// reset eyes controller and integral upon saccades transition on=>off
if (saccadeUnderWayOld && !commData->saccadeUnderway)
{
ctrl->resetCtrlEyes();
qd[0]=fbHead[3];
qd[1]=fbHead[4];
qd[2]=fbHead[5];
I->reset(qd);
}
// update reference
qd=I->integrate(v+commData->get_counterv());
}
else
commData->set_counterv(zeros(qd.length()));
// set a new target position
commData->set_xd(xd);
commData->set_x(fp,timeStamp);
commData->set_fpFrame(chainNeck->getH());
if (!commData->saccadeUnderway)
{
commData->set_qd(3,qd[0]);
commData->set_qd(4,qd[1]);
commData->set_qd(5,qd[2]);
}
// latch the saccades status
saccadeUnderWayOld=commData->saccadeUnderway;
saccadesRxTargets=commData->port_xd->get_rx();
}
}
bool Localizer::triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x)
{
if ((pxl.length()<2) || (pxr.length()<2))
{
yError("Not enough values given for the pixels!");
return false;
}
if (PrjL && PrjR)
{
Vector torso=commData->get_torso();
Vector head=commData->get_q();
Vector qL(8);
qL[0]=torso[0];
qL[1]=torso[1];
qL[2]=torso[2];
qL[3]=head[0];
qL[4]=head[1];
qL[5]=head[2];
qL[6]=head[3];
qL[7]=head[4]+head[5]/2.0;
Vector qR=qL;
qR[7]-=head[5];
mutex.lock();
Matrix HL=SE3inv(eyeL->getH(qL));
Matrix HR=SE3inv(eyeR->getH(qR));
mutex.unlock();
Matrix tmp=zeros(3,4); tmp(2,2)=1.0;
tmp(0,2)=pxl[0]; tmp(1,2)=pxl[1];
Matrix AL=(*PrjL-tmp)*HL;
tmp(0,2)=pxr[0]; tmp(1,2)=pxr[1];
Matrix AR=(*PrjR-tmp)*HR;
Matrix A(4,3);
Vector b(4);
for (int i=0; i<2; i++)
{
b[i]=-AL(i,3);
b[i+2]=-AR(i,3);
for (int j=0; j<3; j++)
{
A(i,j)=AL(i,j);
A(i+2,j)=AR(i,j);
}
}
// solve the least-squares problem
x=pinv(A)*b;
return true;
}
else
{
yError("Unspecified projection matrix for at least one camera!");
return false;
}
}
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;
}
Vector Localizer::get3DPoint(const string &type, const Vector &ang)
{
double azi=ang[0];
double ele=ang[1];
double ver=ang[2];
Vector q(8,0.0);
if (type=="rel")
{
Vector torso=commData->get_torso();
Vector head=commData->get_q();
q[0]=torso[0];
q[1]=torso[1];
q[2]=torso[2];
q[3]=head[0];
q[4]=head[1];
q[5]=head[2];
q[6]=head[3];
q[7]=head[4];
ver+=head[5];
}
// impose vergence != 0.0
if (ver<commData->get_minAllowedVergence())
ver=commData->get_minAllowedVergence();
mutex.lock();
q[7]+=ver/2.0;
eyeL->setAng(q);
q[7]-=ver;
eyeR->setAng(q);
Vector fp(4);
fp[3]=1.0; // impose homogeneous coordinates
// compute new fp due to changed vergence
CartesianHelper::computeFixationPointData(*(eyeL->asChain()),*(eyeR->asChain()),fp);
mutex.unlock();
// compute rotational matrix to
// account for elevation and azimuth
Vector x(4), y(4);
x[0]=1.0; y[0]=0.0;
x[1]=0.0; y[1]=1.0;
x[2]=0.0; y[2]=0.0;
x[3]=ele; y[3]=azi;
Matrix R=axis2dcm(y)*axis2dcm(x);
Vector fph, xd;
if (type=="rel")
{
Matrix frame=commData->get_fpFrame();
fph=SE3inv(frame)*fp; // get fp wrt relative head-centered frame
xd=frame*(R*fph); // apply rotation and retrieve fp wrt root frame
}
else
{
fph=invEyeCAbsFrame*fp; // get fp wrt absolute head-centered frame
xd=eyeCAbsFrame*(R*fph); // apply rotation and retrieve fp wrt root frame
}
xd.pop_back();
return xd;
}