utils.cpp

Go to the documentation of this file.

/* 
 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
 * Author: Ugo Pattacini, Alessandro Roncone
 * email:  ugo.pattacini@iit.it, alessandro.roncone@iit.it
 * website: www.robotcub.org
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details
*/
 
#include <cmath>
#include <limits>
#include <algorithm>
 
#include <iCub/utils.h>
#include <iCub/solver.h>
 
constexpr int32_t MUTEX_XD       = 0;
constexpr int32_t MUTEX_QD       = 1;
constexpr int32_t MUTEX_X        = 2;
constexpr int32_t MUTEX_Q        = 3;
constexpr int32_t MUTEX_TORSO    = 4;
constexpr int32_t MUTEX_V        = 5;
constexpr int32_t MUTEX_COUNTERV = 6;
constexpr int32_t MUTEX_FPFRAME  = 7;
 
/************************************************************************/
xdPort::xdPort(void *_slv) : slv(_slv)
{   
    isNewDelayed=isNew=false;
    locked=false;
    closing=false;
    rx=0;
 
    useCallback();
    start();
}
 
 
/************************************************************************/
void xdPort::init(const Vector &xd0)
{
    xdDelayed=xd=xd0;
}
 
 
/************************************************************************/
xdPort::~xdPort()
{
    closing=true;
    cv_triggerNeck.notify_all();
    stop();
}
 
 
/************************************************************************/
void xdPort::onRead(Bottle &b)
{
    lock_guard<mutex> lg(mutex_0);
    if (locked)
        return;
        
    size_t n=std::min(b.size(),xd.length());
    for (size_t i=0; i<n; i++)
        xd[i]=b.get(i).asFloat64();
 
    isNew=true;
    rx++;
 
    cv_triggerNeck.notify_all();
}
 
 
/************************************************************************/
bool xdPort::set_xd(const Vector &_xd)
{
    lock_guard<mutex> lg(mutex_0);
    if (locked)
        return false;
 
    xd=_xd;
    isNew=true;
    rx++;
    cv_triggerNeck.notify_all();
    return true;
}
 
 
/************************************************************************/
Vector xdPort::get_xd()
{
    lock_guard<mutex> lg(mutex_0);
    Vector _xd=xd;
    return _xd;
}
 
 
/************************************************************************/
Vector xdPort::get_xdDelayed()
{
    lock_guard<mutex> lg(mutex_1);
    Vector _xdDelayed=xdDelayed;
    return _xdDelayed;
}
 
 
/************************************************************************/
void xdPort::run()
{
    while (!isStopping() && !closing)
    {
        unique_lock<mutex> lck(mtx_triggerNeck);
        cv_triggerNeck.wait(lck);
 
        double timeDelay=0.0;
        double theta=static_cast<Solver*>(slv)->neckTargetRotAngle(xd);
        if (theta<NECKSOLVER_RESTORINGANGLE)
            timeDelay=NECKSOLVER_ACTIVATIONDELAY;
 
        Time::delay(timeDelay);
 
        lock_guard<mutex> lg(mutex_1);
        xdDelayed=xd;
        isNewDelayed=true;
    }
}
 
 
/************************************************************************/
ExchangeData::ExchangeData()
{
    imu.resize(12,0.0);
    port_xd=nullptr;
 
    ctrlActive=false;
    trackingModeOn=false;
    saccadeUnderway=false;
    minAllowedVergence=0.0;    
    eyesBoundVer=-1.0;
    neckSolveCnt=0;
 
    saccadesInhibitionPeriod=SACCADES_INHIBITION_PERIOD;
    saccadesActivationAngle=SACCADES_ACTIVATION_ANGLE;
 
    eyeTiltLim.resize(2);
    eyeTiltLim[0]=-std::numeric_limits<double>::max();
    eyeTiltLim[1]=std::numeric_limits<double>::max();
 
    robotName="";
    localStemName="";
    head_version=iKinLimbVersion("1.0");
    tweakOverwrite=true;
    tweakFile="";
    iGyro = nullptr;
    iAccel = nullptr;
}
 
 
/************************************************************************/
void ExchangeData::resize_v(const int sz, const double val)
{
    lock_guard<mutex> lg(mtx[MUTEX_V]);
    v.resize(sz,val);
}
 
 
/************************************************************************/
void ExchangeData::resize_counterv(const int sz, const double val)
{
    lock_guard<mutex> lg(mtx[MUTEX_COUNTERV]);
    counterv.resize(sz,val);
}
 
 
/************************************************************************/
void ExchangeData::set_xd(const Vector &_xd)
{
    lock_guard<mutex> lg(mtx[MUTEX_XD]);
    xd=_xd;
}
 
 
/************************************************************************/
void ExchangeData::set_qd(const Vector &_qd)
{
    lock_guard<mutex> lg(mtx[MUTEX_QD]);
    qd=_qd;
}
 
 
/************************************************************************/
void ExchangeData::set_qd(const int i, const double val)
{
    lock_guard<mutex> lg(mtx[MUTEX_QD]);
    qd[i]=val;
}
 
 
/************************************************************************/
void ExchangeData::set_x(const Vector &_x)
{
    lock_guard<mutex> lg(mtx[MUTEX_X]);
    x=_x;
}
 
 
/************************************************************************/
void ExchangeData::set_x(const Vector &_x, const double stamp)
{
    lock_guard<mutex> lg(mtx[MUTEX_X]);
    x=_x;
    x_stamp=stamp;
}
 
 
/************************************************************************/
void ExchangeData::set_q(const Vector &_q)
{
    lock_guard<mutex> lg(mtx[MUTEX_Q]);
    q=_q;
}
 
 
/************************************************************************/
void ExchangeData::set_torso(const Vector &_torso)
{
    lock_guard<mutex> lg(mtx[MUTEX_TORSO]);
    torso=_torso;
}
 
 
/************************************************************************/
void ExchangeData::set_v(const Vector &_v)
{
    lock_guard<mutex> lg(mtx[MUTEX_V]);
    v=_v;
}
 
 
/************************************************************************/
void ExchangeData::set_counterv(const Vector &_counterv)
{
    lock_guard<mutex> lg(mtx[MUTEX_COUNTERV]);
    counterv=_counterv;
}
 
 
/************************************************************************/
void ExchangeData::set_fpFrame(const Matrix &_S)
{
    lock_guard<mutex> lg(mtx[MUTEX_FPFRAME]);
    S=_S;
}
 
