Example #1
0
void Solver::resume()
{
    mutex.lock();

    // read encoders
    getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
    updateTorsoBlockedJoints(chainNeck,fbTorso);
    updateTorsoBlockedJoints(chainEyeL,fbTorso);
    updateTorsoBlockedJoints(chainEyeR,fbTorso);        

    // update kinematics
    updateAngles();
    updateNeckBlockedJoints(chainEyeL,fbHead);
    updateNeckBlockedJoints(chainEyeR,fbHead);
    chainNeck->setAng(neckPos);
    chainEyeL->setAng(nJointsTorso+3,gazePos[0]);                chainEyeR->setAng(nJointsTorso+3,gazePos[0]);
    chainEyeL->setAng(nJointsTorso+4,gazePos[1]+gazePos[2]/2.0); chainEyeR->setAng(nJointsTorso+4,gazePos[1]-gazePos[2]/2.0);

    // update latched quantities
    fbTorsoOld=fbTorso;
    
    mutex.unlock();

    commData->port_xd->unlock();
    RateThread::resume();
    yInfo("Solver has been resumed!");
}
void Controller::resume()
{
    getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
    fbNeck=fbHead.subVector(0,2);
    fbEyes=fbHead.subVector(3,5);
    
    RateThread::resume();
    yInfo("Controller has been resumed!");
    notifyEvent("resumed");
}
Example #3
0
PUBLIC bool feedback(cchar *kind, cchar *fmt, ...)
{
    va_list     args;

    va_start(args, fmt);
    espSetFeedbackv(getStream(), kind, fmt, args);
    va_end(args);

    /*
        Return true if there is not an error feedback message
     */
    return getFeedback("error") == 0;
}
Example #4
0
void Controller::resume()
{
    if (Robotable)
    {
        getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
        fbNeck=fbHead.subVector(0,2);
        fbEyes=fbHead.subVector(3,5);
    }
    
    RateThread::resume();
    printf("Controller has been resumed!\n");
    notifyEvent("resumed");
}
    void jointStateCallback(const sensor_msgs::JointState::ConstPtr &msg) {
        size_t msgSize = msg->name.size(), jointInfoSize = _jointInfo.name.size(), msgSizeP = msg->position.size(),
                msgSizeV = msg->velocity.size(), msgSizeE = msg->effort.size();
        for (int i = 0; i < msgSize; i++) {
            for (int j = 0; j < jointInfoSize; j++) {
                if (msg->name[i] == _jointInfo.name[j]) {
                    if (i < msgSizeP) _jointInfo.position[j] = msg->position[i];
                    if (i < msgSizeV) _jointInfo.velocity[j] = msg->velocity[i];
                    if (i < msgSizeE) _jointInfo.effort[j] = msg->effort[i];
                }
            }
        }

        if(_publishFeedback) {
            size_t size = _feedback.joint_names.size();

            _feedback.actual.positions.clear();
            _feedback.actual.velocities.clear();
            _feedback.actual.effort.clear();

            _feedback.error.positions.clear();
            _feedback.error.velocities.clear();
            _feedback.error.effort.clear();

            for (int i = 0; i < size; ++i) {
                feedback_t singleJointFeedback = getFeedback(_feedback.joint_names[i], _feedback.desired.positions[i],
                                                             0.0,
                                                             _feedback.desired.velocities[i]);
                _feedback.actual.positions.push_back(singleJointFeedback.actual.position);
                _feedback.actual.velocities.push_back(singleJointFeedback.actual.velocity);
                _feedback.actual.effort.push_back(singleJointFeedback.actual.effort);

                _feedback.error.positions.push_back(singleJointFeedback.error.position);
                _feedback.error.velocities.push_back(singleJointFeedback.error.velocity);
                _feedback.error.effort.push_back(singleJointFeedback.error.effort);
            }
            _actionServer.publishFeedback(_feedback);
        }
    }
