iDyn.h

Go to the documentation of this file.

/*
 * Copyright (C) 2010-2011 RobotCub Consortium
 * Author: Serena Ivaldi, Matteo Fumagalli
 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
 *
 */
 
#ifndef __IDYN_H__
#define __IDYN_H__
 
#include <yarp/os/Property.h>
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
 
#include <iCub/ctrl/math.h>
 
#include <iCub/iKin/iKinFwd.h>
#include <iCub/iDyn/iDynInv.h>
 
#include <deque>
#include <string>
 
 
namespace iCub
{
 
namespace iDyn
{
    void notImplemented(const unsigned int verbose);
    void notImplemented(const unsigned int verbose, const std::string &msg);
    void workInProgress(const unsigned int verbose, const std::string &msg);
    bool asWrench(yarp::sig::Vector &w, const yarp::sig::Vector &f, const yarp::sig::Vector &m);
    yarp::sig::Vector asWrench(const yarp::sig::Vector &f, const yarp::sig::Vector &m);
    bool asForceMoment(const yarp::sig::Vector &w, yarp::sig::Vector &f, yarp::sig::Vector &m);
 
    class OneLinkNewtonEuler;
    class BaseLinkNewtonEuler;
    class FinalLinkNewtonEuler;
    class SensorLinkNewtonEuler;
    class ContactNewtonEuler;
    class iDynContactSolver;
    class OneChainNewtonEuler;
    class OneChainSensorNewtonEuler;
    class iDynSensor;
    class iGenericFrame;
    class iFrameOnLink;
    class iFTransformation;
    class iDynSensorLeg;
    class iDynSensorArm;
    class iCubWholeBody;
 
 
 
class iDynLink : public iKin::iKinLink
{
    friend class iDynChain;
    friend class OneLinkNewtonEuler;
 
protected:
    // DH rototranslation matrix (it's the same matrix you get calling iKinLink->getH(true) but it's stored here for performance reason)
    yarp::sig::Matrix H_store;
    yarp::sig::Matrix R_store;
    yarp::sig::Vector r_store;
    yarp::sig::Vector r_proj_store;
    // flag that is true is if H_store is valid, false otherwise
    bool H_store_valid;
 
    double m;   
    yarp::sig::Matrix HC;
    yarp::sig::Matrix RC;
    yarp::sig::Vector rc, rc_proj;
    yarp::sig::Matrix I;    
    double dq;
    double ddq;                 
    yarp::sig::Vector w;
    yarp::sig::Vector dw;
    yarp::sig::Vector dwM;      
 
    yarp::sig::Vector dp;   
    yarp::sig::Vector dpC;  
    yarp::sig::Vector ddp;  
    yarp::sig::Vector ddpC;     
 
    yarp::sig::Vector F;
    yarp::sig::Vector Mu;   
    double Tau;                 
 
    //only including motor dynamic
 
    double Im;  
    double kr;  
    double Fv;  
    double Fs;                  
 
    iDynLink();
 
    virtual void clone(const iDynLink &l);  
 
    virtual void updateHstore();
 
public:
 
    //~~~~~~~~~~~~~~~~~~~~~~
    //   set methods
    //~~~~~~~~~~~~~~~~~~~~~~
 
     bool setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im);
 
     bool setDynamicParameters(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I);
 
     bool setStaticParameters(const double _m, const yarp::sig::Matrix &_HC);
     
     bool setInertia(const yarp::sig::Matrix &_I);
 
     void setInertia(const double Ixx, const double Ixy, const double Ixz, const double Iyy, const double Iyz, const double Izz);
 
     void setMass(const double _m);
 
    double setAng(const double _teta);
 
     double setDAng(const double _dteta);
    
     double setD2Ang(const double _ddteta);
 
     const yarp::sig::Vector& getLinVel()  const;
 
     const yarp::sig::Vector& getLinVelC() const;
 
     void setAngPosVelAcc(const double _teta,const double _dteta,const double _ddteta);
 
     bool setCOM(const yarp::sig::Matrix &_HC);
     bool setCOM(const yarp::sig::Vector &_rC);
 
     void setCOM(const double _rCx, const double _rCy, const double _rCz);
 
     bool setForce(const yarp::sig::Vector &_F);
 
     bool setMoment(const yarp::sig::Vector &_Mu);
 