/************************************************************************/
Vector ExchangeData::get_xd()
{
    lock_guard<mutex> lg(mtx[MUTEX_XD]);
    Vector _xd=xd;
    return _xd;
}
 
 
/************************************************************************/
Vector ExchangeData::get_qd()
{
    lock_guard<mutex> lg(mtx[MUTEX_QD]);
    Vector _qd=qd;
    return _qd;
}
 
 
/************************************************************************/
Vector ExchangeData::get_x()
{
    lock_guard<mutex> lg(mtx[MUTEX_X]);
    Vector _x=x;
    return _x;
}
 
 
/************************************************************************/
Vector ExchangeData::get_x(double &stamp)
{
    lock_guard<mutex> lg(mtx[MUTEX_X]);
    Vector _x=x;
    stamp=x_stamp;
    return _x;
}
 
 
/************************************************************************/
Vector ExchangeData::get_q()
{
    lock_guard<mutex> lg(mtx[MUTEX_Q]);
    Vector _q=q;
    return _q;
}
 
 
/************************************************************************/
Vector ExchangeData::get_torso()
{
    lock_guard<mutex> lg(mtx[MUTEX_TORSO]);
    Vector _torso=torso;
    return _torso;
}
 
 
/************************************************************************/
Vector ExchangeData::get_v()
{
    lock_guard<mutex> lg(mtx[MUTEX_V]);
    Vector _v=v;
    return _v;
}
 
 
/************************************************************************/
Vector ExchangeData::get_counterv()
{
    lock_guard<mutex> lg(mtx[MUTEX_COUNTERV]);
    Vector _counterv=counterv;
    return _counterv;
}
 
 
/************************************************************************/
Matrix ExchangeData::get_fpFrame()
{
    lock_guard<mutex> lg(mtx[MUTEX_FPFRAME]);
    Matrix _S=S;
    return _S;
}
 
/************************************************************************/
std::pair<Vector,bool>  ExchangeData::get_gyro() {
    std::pair<Vector, bool> ret;
    Vector gyro(3,0.0);
    if (! iGyro) {
        ret.second = false;
    }
    else {
        double ts;
        ret.second =  iGyro->getThreeAxisGyroscopeStatus(0) == MAS_OK;
        ret.second &= iGyro->getThreeAxisGyroscopeMeasure(0, gyro, ts);
        gyro *= CTRL_DEG2RAD;
    }
    ret.first = gyro;
    return ret; // RVO
}
 