void Controller::run()
{
    LockGuard guard(mutexRun);
    
    mutexCtrl.lock();
    bool jointsHealthy=areJointsHealthyAndSet();
    mutexCtrl.unlock();

    if (!jointsHealthy)
    {
        stopControlHelper();
        port_xd->get_new()=false;
    }

    string event="none";

    // verify if any saccade is still underway
    mutexCtrl.lock();
    if (commData->get_isSaccadeUnderway() && (Time::now()-saccadeStartTime>=Ts))
    {
        bool done;
        posHead->checkMotionDone(eyesJoints.size(),eyesJoints.getFirst(),&done);
        commData->get_isSaccadeUnderway()=!done;

        if (!commData->get_isSaccadeUnderway())
            notifyEvent("saccade-done");
    }
    mutexCtrl.unlock();
    
    // get data
    double x_stamp;
    Vector xd=commData->get_xd();
    Vector x=commData->get_x(x_stamp);
    Vector new_qd=commData->get_qd();

    // read feedbacks
    q_stamp=Time::now();
    if (!getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&q_stamp))
    {
        yWarning("Communication timeout detected!");
        notifyEvent("comm-timeout");
        suspend();
        return;
    }

    IntState->reset(fbHead);

    fbNeck=fbHead.subVector(0,2);
    fbEyes=fbHead.subVector(3,5);

    double errNeck=norm(new_qd.subVector(0,2)-fbNeck);
    double errEyes=norm(new_qd.subVector(3,new_qd.length()-1)-fbEyes);
    bool swOffCond=(Time::now()-ctrlActiveRisingEdgeTime<GAZECTRL_SWOFFCOND_DISABLETIME) ? false :
                   (!commData->get_isSaccadeUnderway() &&
                   (errNeck<GAZECTRL_MOTIONDONE_NECK_QTHRES*CTRL_DEG2RAD) &&
                   (errEyes<GAZECTRL_MOTIONDONE_EYES_QTHRES*CTRL_DEG2RAD));

    // verify control switching conditions
    if (commData->get_isCtrlActive())
    {
        // switch-off condition
        if (swOffCond)
        {
            event="motion-done";

            mutexCtrl.lock();
            stopLimb(false);
            mutexCtrl.unlock();
        }
        // manage new target while controller is active
        else if (port_xd->get_new())
        {
            event="motion-onset";

            mutexData.lock();
            motionOngoingEventsCurrent=motionOngoingEvents;
            mutexData.unlock();
        }

        port_xd->get_new()=false;
    }
    else if (jointsHealthy)
    {
        // inhibition is cleared upon new target arrival
        if (ctrlInhibited)
            ctrlInhibited=!port_xd->get_new();

        // switch-on condition
        commData->get_isCtrlActive()=port_xd->get_new() ||
                                     (!ctrlInhibited &&
                                     (new_qd[0]!=qd[0]) || (new_qd[1]!=qd[1]) || (new_qd[2]!=qd[2]) ||
                                     (!commData->get_canCtrlBeDisabled() && (norm(port_xd->get_xd()-x)>GAZECTRL_MOTIONSTART_XTHRES)));

        // reset controllers
        if (commData->get_isCtrlActive())
        {
            ctrlActiveRisingEdgeTime=Time::now();
            port_xd->get_new()=false;

            Vector zeros(3,0.0);
            mjCtrlNeck->reset(zeros);
            mjCtrlEyes->reset(zeros);
            IntPlan->reset(fbNeck);

            event="motion-onset";

            mutexData.lock();
            motionOngoingEventsCurrent=motionOngoingEvents;
            mutexData.unlock();
        }
    }

    mutexCtrl.lock();
    if (event=="motion-onset")
    {
        setJointsCtrlMode();
        q0deg=CTRL_RAD2DEG*fbHead;
    }
    mutexCtrl.unlock();

    qd=new_qd;
    qdNeck=qd.subVector(0,2);
    qdEyes=qd.subVector(3,5);
    
    if (commData->get_isCtrlActive())
    {
        // control loop
        vNeck=mjCtrlNeck->computeCmd(neckTime,qdNeck-fbNeck);
        IntPlan->integrate(vNeck);

        if (unplugCtrlEyes)
        {
            if (Time::now()-saccadeStartTime>=Ts)
                vEyes=commData->get_counterv();
        }
        else
            vEyes=mjCtrlEyes->computeCmd(eyesTime,qdEyes-fbEyes)+commData->get_counterv();
    }
    else
    {
        vNeck=0.0;
        vEyes=0.0;
    }

    v.setSubvector(0,vNeck);
    v.setSubvector(3,vEyes);

    // apply bang-bang just in case to compensate
    // for unachievable low velocities
    for (size_t i=0; i<v.length(); i++)
        if ((v[i]!=0.0) && (v[i]>-minAbsVel) && (v[i]<minAbsVel))
            v[i]=yarp::math::sign(qd[i]-fbHead[i])*minAbsVel;

    // convert to degrees
    mutexData.lock();
    qddeg=CTRL_RAD2DEG*qd;
    qdeg =CTRL_RAD2DEG*fbHead;
    vdeg =CTRL_RAD2DEG*v;
    mutexData.unlock();

    // send commands to the robot
    if (commData->get_isCtrlActive())
    {
        mutexCtrl.lock();

        Vector posdeg;
        if (neckPosCtrlOn)
        {
            posdeg=(CTRL_RAD2DEG)*IntPlan->get();
            posNeck->setPositions(neckJoints.size(),neckJoints.getFirst(),posdeg.data());
            velHead->velocityMove(eyesJoints.size(),eyesJoints.getFirst(),vdeg.subVector(3,5).data());
        }
        else
            velHead->velocityMove(vdeg.data());

        if (commData->debugInfoEnabled && (port_debug.getOutputCount()>0))
        {
            Bottle info;
            int j=0;

            if (neckPosCtrlOn)
            {
                for (size_t i=0; i<posdeg.length(); i++)
                {
                    ostringstream ss;
                    ss<<"pos_"<<i;
                    info.addString(ss.str().c_str());
                    info.addDouble(posdeg[i]);
                }

                j=eyesJoints[0];
            }

            for (size_t i=j; i<vdeg.length(); i++)
            {
                ostringstream ss;
                ss<<"vel_"<<i;
                info.addString(ss.str().c_str());
                info.addDouble(vdeg[i]);
            }

            port_debug.prepare()=info;
            txInfo_debug.update(q_stamp);
            port_debug.setEnvelope(txInfo_debug);
            port_debug.writeStrict();
        }

        mutexCtrl.unlock();
    }

    // print info
    if (commData->verbose)
        printIter(xd,x,qddeg,qdeg,vdeg,1.0); 

    // send x,q through YARP ports
    Vector q(nJointsTorso+nJointsHead);
    int j;
    for (j=0; j<nJointsTorso; j++)
        q[j]=CTRL_RAD2DEG*fbTorso[j];
    for (; (size_t)j<q.length(); j++)
        q[j]=qdeg[j-nJointsTorso];

    txInfo_x.update(x_stamp);
    if (port_x.getOutputCount()>0)
    {
        port_x.prepare()=x;
        port_x.setEnvelope(txInfo_x);
        port_x.write();
    }

    txInfo_q.update(q_stamp);
    if (port_q.getOutputCount()>0)
    {
        port_q.prepare()=q;
        port_q.setEnvelope(txInfo_q);
        port_q.write();
    }

    // update pose information
    mutexChain.lock();

    for (int i=0; i<nJointsTorso; i++)
    {
        chainNeck->setAng(i,fbTorso[i]);
        chainEyeL->setAng(i,fbTorso[i]);
        chainEyeR->setAng(i,fbTorso[i]);
    }
    for (int i=0; i<3; i++)
    {
        chainNeck->setAng(nJointsTorso+i,fbHead[i]);
        chainEyeL->setAng(nJointsTorso+i,fbHead[i]);
        chainEyeR->setAng(nJointsTorso+i,fbHead[i]);
    }
    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);

    txInfo_pose.update(q_stamp);

    mutexChain.unlock();

    if (event=="motion-onset")
        notifyEvent(event);

    motionOngoingEventsHandling();

    if (event=="motion-done")
    {
        motionOngoingEventsFlush();
        notifyEvent(event);
    }

    // update joints angles
    fbHead=IntState->integrate(v);
    commData->set_q(fbHead);
    commData->set_torso(fbTorso);
    commData->set_v(v);
}
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;
}
Example #8
0
void Controller::run()
{
    LockGuard guard(mutexRun);
    
    mutexCtrl.lock();
    bool jointsHealthy=areJointsHealthyAndSet();
    mutexCtrl.unlock();

    if (!jointsHealthy)
    {
        stopControlHelper();
        commData->port_xd->get_new()=false;
    }

    string event="none";

    // verify if any saccade is still underway
    mutexCtrl.lock();
    if (commData->saccadeUnderway && (Time::now()-saccadeStartTime>=Ts))
    {
        bool done;
        posHead->checkMotionDone(eyesJoints.size(),eyesJoints.getFirst(),&done);
        commData->saccadeUnderway=!done;

        if (!commData->saccadeUnderway)
            notifyEvent("saccade-done");
    }
    mutexCtrl.unlock();
    
    // get data
    double x_stamp;
    Vector xd=commData->get_xd();
    Vector x=commData->get_x(x_stamp);
    qd=commData->get_qd();

    // read feedbacks
    q_stamp=Time::now();
    if (!getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData,&q_stamp))
    {
        yError("Communication timeout detected!");
        notifyEvent("comm-timeout");
        suspend();
        return;
    }

    // update pose information
    {
        mutexChain.lock();
        for (int i=0; i<nJointsTorso; i++)
        {
            chainNeck->setAng(i,fbTorso[i]);
            chainEyeL->setAng(i,fbTorso[i]);
            chainEyeR->setAng(i,fbTorso[i]);
        }
        for (int i=0; i<3; i++)
        {
            chainNeck->setAng(nJointsTorso+i,fbHead[i]);
            chainEyeL->setAng(nJointsTorso+i,fbHead[i]);
            chainEyeR->setAng(nJointsTorso+i,fbHead[i]);
        }

        chainEyeL->setAng(nJointsTorso+3,fbHead[3]);               
        chainEyeL->setAng(nJointsTorso+4,fbHead[4]+fbHead[5]/2.0);
        chainEyeR->setAng(nJointsTorso+3,fbHead[3]);
        chainEyeR->setAng(nJointsTorso+4,fbHead[4]-fbHead[5]/2.0);

        txInfo_pose.update(q_stamp);
        mutexChain.unlock();
    }

    IntState->reset(fbHead);

    fbNeck=fbHead.subVector(0,2);
    fbEyes=fbHead.subVector(3,5);

    double errNeck=norm(qd.subVector(0,2)-fbNeck);
    double errEyes=norm(qd.subVector(3,qd.length()-1)-fbEyes);
    bool swOffCond=(Time::now()-ctrlActiveRisingEdgeTime<GAZECTRL_SWOFFCOND_DISABLESLOT*(0.001*getRate())) ? false :
                   (!commData->saccadeUnderway &&
                   (errNeck<GAZECTRL_MOTIONDONE_NECK_QTHRES*CTRL_DEG2RAD) &&
                   (errEyes<GAZECTRL_MOTIONDONE_EYES_QTHRES*CTRL_DEG2RAD));

    // verify control switching conditions
    if (commData->ctrlActive)
    {
        // manage new target while controller is active
        if (commData->port_xd->get_new())
        {
            reliableGyro=true;

            event="motion-onset";

            mutexData.lock();
            motionOngoingEventsCurrent=motionOngoingEvents;
            mutexData.unlock();

            ctrlActiveRisingEdgeTime=Time::now();
            commData->port_xd->get_new()=false;
        }
        // switch-off condition
        else if (swOffCond)
        {
            if (commData->trackingModeOn)
            {
                if (!motionDone)
                    event="motion-done";
                motionDone=true;
            }
            else
            {
                event="motion-done";
                mutexCtrl.lock(); 
                stopLimb(false);
                mutexCtrl.unlock();
            }
        }        
    }
    else if (jointsHealthy)
    {
        // inhibition is cleared upon new target arrival
        if (ctrlInhibited)
            ctrlInhibited=!commData->port_xd->get_new();

        // switch-on condition
        commData->ctrlActive=commData->port_xd->get_new() || (!ctrlInhibited && (commData->neckSolveCnt>0));

        // reset controllers
        if (commData->ctrlActive)
        {
            ctrlActiveRisingEdgeTime=Time::now();
            commData->port_xd->get_new()=false;
            commData->neckSolveCnt=0;

            if (stabilizeGaze)
            {
                mjCtrlNeck->reset(vNeck);
                mjCtrlEyes->reset(vEyes);
            }
            else
            {
                mjCtrlNeck->reset(zeros(fbNeck.length())); 
                mjCtrlEyes->reset(zeros(fbEyes.length()));
                IntStabilizer->reset(zeros(vNeck.length()));
                IntPlan->reset(fbNeck);
            }
            
            reliableGyro=true;

            event="motion-onset";

            mutexData.lock();
            motionOngoingEventsCurrent=motionOngoingEvents;
            mutexData.unlock();
        }
    }

    mutexCtrl.lock();
    if (event=="motion-onset")
    {
        setJointsCtrlMode();
        jointsToSet.clear();
        motionDone=false;
        q0=fbHead;
    }
    mutexCtrl.unlock();

    if (commData->trackingModeOn || stabilizeGaze)
    {
        mutexCtrl.lock();
        setJointsCtrlMode();
        mutexCtrl.unlock();
    }

    qdNeck=qd.subVector(0,2);
    qdEyes=qd.subVector(3,5);

    // compute current point [%] in the path
    double dist=norm(qd-q0);
    pathPerc=(dist>IKIN_ALMOST_ZERO)?norm(fbHead-q0)/dist:1.0;
    pathPerc=sat(pathPerc,0.0,1.0);
    
    if (commData->ctrlActive)
    {
        // control
        vNeck=mjCtrlNeck->computeCmd(neckTime,qdNeck-fbNeck);
        
        if (unplugCtrlEyes)
        {
            if (Time::now()-saccadeStartTime>=Ts)
                vEyes=commData->get_counterv();
        }
        else
            vEyes=mjCtrlEyes->computeCmd(eyesTime,qdEyes-fbEyes)+commData->get_counterv();

        // stabilization
        if (commData->stabilizationOn)
        {
            Vector gyro=CTRL_DEG2RAD*commData->get_imu().subVector(6,8);
            Vector dx=computedxFP(imu->getH(cat(fbTorso,fbNeck)),zeros(fbNeck.length()),gyro,x);
            Vector imuNeck=computeNeckVelFromdxFP(x,dx);

            if (reliableGyro)
            {
                vNeck=commData->stabilizationGain*IntStabilizer->integrate(vNeck-imuNeck);

                // only if the speed is low and we are close to the target
                if ((norm(vNeck)<commData->gyro_noise_threshold) && (pathPerc>0.9))
                    reliableGyro=false;
            }
            // hysteresis
            else if ((norm(imuNeck)>1.5*commData->gyro_noise_threshold) || (pathPerc<0.9))
            {
                IntStabilizer->reset(zeros(vNeck.length()));
                reliableGyro=true;
            }
        }

        IntPlan->integrate(vNeck);
    }
    else if (stabilizeGaze)
    {
        Vector gyro=CTRL_DEG2RAD*commData->get_imu().subVector(6,8);
        Vector dx=computedxFP(imu->getH(cat(fbTorso,fbNeck)),zeros(fbNeck.length()),gyro,x);
        Vector imuNeck=computeNeckVelFromdxFP(x,dx);

        vNeck=commData->stabilizationGain*IntStabilizer->integrate(-1.0*imuNeck);
        vEyes=computeEyesVelFromdxFP(dx);

        IntPlan->integrate(vNeck);
    }
    else 
    {
        vNeck=0.0;
        vEyes=0.0;
    }

    v.setSubvector(0,vNeck);
    v.setSubvector(3,vEyes);

    // apply bang-bang just in case to compensate
    // for unachievable low velocities
    for (size_t i=0; i<v.length(); i++)
        if ((v[i]!=0.0) && (v[i]>-min_abs_vel) && (v[i]<min_abs_vel))
            v[i]=yarp::math::sign(qd[i]-fbHead[i])*min_abs_vel;

    // convert to degrees
    mutexData.lock();
    qddeg=CTRL_RAD2DEG*qd;
    qdeg =CTRL_RAD2DEG*fbHead;
    vdeg =CTRL_RAD2DEG*v;
    mutexData.unlock();

    // send commands to the robot
    if (commData->ctrlActive || stabilizeGaze)
    {
        mutexCtrl.lock();

        Vector posdeg;
        if (commData->neckPosCtrlOn)
        {
            posdeg=(CTRL_RAD2DEG)*IntPlan->get();
            posNeck->setPositions(neckJoints.size(),neckJoints.getFirst(),posdeg.data());
            velHead->velocityMove(eyesJoints.size(),eyesJoints.getFirst(),vdeg.subVector(3,5).data());
        }
        else
            velHead->velocityMove(vdeg.data());

        if (commData->debugInfoEnabled && (port_debug.getOutputCount()>0))
        {
            Bottle info;
            int j=0;

            if (commData->neckPosCtrlOn)
            {
                for (size_t i=0; i<posdeg.length(); i++)
                {
                    ostringstream ss;
                    ss<<"pos_"<<i;
                    info.addString(ss.str().c_str());
                    info.addDouble(posdeg[i]);
                }

                j=eyesJoints[0];
            }

            for (size_t i=j; i<vdeg.length(); i++)
            {
                ostringstream ss;
                ss<<"vel_"<<i;
                info.addString(ss.str().c_str());
                info.addDouble(vdeg[i]);
            }

            port_debug.prepare()=info;
            txInfo_debug.update(q_stamp);
            port_debug.setEnvelope(txInfo_debug);
            port_debug.writeStrict();
        }

        mutexCtrl.unlock();
    }

    // print info
    if (commData->verbose)
        printIter(xd,x,qddeg,qdeg,vdeg,1.0); 

    // send x,q through YARP ports
    Vector q(nJointsTorso+nJointsHead);
    int j;
    for (j=0; j<nJointsTorso; j++)
        q[j]=CTRL_RAD2DEG*fbTorso[j];
    for (; (size_t)j<q.length(); j++)
        q[j]=qdeg[j-nJointsTorso];

    txInfo_x.update(x_stamp);
    if (port_x.getOutputCount()>0)
    {
        port_x.prepare()=x;
        port_x.setEnvelope(txInfo_x);
        port_x.write();
    }

    txInfo_q.update(q_stamp);
    if (port_q.getOutputCount()>0)
    {
        port_q.prepare()=q;
        port_q.setEnvelope(txInfo_q);
        port_q.write();
    }

    if (event=="motion-onset")
        notifyEvent(event);

    motionOngoingEventsHandling();

    if (event=="motion-done")
    {
        motionOngoingEventsFlush();
        notifyEvent(event);
    }

    // update joints angles
    fbHead=IntState->integrate(v);
    commData->set_q(fbHead);
    commData->set_torso(fbTorso);
    commData->set_v(v);
}
Example #9
0
void Solver::run()
{
    typedef enum { ctrl_off, ctrl_wait, ctrl_on } cstate;
    static cstate state_=ctrl_off;

    mutex.lock();

    // get the current target
    Vector xd=commData->port_xd->get_xdDelayed();

    // update the target straightaway 
    commData->set_xd(xd);

    // read encoders
    getFeedback(fbTorso,fbHead,drvTorso,drvHead,commData);
    updateTorsoBlockedJoints(chainNeck,fbTorso);
    updateTorsoBlockedJoints(chainEyeL,fbTorso);
    updateTorsoBlockedJoints(chainEyeR,fbTorso);

    bool torsoChanged=(norm(fbTorso-fbTorsoOld)>NECKSOLVER_ACTIVATIONANGLE_JOINTS*CTRL_DEG2RAD);

    // update kinematics
    updateAngles();
    updateNeckBlockedJoints(chainEyeL,fbHead);
    updateNeckBlockedJoints(chainEyeR,fbHead);
    chainNeck->setAng(neckPos);

    // hereafter accumulate solving conditions: the order does matter

    // 1) compute the angular distance
    double theta=neckTargetRotAngle(xd);
    bool doSolve=(theta>NECKSOLVER_ACTIVATIONANGLE);

    // 2) skip if controller is active and no torso motion is detected
    doSolve&=!(commData->ctrlActive && !torsoChanged);

    // 3) skip if controller is inactive and we are not in tracking mode
    doSolve&=commData->ctrlActive || commData->trackingModeOn;

    // 4) solve straightaway if we are in tracking mode and torso motion is detected
    doSolve|=commData->trackingModeOn && torsoChanged;

    // 5) solve straightaway if the target has changed
    doSolve|=commData->port_xd->get_newDelayed();

    // 6) skip if the angle to target is lower than the user tolerance
    doSolve&=(theta>neckAngleUserTolerance);

    // clear triggers
    commData->port_xd->get_newDelayed()=false;

    // call the solver for neck
    if (doSolve)
    {
        Vector gDir(4,0.0);
        gDir[2]=gDir[3]=1.0;
        if (commData->stabilizationOn)
        {
            Vector acc=commData->get_imu().subVector(3,5); 
            acc.push_back(1.0); // impose homogeneous coordinates

            Matrix H=imu->getH(cat(fbTorso,fbHead.subVector(0,2)));
            gDir=H*acc;            
        }

        Vector xdUserTol=computeTargetUserTolerance(xd);
        neckPos=invNeck->solve(neckPos,xdUserTol,gDir);

        // update neck pitch,roll,yaw        
        commData->set_qd(0,neckPos[0]);
        commData->set_qd(1,neckPos[1]);
        commData->set_qd(2,neckPos[2]);
        commData->neckSolveCnt++;

        state_=ctrl_wait;
    }

    if (state_==ctrl_off)
    {
        // keep neck targets equal to current angles
        // to avoid glitches in the control (especially
        // during stabilization)
        commData->set_qd(0,neckPos[0]);
        commData->set_qd(1,neckPos[1]);
        commData->set_qd(2,neckPos[2]);
    }
    else if (state_==ctrl_wait)
    {
        if (commData->ctrlActive)
            state_=ctrl_on;
    }
    else if (state_==ctrl_on)
    {
        if (!commData->ctrlActive)
            state_=ctrl_off;
    }
    
    // latch quantities
    fbTorsoOld=fbTorso;

    mutex.unlock();
}
Example #10
0
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;
}
Example #11
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();
    }
}
Example #12
0
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;
}
int main()
{
	// not recommended to do something with sizes :(
	COORD size = {9, 8};
	drawTable(size);

	// task - aim, request - user input
	i4 task(generateTask());
	i4 request;

	// index to watch which digit user is entered
	// line - offset for the log output
	int index = 0, line = 4;
	
	bool bQuit = false;
	while (!bQuit)
	{
		char ch[] = {readKey(), '\0'};
		int code = (int)(ch[0]);

		// if input is digit
		if (code > 47 && code < 58 && index < 4)
		{
			int num = code - 48;
			bool bDublicate = false;
			for (int i = 0; i < index; ++i)
			{
				if (request[i] == num)
				{
					bDublicate = true;
					break;
				}
			}

			if (bDublicate)
				continue;

			request[index] = num;
			++index;
			print({2 + index, 1}, ch);
		}

		// if input is erase
		if (code == KEY_BACK && index > 0)
			print({2 + index--, 1}, "_");

		// if input is confirm
		if (code == KEY_ENTER && index == 4)
		{
			print({3, 1}, "____");
			i2 fb = getFeedback(request, task);

			// hard way to do simple thing
			char c[] = {
				request[0] + 48,
				request[1] + 48,
				request[2] + 48,
				request[3] + 48,
				' ',
				fb[0] + 48,
				':',
				fb[1] + 48,
				'\0'
			};
			print({1, line}, c);
			++line;
			index = 0;

			// make the table longer if needed
			if (line > size.Y-1)
			{
				print({0, size.Y}, "|");
				print({size.X, size.Y}, "|");
				++size.Y;
			}

			// win event
			if (fb[0] == 4)
			{
				for (int i = 0; i < size.X-1; ++i)
					print({1 + i, line}, "!");

				readKey();
				break;
			}
		}

		// if input is exit
		if (code == KEY_ESC)
			return 0;
		//printf("%d", code);
	}
	clearScreen();
	system("pause");
	return 0;
}