     bool setForceMoment(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
 
     void setTorque(const double _Tau);
 
 
     //~~~~~~~~~~~~~~~~~~~~~~
     //   get methods
     //~~~~~~~~~~~~~~~~~~~~~~
    
     const yarp::sig::Matrix&       getInertia()    const;
     double                 getMass()       const;
     double                 getIm()         const;
     double                 getKr()         const;
     double                 getFs()         const;
     double                 getFv()         const;
     const yarp::sig::Matrix&       getCOM()        const;
     double                 getDAng()       const;
     double                 getD2Ang()      const;
     const yarp::sig::Vector&   getW()      const;
     const yarp::sig::Vector&   getdW()     const;
     const yarp::sig::Vector&   getdWM()    const;
     const yarp::sig::Vector&   getLinAcc()     const;
     const yarp::sig::Vector&   getLinAccC()    const;
     const yarp::sig::Vector&   getForce()      const;
     const yarp::sig::Vector&   getMoment()     const;
     double             getTorque()     const;
    const yarp::sig::Matrix&    getH();
    const yarp::sig::Matrix&        getR();
    const yarp::sig::Matrix&        getRC()     const;
 
    const yarp::sig::Vector&        getr(bool proj=false);
    const yarp::sig::Vector&        getrC(bool proj=false) const;
 
     //~~~~~~~~~~~~~~~~~~~~~~
     //   basic functions
     //~~~~~~~~~~~~~~~~~~~~~~
  
    iDynLink(double _A, double _D, double _Alpha, double _Offset, double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
     
    iDynLink(const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, double _A, double _D, double _Alpha, double _Offset, double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
 
    iDynLink(const double _m, const yarp::sig::Vector &_C, const yarp::sig::Matrix &_I, double _A, double _D, double _Alpha, double _Offset, double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
 
    iDynLink(const double _m, const double _rCx, const double _rCy, const double _rCz, const double Ixx, const double Ixy, const double Ixz, const double Iyy, const double Iyz, const double Izz, double _A, double _D, double _Alpha, double _Offset, double _Min=-iCub::ctrl::CTRL_PI, double _Max=iCub::ctrl::CTRL_PI);
 
    iDynLink(const iDynLink &c);
 
    void zero();
     
    iDynLink &operator=(const iDynLink &c);
 
};
 
 
 
class iDynChain : public iKin::iKinChain
{
    friend class OneChainNewtonEuler;
    friend class iDynInvSensor;
    friend class iDynSensor;
    friend class RigidBodyTransformation;
    friend class iDynContactSolver;
 
protected:
    
    ChainIterationMode iterateMode_kinematics;  
    ChainIterationMode iterateMode_wrench;
 
    //curr_q = q pos is already in iKinChain
    yarp::sig::Vector curr_dq;  
    yarp::sig::Vector curr_ddq; 
 
    OneChainNewtonEuler *NE;
 
    const yarp::sig::Vector zero0;
 
    virtual void clone(const iDynChain &c);
 
    virtual void build();
 
    virtual void dispose();
 
    iDynLink * refLink(const unsigned int i);
 
 
public:
 
    iDynChain();
 
    iDynChain(const iDynChain &c);
 
    iDynChain &operator=(const iDynChain &c);
 
    yarp::sig::Vector setAng(const yarp::sig::Vector &q);
 
    yarp::sig::Vector setDAng(const yarp::sig::Vector &dq);
 
    yarp::sig::Vector setD2Ang(const yarp::sig::Vector &ddq);
 
    yarp::sig::Vector getDAng();
 
    yarp::sig::Vector getD2Ang();
 
    yarp::sig::Vector getJointBoundMin();
 
    yarp::sig::Vector getJointBoundMax();
 
    double setAng(const unsigned int i, double q);
 
    double setDAng(const unsigned int i, double dq);
 
    double setD2Ang(const unsigned int i, double ddq);
 
    double getDAng(const unsigned int i);
 
    double getD2Ang(const unsigned int i);
 
    yarp::sig::Vector getMasses() const;
 
    bool setMasses(yarp::sig::Vector _m);
 
    double getMass(const unsigned int i) const;
 
    bool setMass(const unsigned int i, const double _m);
 
    yarp::sig::Matrix getInertia(const unsigned int i) const;
 
    yarp::sig::Matrix getForces() const;
 
    yarp::sig::Matrix getMoments() const;
 
    yarp::sig::Vector getTorques() const;
 
    const yarp::sig::Vector& getForce(const unsigned int iLink) const;
 
    const yarp::sig::Vector& getMoment(const unsigned int iLink) const;
 
    double getTorque(const unsigned int iLink) const;
 
    yarp::sig::Vector getLinVel(const unsigned int i) const;
 
    yarp::sig::Vector getLinVelCOM(const unsigned int i) const;
 
    yarp::sig::Vector getLinAcc(const unsigned int i) const;
 
    const yarp::sig::Vector& getLinAccCOM(const unsigned int i) const;
 
    yarp::sig::Vector getAngVel(const unsigned int i) const;
 
    yarp::sig::Vector getAngAcc (const unsigned int i) const;
 
    bool setDynamicParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I, const double _kr, const double _Fv, const double _Fs, const double _Im);
 
    bool setDynamicParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC, const yarp::sig::Matrix &_I);
 
    bool setStaticParameters(const unsigned int i, const double _m, const yarp::sig::Matrix &_HC);
 
    void prepareNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC);
 
    bool computeNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend);
 