/************************************************************************/
std::pair<Vector,bool>  ExchangeData::get_accel() {
    std::pair<Vector, bool> ret;
    Vector accel(3,0.0);
    if (! iAccel) {
        ret.second = false;
    }
    else {
        double ts;
        ret.second =  iAccel->getThreeAxisLinearAccelerometerStatus(0) == MAS_OK;
        ret.second &= iAccel->getThreeAxisLinearAccelerometerMeasure(0, accel, ts);
    }
    ret.first = accel;
    return ret; // RVO
}
 
 
/************************************************************************/
bool GazeComponent::getExtrinsicsMatrix(const string &type, Matrix &M)
{
    if (type=="left")
    {
        M=eyeL->asChain()->getHN();
        return true;
    }
    else if (type=="right")
    {
        M=eyeR->asChain()->getHN();
        return true;
    }
    else
        return false;
}
 
 
/************************************************************************/
bool GazeComponent::setExtrinsicsMatrix(const string &type, const Matrix &M)
{
    if (type=="left")
    {
        eyeL->asChain()->setHN(M);
        return true;
    }
    else if (type=="right")
    {
        eyeR->asChain()->setHN(M);
        return true;
    }
    else
        return false;
}
 
 
/************************************************************************/
bool getCamParams(const ResourceFinder &rf, const string &type,
                  Matrix **Prj, int &w, int &h, const bool verbose)
{
    ResourceFinder &_rf=const_cast<ResourceFinder&>(rf);
    *Prj=nullptr;
 
    if (!_rf.isConfigured())
        return false;
 
    string message=_rf.findFile("from");
    if (!message.empty())
    {
        message+=": intrinsic parameters for "+type;
        Bottle &parType=_rf.findGroup(type);
        if (parType.check("w")  && parType.check("h") &&
            parType.check("fx") && parType.check("fy") &&
            parType.check("cx") && parType.check("cy"))
        {
            w=parType.find("w").asInt32();
            h=parType.find("h").asInt32();
            double fx=parType.find("fx").asFloat64();
            double fy=parType.find("fy").asFloat64();
            double cx=parType.find("cx").asFloat64();
            double cy=parType.find("cy").asFloat64();
 
            if (verbose)
            {
                yInfo("%s found:",message.c_str());
                yInfo("w  = %d",w);
                yInfo("h  = %d",h);
                yInfo("fx = %g",fx);
                yInfo("fy = %g",fy);
                yInfo("cx = %g",cx);
                yInfo("cy = %g",cy);
            }
 
            *Prj=new Matrix(eye(3,4));
 
            Matrix &K=**Prj;
            K(0,0)=fx; K(1,1)=fy;
            K(0,2)=cx; K(1,2)=cy;
 
            return true;
        }
    }
    else
    {
        message=_rf.find("from").asString();
        message+=": intrinsic parameters for "+type;
    }
 
    if (verbose)
        yWarning("%s not found!",message.c_str());
 
    return false;
}
 
 
/************************************************************************/
bool getAlignHN(const ResourceFinder &rf, const string &type,
                iKinChain *chain, const bool verbose)
{
    ResourceFinder &_rf=const_cast<ResourceFinder&>(rf);
    if ((chain!=nullptr) && _rf.isConfigured())
    {
        string message=_rf.findFile("from");
        if (!message.empty())
        {
            message+=": aligning matrix for "+type;
            Bottle &parType=_rf.findGroup(type);
            if (Bottle *bH=parType.find("HN").asList())
            {
                int i=0;
                int j=0;
 
                Matrix HN(4,4); HN=0.0;
                for (int cnt=0; (cnt<bH->size()) && (cnt<HN.rows()*HN.cols()); cnt++)
                {
                    HN(i,j)=bH->get(cnt).asFloat64();
                    if (++j>=HN.cols())
                    {
                        i++;
                        j=0;
                    }
                }
 
                // enforce the homogeneous property
                HN(3,0)=HN(3,1)=HN(3,2)=0.0;
                HN(3,3)=1.0;
 
                chain->setHN(HN);
 
                if (verbose)
                {
                    yInfo("%s found:",message.c_str());
                    yInfo("%s",HN.toString(3,3).c_str());
                }
 
                return true;
            }
        }
        else
        {
            message=_rf.find("from").asString();
            message+=": aligning matrix for "+type;
        }
 
        if (verbose)
            yWarning("%s not found!",message.c_str());
    }
 
    return false;
}
 
 
/************************************************************************/
Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso,
                         PolyDriver *drvHead, const ExchangeData *commData)
{
    IEncoders      *encs;
    IControlLimits *lims;
 
    double min, max;
    int nJointsTorso=3;    
 
    if (drvTorso!=nullptr)
    {
        drvTorso->view(encs);
        drvTorso->view(lims);        
        encs->getAxes(&nJointsTorso);
 
        for (int i=0; i<nJointsTorso; i++)
        {   
            if (lims->getLimits(i,&min,&max))
            {
                if (commData->head_version<iKinLimbVersion("3.0"))
                {
                    (*chain)[nJointsTorso-1-i].setMin(CTRL_DEG2RAD*min);
                    (*chain)[nJointsTorso-1-i].setMax(CTRL_DEG2RAD*max);
                }
                else
                {
                    (*chain)[i].setMin(CTRL_DEG2RAD*min);
                    (*chain)[i].setMax(CTRL_DEG2RAD*max);
                }
            }
            else
                yError("unable to retrieve limits for torso joint #%d",i);
        }
    }
 
    drvHead->view(encs);
    drvHead->view(lims);
    int nJointsHead;
    encs->getAxes(&nJointsHead);
    Matrix lim(nJointsHead,2);
 
    for (int i=0; i<nJointsHead; i++)
    {   
        if (lims->getLimits(i,&min,&max))
        {
            // limit eye's tilt due to eyelids
            if (i==3)
            {
                min=std::max(min,commData->eyeTiltLim[0]);
                max=std::min(max,commData->eyeTiltLim[1]);
            }
 
            lim(i,0)=CTRL_DEG2RAD*min;
            lim(i,1)=CTRL_DEG2RAD*max;
 
            // just one eye's got only 5 dofs
            if (i<nJointsHead-1)
            {
                (*chain)[nJointsTorso+i].setMin(lim(i,0));
                (*chain)[nJointsTorso+i].setMax(lim(i,1));
            }
        }
        else
            yError("unable to retrieve limits for head joint #%d",i);
    }
 
    return lim;
}
 
 
/************************************************************************/
void copyJointsBounds(iKinChain *ch1, iKinChain *ch2)
{
    unsigned int N1=ch1->getN();
    unsigned int N2=ch2->getN();
    unsigned int N =N1>N2 ? N2 : N1;
 
    for (unsigned int i=0; i<N; i++)
    {
        (*ch2)[i].setMin((*ch1)[i].getMin());
        (*ch2)[i].setMax((*ch1)[i].getMax());
    }
}
 
 
/************************************************************************/
void updateTorsoBlockedJoints(iKinChain *chain, const Vector &fbTorso)
{
    for (unsigned int i=0; i<(unsigned int)fbTorso.length(); i++)
         chain->setBlockingValue(i,fbTorso[i]);
}
 
 
/************************************************************************/
void updateNeckBlockedJoints(iKinChain *chain, const Vector &fbNeck)
{
    for (int i=0; i<3; i++)
         chain->setBlockingValue(3+i,fbNeck[i]);
}
 
 
/************************************************************************/
bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso,
                 PolyDriver *drvHead, const ExchangeData *commData,
                 double *timeStamp)
{
    IEncodersTimed *encs;
 
    int nJointsTorso=(int)fbTorso.length();
    int nJointsHead=(int)fbHead.length();
 
    Vector fb(std::max(nJointsTorso,nJointsHead));
    Vector stamps(nJointsTorso+nJointsHead,0.0);
    bool ret=true;
    
    if (drvTorso!=nullptr)
    {
        drvTorso->view(encs);
        if (encs->getEncodersTimed(fb.data(),stamps.data()))
        {
            for (int i=0; i<nJointsTorso; i++)
                fbTorso[i]=CTRL_DEG2RAD*((commData->head_version<iKinLimbVersion("3.0"))?fb[nJointsTorso-1-i]:fb[i]);
        }
        else
            ret=false;
    }
    else
        fbTorso=0.0;
 
    drvHead->view(encs);
    if (encs->getEncodersTimed(fb.data(),stamps.data()+nJointsTorso))
    {
        for (int i=0; i<nJointsHead; i++)
            fbHead[i]=CTRL_DEG2RAD*fb[i];
    }
    else
        ret=false;
    
    // impose vergence != 0.0
    if (commData!=nullptr)
        fbHead[nJointsHead-1]=std::max(fbHead[nJointsHead-1],commData->minAllowedVergence);
    
    // retrieve the highest encoders time stamp
    if (timeStamp!=nullptr)
        *timeStamp=findMax(stamps);
 
    return ret;
}