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);
}
Exemple #6
0
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);
}
Exemple #9
0
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;
}
Exemple #10
0
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;
}