iKinFwd.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/

#ifndef __IKINFWD_H__
#define __IKINFWD_H__

#include <string>
#include <deque>

#include <yarp/os/Property.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
#include <yarp/math/Math.h>

#include <iCub/ctrl/math.h>


namespace iCub
{

namespace iKin
{

void notImplemented(const unsigned int verbose);

class iKinLink
{
protected:
    double       A;
    double       D;
    double       Alpha;
    double       Offset;
    double       c_alpha;
    double       s_alpha;
    double       Min;
    double       Max;
    double       Ang;
    bool         blocked;
    bool         cumulative;
    bool         constrained;
    unsigned int verbose;

    yarp::sig::Matrix H;
    yarp::sig::Matrix cumH;
    yarp::sig::Matrix DnH;

    const yarp::sig::Matrix zeros1x1;
    const yarp::sig::Vector zeros1;

    friend class iKinChain;

    // Default constructor: not implemented.
    iKinLink();

    virtual void clone(const iKinLink &l);    
    bool         isCumulative()     { return cumulative;          }
    void         block()            { blocked=true;               }
    void         block(double _Ang) { setAng(_Ang); blocked=true; }
    void         release()          { blocked=false;              }
    void         rmCumH()           { cumulative=false;           }
    void         addCumH(const yarp::sig::Matrix &_cumH);

public:
    iKinLink(double _A, double _D, double _Alpha, double _Offset,
             double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);

    iKinLink(const iKinLink &l);

    iKinLink &operator=(const iKinLink &l);

    void setConstraint(bool _constrained) { constrained=_constrained; }

    bool getConstraint() const { return constrained; }

    void setVerbosity(unsigned int _verbose) { verbose=_verbose; }

    unsigned int getVerbosity() const { return verbose; }

    bool isBlocked() const { return blocked; }

    double getA() const { return A; }

    void setA(const double _A) { A=_A; }

    double getD() const { return D; }

    void setD(const double _D);

    double getAlpha() const { return Alpha; }

    void setAlpha(const double _Alpha);

    double getOffset() const { return Offset; }

    void setOffset(const double _Offset) { Offset=_Offset; }

    double getMin() const { return Min; }

    void setMin(const double _Min);

    double getMax() const { return Max; }

    void setMax(const double _Max);

    double getAng() const { return Ang; }

    double setAng(double _Ang);

    yarp::sig::Matrix getH(bool c_override=false);

    yarp::sig::Matrix getH(double _Ang, bool c_override=false);

    yarp::sig::Matrix getDnH(unsigned int n=1, bool c_override=false);

    virtual ~iKinLink() { }

    // void methods for iDyn
    // set
    virtual double setDAng(const double)                                          { notImplemented(verbose); return 0.0;   }
    virtual double setD2Ang(const double)                                         { notImplemented(verbose); return 0.0;   }
    virtual void   setPosVelAcc(const double, const double, const double)         { notImplemented(verbose);               }
    virtual bool   setDynamicParameters(const double, const yarp::sig::Matrix&,
                                        const yarp::sig::Matrix&, const double,
                                        const double, const double, const double) { notImplemented(verbose); return false; }
    virtual bool   setDynamicParameters(const double, const yarp::sig::Matrix&,
                                        const yarp::sig::Matrix&)                 { notImplemented(verbose); return false; }
    virtual bool   setStaticParameters(const double, const yarp::sig::Matrix&)    { notImplemented(verbose); return false; }
    virtual bool   setInertia(const yarp::sig::Matrix &)                          { notImplemented(verbose); return false; }
    virtual void   setMass(const double)                                          { notImplemented(verbose);               }
    virtual bool   setCOM(const yarp::sig::Matrix&)                               { notImplemented(verbose); return false; }
    virtual bool   setCOM(const yarp::sig::Vector&)                               { notImplemented(verbose); return false; }
    virtual bool   setForce(const yarp::sig::Vector&, const yarp::sig::Vector&)   { notImplemented(verbose); return false; }
    virtual bool   setMoment(const yarp::sig::Vector&)                            { notImplemented(verbose); return false; }
    virtual void   setTorque(const double)                                        { notImplemented(verbose);               }

