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);
}
Example #2
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);
}