    bool computeNewtonEuler();
 
    bool initNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend);
    
    bool initNewtonEuler();
 
    void setModeNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC);
 
    yarp::sig::Matrix getForcesNewtonEuler() const;
 
    yarp::sig::Matrix getMomentsNewtonEuler() const;
 
    yarp::sig::Vector getTorquesNewtonEuler() const;
 
    yarp::sig::Vector getForceMomentEndEff() const;
 
    void setIterModeKinematic(const ChainIterationMode _iterateMode_kinematics = FORWARD );
 
    void setIterModeWrench(const ChainIterationMode _iterateMode_wrench = BACKWARD );
 
    void setIterMode(const ChainComputationMode mode = KINFWD_WREBWD);
 
    ChainIterationMode getIterModeKinematic() const;
 
    ChainIterationMode getIterModeWrench() const;
 
    void computeKinematicNewtonEuler();
 
    void computeWrenchNewtonEuler();
 
    bool initKinematicNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
 
    bool initWrenchNewtonEuler(const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend);
 
    void getKinematicNewtonEuler( yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp);
 
    void getFrameKinematic(unsigned int i, yarp::sig::Vector &w, yarp::sig::Vector &dw, yarp::sig::Vector &ddp);
 
    void getFrameWrench(unsigned int i, yarp::sig::Vector &F, yarp::sig::Vector &Mu);
    void getWrenchNewtonEuler( yarp::sig::Vector &F,  yarp::sig::Vector &Mu);
    
    virtual ~iDynChain();
 
 
    //------------
    // jacobians
    //------------
 
    yarp::sig::Matrix computeGeoJacobian(const unsigned int iLinkN , const yarp::sig::Matrix &Pn );
 
    yarp::sig::Matrix computeGeoJacobian(const unsigned int iLinkN, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0 );
    
    yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn );
 
    yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0 );
 
    yarp::sig::Matrix getDenHart(unsigned int i);
 
    //---------------
    // jacobians COM
    //---------------
 
    yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink);
 
    yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn);
 
    yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0 );
 
    yarp::sig::Matrix getCOM(unsigned int iLink);
 
    yarp::sig::Matrix getHCOM(unsigned int iLink);
 
 
    //---------------------------
    // Lagrange Formulation
    //---------------------------
 
    yarp::sig::Matrix computeMassMatrix();
 
    yarp::sig::Matrix computeMassMatrix(const yarp::sig::Vector& q);
 
    yarp::sig::Vector computeCcTorques();
 
    yarp::sig::Vector computeCcTorques(const yarp::sig::Vector& q, const yarp::sig::Vector& dq);
 
    yarp::sig::Vector computeGravityTorques(const yarp::sig::Vector& ddp0);
 
    yarp::sig::Vector computeGravityTorques(const yarp::sig::Vector& ddp0, const yarp::sig::Vector& q);
 
    yarp::sig::Vector computeCcGravityTorques(const yarp::sig::Vector& ddp0);
 
    yarp::sig::Vector computeCcGravityTorques(const yarp::sig::Vector& ddp0, const yarp::sig::Vector& q, const yarp::sig::Vector& dq);
 
 
 
};
 
 
 
 
 
 
class iDynLimb : public iDynChain
{
protected:
    std::deque<iDynLink*> linkList;
    std::string           type;
    bool                  configured;
 