    // get
    virtual const  yarp::sig::Matrix &getInertia() const { notImplemented(verbose); return zeros1x1; }
    virtual double                    getMass()    const { notImplemented(verbose); return 0.0;      }
    virtual double                    getIm()      const { notImplemented(verbose); return 0.0;      }
    virtual double                    getKr()      const { notImplemented(verbose); return 0.0;      }
    virtual double                    getFs()      const { notImplemented(verbose); return 0.0;      }
    virtual double                    getFv()      const { notImplemented(verbose); return 0.0;      }
    virtual const  yarp::sig::Matrix &getCOM()     const { notImplemented(verbose); return zeros1x1; }
    virtual double                    getDAng()    const { notImplemented(verbose); return 0.0;      }
    virtual double                    getD2Ang()   const { notImplemented(verbose); return 0.0;      }
    virtual const  yarp::sig::Matrix &getR()             { notImplemented(verbose); return zeros1x1; }
    virtual const  yarp::sig::Matrix &getRC()            { notImplemented(verbose); return zeros1x1; }
    virtual const  yarp::sig::Vector &getr()             { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getrC()            { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getW()       const { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getdW()      const { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getdWM()     const { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getLinAcc()  const { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getLinAccC() const { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getLinVel()  const { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getLinVelC() const { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getForce()   const { notImplemented(verbose); return zeros1;   }
    virtual const  yarp::sig::Vector &getMoment()  const { notImplemented(verbose); return zeros1;   }
    virtual double                    getTorque()  const { notImplemented(verbose); return 0.0;      }
};


class iKinChain
{
protected:
    unsigned int      N;
    unsigned int      DOF;
    unsigned int      verbose;
    yarp::sig::Matrix H0;
    yarp::sig::Matrix HN;
    yarp::sig::Vector curr_q;

    std::deque<iKinLink*> allList;
    std::deque<iKinLink*> quickList;

    std::deque<unsigned int> hash;
    std::deque<unsigned int> hash_dof;

    yarp::sig::Matrix hess_J;
    yarp::sig::Matrix hess_Jlnk;

    virtual void clone(const iKinChain &c);
    virtual void build();
    virtual void dispose();

    yarp::sig::Vector RotAng(const yarp::sig::Matrix &R);
    yarp::sig::Vector dRotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dR);
    yarp::sig::Vector d2RotAng(const yarp::sig::Matrix &R, const yarp::sig::Matrix &dRi,
                               const yarp::sig::Matrix &dRj, const yarp::sig::Matrix &d2R);

public:
    iKinChain();

    iKinChain(const iKinChain &c);

    iKinChain &operator=(const iKinChain &c);

    iKinChain &operator<<(iKinLink &l);

    iKinChain &operator--(int);

    iKinLink &operator[](const unsigned int i) { return *allList[i]; }

    iKinLink &operator()(const unsigned int i) { return *quickList[hash_dof[i]]; }

    bool addLink(const unsigned int i, iKinLink &l);

    bool rmLink(const unsigned int i);

    void pushLink(iKinLink &l);

    void clear();

    void popLink();

    bool blockLink(const unsigned int i, double Ang);

    bool blockLink(const unsigned int i) { return blockLink(i,getAng(i)); }

    bool setBlockingValue(const unsigned int i, double Ang);

    bool releaseLink(const unsigned int i);

    bool isLinkBlocked(const unsigned int i);

    void setAllConstraints(bool _constrained);

    void setConstraint(unsigned int i, bool _constrained) { allList[i]->setConstraint(_constrained); }

    bool getConstraint(unsigned int i) { return allList[i]->getConstraint(); }

    void setAllLinkVerbosity(unsigned int _verbose);

    void setVerbosity(unsigned int _verbose) { verbose=_verbose; }

    unsigned int getVerbosity() const { return verbose; }

    unsigned int getN() const { return N; }

    unsigned int getDOF() const { return DOF; }

    yarp::sig::Matrix getH0() const { return H0; }

    bool setH0(const yarp::sig::Matrix &_H0);

    yarp::sig::Matrix getHN() const { return HN; }

    bool setHN(const yarp::sig::Matrix &_HN);

    yarp::sig::Vector setAng(const yarp::sig::Vector &q);

    yarp::sig::Vector getAng();

    double setAng(const unsigned int i, double _Ang);

    double getAng(const unsigned int i);

    yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false);

    yarp::sig::Matrix getH();

    yarp::sig::Matrix getH(const yarp::sig::Vector &q);

    yarp::sig::Vector Pose(const unsigned int i, const bool axisRep=true);

    yarp::sig::Vector Position(const unsigned int i);

    yarp::sig::Vector EndEffPose(const bool axisRep=true);

    yarp::sig::Vector EndEffPose(const yarp::sig::Vector &q, const bool axisRep=true);

    yarp::sig::Vector EndEffPosition();

    yarp::sig::Vector EndEffPosition(const yarp::sig::Vector &q);

    yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col);

    yarp::sig::Matrix AnaJacobian(unsigned int col=3);

    yarp::sig::Matrix AnaJacobian(const yarp::sig::Vector &q, unsigned int col=3);

    yarp::sig::Matrix GeoJacobian(const unsigned int i);

    yarp::sig::Matrix GeoJacobian();

    yarp::sig::Matrix GeoJacobian(const yarp::sig::Vector &q);

    yarp::sig::Vector Hessian_ij(const unsigned int i, const unsigned int j);

    void prepareForHessian();

    yarp::sig::Vector fastHessian_ij(const unsigned int i, const unsigned int j);

    yarp::sig::Vector Hessian_ij(const unsigned int lnk, const unsigned int i,
                                 const unsigned int j);

    void prepareForHessian(const unsigned int lnk);

    yarp::sig::Vector fastHessian_ij(const unsigned int lnk, const unsigned int i,
                                     const unsigned int j);

    yarp::sig::Matrix DJacobian(const yarp::sig::Vector &dq);

    yarp::sig::Matrix DJacobian(const unsigned int lnk, const yarp::sig::Vector &dq);

    virtual ~iKinChain();
};


class iKinLimb : public iKinChain
{
protected:
    std::deque<iKinLink*> linkList;
    std::string           type;

    virtual void getMatrixFromProperties(const yarp::os::Property &options,
                                         const std::string &tag, yarp::sig::Matrix &H);
    virtual void setMatrixToProperties(yarp::os::Property &options,
                                       const std::string &tag, yarp::sig::Matrix &H);
    virtual void allocate(const std::string &_type);
    virtual void clone(const iKinLimb &limb);
    virtual void dispose();

    // make the following methods protected in order to prevent user from changing
    // too easily the internal structure of the chain;
    // to get access anyway to these hidden methods, user can rely on asChain()
    iKinChain &operator=(const iKinChain &c)              { return iKinChain::operator=(c);  }
    iKinChain &operator<<(iKinLink &l)                    { return iKinChain::operator<<(l); }
    iKinChain &operator--(int)                            { return iKinChain::operator--(0); }
    iKinLink  &operator[](const unsigned int i)           { return iKinChain::operator[](i); }
    iKinLink  &operator()(const unsigned int i)           { return iKinChain::operator()(i); }
    bool       addLink(const unsigned int i, iKinLink &l) { return iKinChain::addLink(i,l);  }
    bool       rmLink(const unsigned int i)               { return iKinChain::rmLink(i);     }
    void       pushLink(iKinLink &l)                      { iKinChain::pushLink(l);          }
    void       clear()                                    { iKinChain::clear();              }
    void       popLink()                                  { iKinChain::popLink();            }
    void       pushLink(iKinLink *pl);

public:
    iKinLimb();

    iKinLimb(const std::string &_type);

    iKinLimb(const iKinLimb &limb);

    iKinLimb(const yarp::os::Property &options);

    bool fromLinksProperties(const yarp::os::Property &options);

    bool toLinksProperties(yarp::os::Property &options);

    bool isValid() const { return (N>0); }

    iKinLimb &operator=(const iKinLimb &limb);

    iKinChain *asChain() { return static_cast<iKinChain*>(this); }

    std::string getType() const { return type; }

    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*>&) { notImplemented(verbose); return true; }

    virtual ~iKinLimb();
};


class iCubTorso : public iKinLimb
{
protected:
    double version;

    virtual void allocate(const std::string &_type);

public:
    iCubTorso();

    iCubTorso(const std::string &_type);

    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
};


class iCubArm : public iKinLimb
{
protected:
    double version;

    virtual void allocate(const std::string &_type);

public:
    iCubArm();

    iCubArm(const std::string &_type);

    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
};


class iCubFinger : public iKinLimb
{
protected:
    std::string hand;
    std::string finger;
    std::string version;

    virtual void allocate(const std::string &_type);
    virtual void clone(const iCubFinger &finger);

public:
    iCubFinger();

    iCubFinger(const std::string &_type);

    iCubFinger(const iCubFinger &finger);

    iCubFinger &operator=(const iCubFinger &finger);

    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);

    virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
                                yarp::sig::Vector &chainJoints);

    virtual bool getChainJoints(const yarp::sig::Vector &motorEncoders,
                                const yarp::sig::Vector &jointEncoders,
                                yarp::sig::Vector &chainJoints,
                                const yarp::sig::Matrix &jointEncodersBounds=yarp::math::zeros(1,2));
};


class iCubLeg : public iKinLimb
{
protected:
    double version;

    virtual void allocate(const std::string &_type);

public:
    iCubLeg();

    iCubLeg(const std::string &_type);

    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
};


class iCubEye : public iKinLimb
{
protected:
    double version;

    virtual void allocate(const std::string &_type);

public:
    iCubEye();

    iCubEye(const std::string &_type);

    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
};


class iCubEyeNeckRef : public iCubEye
{
protected:
    virtual void allocate(const std::string &_type);

public:
    iCubEyeNeckRef();

    iCubEyeNeckRef(const std::string &_type);
};


class iCubHeadCenter : public iCubEye
{
protected:
    virtual void allocate(const std::string &_type);

public:
    iCubHeadCenter();

    iCubHeadCenter(const std::string &_type);
};


class iCubInertialSensor : public iKinLimb
{
protected:
    double version;

    virtual void allocate(const std::string &_type);

public:
    iCubInertialSensor();

    iCubInertialSensor(const std::string &_type);

    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
};

}

}

#endif