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 iKinLimbVersion
{
protected:
    unsigned long int v_major;
    unsigned long int v_minor;
 
public:
    iKinLimbVersion();
 
    iKinLimbVersion(const std::string &version);
 
    iKinLimbVersion(const unsigned long int v_major, const unsigned long int v_minor);
 
    iKinLimbVersion(const iKinLimbVersion& v);
 
    unsigned long int get_major() const;
 
    unsigned long int get_minor() const;
 
    std::string get_version() const;
 
    iKinLimbVersion& operator=(const iKinLimbVersion& v);
 
    bool operator<(const iKinLimbVersion& v) const;
 
    bool operator<=(const iKinLimbVersion& v) const;
 
    bool operator==(const iKinLimbVersion& v) const;
 
    bool operator!=(const iKinLimbVersion& v) const;
 
    bool operator>=(const iKinLimbVersion& v) const;
 
    bool operator>(const iKinLimbVersion& v) const;
 
    iKinLimbVersion operator-(const iKinLimbVersion& v) const;
};
 
 
class iCubTorso : public iKinLimb
{
protected:
    iKinLimbVersion 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:
    iKinLimbVersion 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;
    double fingers_abduction_max{0.0};
 
    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:
    iKinLimbVersion 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:
    iKinLimbVersion 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:
    iKinLimbVersion 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);
};
 
 
class iCubInertialSensorWaist : public iKinLimb
{
protected:
    iKinLimbVersion version;
 
    virtual void allocate(const std::string &_type);
 
public:
    iCubInertialSensorWaist();
 
    iCubInertialSensorWaist(const std::string &_type);
};
 
}
 
}
 
#endif