    virtual void allocate(const std::string &_type);
    virtual void clone(const iDynLimb &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()
    iDynChain       &operator=(const iDynChain &c)                    { return iDynChain::operator=(c);        }
    iKin::iKinChain &operator<<(iKin::iKinLink &l)                    { return iKinChain::operator<<(l);       }
    iKin::iKinChain &operator--(int)                                  { return iKin::iKinChain::operator--(0); }
    iKin::iKinLink  &operator[](const unsigned int i)                 { return iKin::iKinChain::operator[](i); }
    iKin::iKinLink  &operator()(const unsigned int i)                 { return iKin::iKinChain::operator()(i); }
    bool             addLink(const unsigned int i, iKin::iKinLink &l) { return iKin::iKinChain::addLink(i,l);  }
    bool             rmLink(const unsigned int i)                     { return iKin::iKinChain::rmLink(i);     }
    void             pushLink(iKin::iKinLink &l)                      { iKin::iKinChain::pushLink(l);          }
    void             clear()                                          { iKin::iKinChain::clear();              }
    void             popLink()                                        { iKin::iKinChain::popLink();            }
    void             pushLink(iDynLink *pl);
 
public:
    iDynLimb();
 
    iDynLimb(const std::string &_type);
 
    iDynLimb(const iDynLimb &limb);
 
    iDynLimb(const yarp::os::Property &option);
 
    bool fromLinksProperties(const yarp::os::Property &option);
 
    bool isValid() { return configured; }
 
    iDynLimb &operator=(const iDynLimb &limb);
 
    iDynChain *asChain() { return static_cast<iDynChain*>(this); }
 
    std::string getType() { return type; }
 
    virtual ~iDynLimb();
 
    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim)
    { notImplemented(verbose); return true; }
 
};
 
 
class iCubArmDyn : public iDynLimb
{
protected:
    virtual void allocate(const std::string &_type);
 
public:
    iCubArmDyn();
 
    iCubArmDyn(const std::string &_type, const ChainComputationMode _mode=KINFWD_WREBWD);
 
    iCubArmDyn(const iCubArmDyn &arm);
 
    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
 
};
 
 
 
class iCubArmNoTorsoDyn : public iDynLimb
{
protected:
    virtual void allocate(const std::string &_type);
 
public:
    iCubArmNoTorsoDyn();
 
    iCubArmNoTorsoDyn(const std::string &_type, const ChainComputationMode _mode=KINFWD_WREBWD);
 
    iCubArmNoTorsoDyn(const iCubArmNoTorsoDyn &arm);
 
    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
 
};
 
 
class iCubTorsoDyn : public iDynLimb
{
protected:
    virtual void allocate(const std::string &_type);
 
public:
    iCubTorsoDyn();
 
    iCubTorsoDyn(const std::string &_type, const ChainComputationMode _mode=KINFWD_WREBWD);
 
    iCubTorsoDyn(const iCubTorsoDyn &torso);
 
    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
 
 
};
 
 
class iCubLegDyn : public iDynLimb
{
protected:
    virtual void allocate(const std::string &_type);
 
public:
    iCubLegDyn();
 
    iCubLegDyn(const std::string &_type,const ChainComputationMode _mode=KINFWD_WREBWD);
 
    iCubLegDyn(const iCubLegDyn &leg);
 
    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
 
};
 
class iCubLegDynV2 : public iDynLimb
{
protected:
    virtual void allocate(const std::string &_type);
 
public:
    iCubLegDynV2();
 
    iCubLegDynV2(const std::string &_type,const ChainComputationMode _mode=KINFWD_WREBWD);
 
    iCubLegDynV2(const iCubLegDyn &leg);
 
    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
 
};
 
 
class iCubNeckInertialDyn : public iDynLimb
{
protected:
    virtual void allocate(const std::string &_type);
 
public:
    iCubNeckInertialDyn(const ChainComputationMode _mode=KINBWD_WREBWD);
 
    iCubNeckInertialDyn(const iCubNeckInertialDyn &sensor);
 
    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
 
};
 
class iCubNeckInertialDynV2 : public iDynLimb
{
protected:
    virtual void allocate(const std::string &_type);
 
public:
    iCubNeckInertialDynV2(const ChainComputationMode _mode=KINBWD_WREBWD, const std::string &_type ="v2.5");
 
    iCubNeckInertialDynV2(const iCubNeckInertialDynV2 &sensor);
 
    virtual bool alignJointsBounds(const std::deque<yarp::dev::IControlLimits*> &lim);
 
};
 
}
 
}//end namespace
 
#endif