iDynBody.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 __IDYNBODY_H__
#define __IDYNBODY_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 <iCub/iDyn/iDynContact.h>
#include <deque>
#include <string>
 
 
namespace iCub
{
 
namespace iDyn
{
 
    class OneLinkNewtonEuler;
    class BaseLinkNewtonEuler;
    class FinalLinkNewtonEuler;
    class SensorLinkNewtonEuler;
    class ContactNewtonEuler;
    class OneChainNewtonEuler;
    class OneChainSensorNewtonEuler;
    class iDynSensor;
    class iFTransform;
    class iSFrame;
    class iFB;
    class iDynSensorLeg;
    class iDynSensorArm;
 
 
 
// Interaction type
// used to define what side of the chain (limb) is attached to the node
// the base or the end-effector
// note: up, arm and legs are attached with the base
// torso is attached with the end-effector to the upper arm, and with the 
enum InteractionType { RBT_BASE, RBT_ENDEFF };
 
// Flow Type
// define the flow of kinematic/wrench information for each RBT in a node
// RBT_NODE_IN = the variable is set from outside, "propagates" into the limb and then goes into the node
// RBT_NODE_OUT = the variable is read from the node and goes into the limb
enum FlowType{ RBT_NODE_IN, RBT_NODE_OUT };
 
// Jacobian Flow
// used to specify if the direction of computation of the Jacobian is the same as 
// the one of the kinematics of the chain
// JAC_KIN = same flow of kinematics
// JAC_IKIN = inverse flow wrt kinematics
enum JacobType{ JAC_KIN, JAC_IKIN }; 
 
 
#define RBT_HAS_SENSOR      true
#define RBT_NO_SENSOR       false
#define NODE_AFTER_ATTACH   true
#define NODE_NO_ATTACH      false
 
 
struct version_tag
{
    int head_version;
    int head_subversion;
    int legs_version;
    
    version_tag () 
    {
        head_version=1;
        head_subversion=0;
        legs_version=1;
    }
};
 
//enum partEnum{ LEFT_ARM=0, RIGHT_ARM, LEFT_LEG, RIGHT_LEG, TORSO, HEAD, ALL }; 
 
class RigidBodyTransformation
{
protected:
 
    iDyn::iDynLimb *limb;
 
    FlowType kinFlow;
 
    FlowType wreFlow;
 
    yarp::sig::Matrix H;
 
    NewEulMode mode;    
    
    std::string info;
    
    unsigned int verbose;
 
    bool hasSensor;
 
    // these variables are not redundant: because multiple RBT can be attached 
    // to a node, and different policies of information sharing can exist
 
    yarp::sig::Vector w;    
    yarp::sig::Vector dw;
    yarp::sig::Vector ddp;  
    yarp::sig::Vector F;    
    yarp::sig::Vector Mu;   
        
    
    // useful methods
 
    yarp::sig::Matrix   getR(); 
    
    yarp::sig::Vector   getr(bool proj=false);
 
    void computeKinematic();
 
    void computeWrench();
 
 
public:
 
    RigidBodyTransformation(iDyn::iDynLimb *_limb, const yarp::sig::Matrix &_H, const std::string &_info, bool _hasSensor = false, const FlowType kin=RBT_NODE_OUT, const FlowType wre=RBT_NODE_IN, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
 
    ~RigidBodyTransformation();
 
    bool setRBT(const yarp::sig::Matrix &_H);
 
    bool setKinematic(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0); 
 
    bool setKinematicMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0); 
 
    bool setWrench(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0); 
 
    bool setWrenchMeasure(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0); 
 
    bool setWrenchMeasure(iDyn::iDynSensor *sensor, const yarp::sig::Vector &Fsens, const yarp::sig::Vector &Musens); 
 
    yarp::sig::Matrix getRBT() const;
    
    void getKinematic( yarp::sig::Vector &wNode, yarp::sig::Vector &dwNode, yarp::sig::Vector &ddpNode); 
    
    void getWrench( yarp::sig::Vector &FNode,  yarp::sig::Vector &MuNode); 
 
    void setInfoFlow(const FlowType kin, const FlowType wre);
 
    FlowType getKinematicFlow() const;
    
    FlowType getWrenchFlow() const;
 
    std::string toString() const;
 
    bool isSensorized() const;
 
    void computeLimbKinematic();
 
    void computeLimbWrench();
 
    unsigned int getNLinks() const;
 
    unsigned int getDOF() const;
 
    yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false);        
    
    yarp::sig::Matrix getH(); 
 
    yarp::sig::Vector getEndEffPose(const bool axisRep = true);
 
    yarp::sig::Matrix computeGeoJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, bool rbtRoto = false);
 
    yarp::sig::Matrix computeGeoJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &H0, bool rbtRoto = false);
 
    yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn, bool rbtRoto = false);
 
    yarp::sig::Matrix computeGeoJacobian(const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &H0, bool rbtRoto = false);
 
    yarp::sig::Matrix computeGeoJacobian(bool rbtRoto = false);
 
    yarp::sig::Matrix computeGeoJacobian(const unsigned int iLink, bool rbtRoto = false);
 
    yarp::sig::Matrix   getR6() const;
 
    yarp::sig::Matrix getH0() const;
 
    bool setH0(const yarp::sig::Matrix &_H0);
 
    //---------------
    //   JAC COM
    //---------------
 
    yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, bool rbtRoto = false);
 
    yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, bool rbtRoto = false);
 
    yarp::sig::Matrix TESTING_computeCOMJacobian(const unsigned int iLink, const yarp::sig::Matrix &Pn, const yarp::sig::Matrix &_H0, bool rbtRoto = false );
 
    yarp::sig::Matrix getHCOM(unsigned int iLink);
 
};
 
 
class iDynNode
{
protected:
 
    std::deque<RigidBodyTransformation> rbtList;
 
    NewEulMode mode;    
    
    std::string info;
    
    unsigned int verbose;
 
    yarp::sig::Vector w;    
    yarp::sig::Vector dw;
    yarp::sig::Vector ddp;  
    yarp::sig::Vector F;    
    yarp::sig::Vector Mu;   
    yarp::sig::Vector COM;  
    double mass;
 
    void zero();
 
    void compute_Pn_HAN(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node);
 
    void compute_Pn_HAN(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node);
 
    void compute_Pn_HAN_COM(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, yarp::sig::Matrix &Pn, yarp::sig::Matrix &H_A_Node);
 
    unsigned int howManyWrenchInputs(bool afterAttach=false) const;
 
    unsigned int howManyKinematicInputs(bool afterAttach=false) const;
 
public:
 
    iDynNode(const NewEulMode _mode=DYNAMIC);
 
    iDynNode(const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
 
    virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN, bool hasSensor=false);
 
    yarp::sig::Matrix getRBT(unsigned int iLimb) const;
 
    bool solveKinematics( const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
 
    bool solveKinematics();
 
    bool setKinematicMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
 
    virtual bool solveWrench();
 
    bool solveWrench(const yarp::sig::Matrix &FM);
 
    bool solveWrench(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M);
 
    virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M);
 
    virtual bool setWrenchMeasure(const yarp::sig::Matrix &FM);
 
    yarp::sig::Vector getForce() const;
    
    yarp::sig::Vector getMoment() const;
    
    yarp::sig::Vector getAngVel() const;
 
    yarp::sig::Vector getAngAcc() const;
 
    yarp::sig::Vector getLinAcc() const;
 
        //-----------------
        //    jacobians
        //-----------------
 
    yarp::sig::Matrix computeJacobian(unsigned int iChain);
 
    yarp::sig::Matrix computeJacobian(unsigned int iChain, unsigned int iLink);
 
    yarp::sig::Matrix computeJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB);
 
    yarp::sig::Matrix computeJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB);
 
    yarp::sig::Vector computePose(unsigned int iChainA, JacobType dirA, unsigned int iChainB, JacobType dirB, const bool axisRep);
 
    yarp::sig::Vector computePose(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB, const bool axisRep);
 
    
    //---------------
    //   JAC COM
    //---------------
 
    yarp::sig::Matrix TESTING_computeCOMJacobian(unsigned int iChain, unsigned int iLink);
 
    yarp::sig::Matrix TESTING_computeCOMJacobian(unsigned int iChainA, JacobType dirA, unsigned int iChainB, unsigned int iLinkB, JacobType dirB);
};
 
 
class iDynSensorNode : public iDynNode
{
 
protected:
 
    std::deque<iDyn::iDynSensor *> sensorList;
 
    unsigned int howManySensors() const;
 
public:
 
    iDynSensorNode(const NewEulMode _mode=DYNAMIC);
 
    iDynSensorNode(const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
 
    virtual void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN);
 
    void addLimb(iDyn::iDynLimb *limb, const yarp::sig::Matrix &H, iDyn::iDynSensor *sensor, const FlowType kinFlow=RBT_NODE_OUT, const FlowType wreFlow=RBT_NODE_IN);
 
    virtual bool solveWrench();
 
    virtual bool setWrenchMeasure(const yarp::sig::Matrix &F, const yarp::sig::Matrix &M, bool afterAttach=false);
 
    virtual bool setWrenchMeasure(const yarp::sig::Matrix &FM, bool afterAttach=false);
 
    yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false);
 
 
 
};
 
 
class iDynSensorTorsoNode : public iDynSensorNode
{
public:
 
    yarp::sig::Matrix HUp;
    yarp::sig::Matrix HLeft;
    yarp::sig::Matrix HRight;
 
    std::string left_name;
    std::string right_name;
    std::string up_name;
    std::string name;
 
    //virtual void build();
 
public:
    iDyn::iDynContactSolver * leftSensor;
    iDyn::iDynContactSolver * rightSensor;
 
 
    iDyn::iDynLimb * left;
    iDyn::iDynLimb * right;
    iDyn::iDynLimb * up;
 
    yarp::sig::Vector total_COM_UP;
    yarp::sig::Vector total_COM_LF;
    yarp::sig::Vector total_COM_RT;
    double            total_mass_UP;
    double            total_mass_LF;
    double            total_mass_RT;
    yarp::sig::Matrix COM_jacob_UP;
    yarp::sig::Matrix COM_jacob_LF;
    yarp::sig::Matrix COM_jacob_RT;
 
    iDynSensorTorsoNode(const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
 
    iDynSensorTorsoNode(const std::string &_info, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
 
    ~iDynSensorTorsoNode();
 
    bool update();
 
    bool update(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, bool afterAttach=true);
    
    bool update(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, const yarp::sig::Vector &FM_up);
 
 
    //----------------
    //      GET
    //----------------
    inline yarp::sig::Matrix getHLeft() { return HLeft; }   
    yarp::sig::Matrix getHRight() { return HRight; }    
    yarp::sig::Matrix getHUp() { return HUp; }  
 
    yarp::sig::Matrix getForces(const std::string &limbType);   
 
    yarp::sig::Vector getForce(const std::string &limbType, const unsigned int iLink) const ;
 
    yarp::sig::Matrix getMoments(const std::string &limbType);
 
    yarp::sig::Vector getMoment(const std::string &limbType, const unsigned int iLink) const;
 
    yarp::sig::Vector getTorques(const std::string &limbType);
 
    double getTorque(const std::string &limbType, const unsigned int iLink) const;
 
    bool computeCOM();
 
    bool EXPERIMENTAL_computeCOMjacobian();
 
    yarp::sig::Vector getTorsoForce() const;
    yarp::sig::Vector getTorsoMoment() const;   
    yarp::sig::Vector getTorsoAngVel() const;
    yarp::sig::Vector getTorsoAngAcc() const;
    yarp::sig::Vector getTorsoLinAcc() const;
 
 
    //----------------
    //      SET
    //----------------
 
    bool setInertialMeasure(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0);
    
    bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left);
 
    bool setSensorMeasurement(const yarp::sig::Vector &FM_right, const yarp::sig::Vector &FM_left, const yarp::sig::Vector &FM_up);
 
 
    //------------------
    //    LIMB CALLS
    //------------------
 
    yarp::sig::Vector setAng(const std::string &limbType, const yarp::sig::Vector &_q);
    yarp::sig::Vector getAng(const std::string &limbType);
    double            setAng(const std::string &limbType, const unsigned int i, double _q);
    double            getAng(const std::string &limbType, const unsigned int i);
 
    yarp::sig::Vector setDAng(const std::string &limbType, const yarp::sig::Vector &_dq);
    yarp::sig::Vector getDAng(const std::string &limbType);
    double            setDAng(const std::string &limbType, const unsigned int i, double _dq);
    double            getDAng(const std::string &limbType, const unsigned int i);                                  
 
    yarp::sig::Vector setD2Ang(const std::string &limbType, const yarp::sig::Vector &_ddq);
    yarp::sig::Vector getD2Ang(const std::string &limbType);
    double            setD2Ang(const std::string &limbType, const unsigned int i, double _ddq);
    double            getD2Ang(const std::string &limbType, const unsigned int i);
 
    unsigned int      getNLinks(const std::string &limbType) const;
 
    yarp::sig::Matrix estimateSensorsWrench(const yarp::sig::Matrix &FM, bool afterAttach=false) 
    { return iDynSensorNode::estimateSensorsWrench(FM,afterAttach); }
 
 
    void clearContactList();
 
    
};
 
 
 
class iCubUpperTorso : public iDynSensorTorsoNode
{
protected:
    version_tag tag;    
    void build();
 
public:
 
    iCubUpperTorso(version_tag tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
    
    ~iCubUpperTorso();
 
};
 
 
class iCubLowerTorso : public iDynSensorTorsoNode
{
protected:
    version_tag tag;
    void build();
 
public:
 
    iCubLowerTorso(version_tag _tag, const NewEulMode _mode=DYNAMIC, unsigned int verb=iCub::skinDynLib::VERBOSE);
    
    ~iCubLowerTorso();
 
};
 
 
class iCubWholeBody
{
protected:
    RigidBodyTransformation * rbt;
    version_tag tag;
 
public:
 
    iCubUpperTorso * upperTorso;
    iCubLowerTorso * lowerTorso;
    
    double whole_mass;
    double lower_mass;
    double upper_mass;
    yarp::sig::Vector whole_COM;
    yarp::sig::Vector upper_COM;
    yarp::sig::Vector lower_COM;
    yarp::sig::Matrix COM_Jacob;
    double sw_getcom;
    iCubWholeBody(version_tag _tag, const NewEulMode mode=DYNAMIC, unsigned int verbose=iCub::skinDynLib::VERBOSE);
 
    ~iCubWholeBody();
 
    void attachLowerTorso(const yarp::sig::Vector &FM_right_leg, const yarp::sig::Vector &FM_left_leg);
 
    bool computeCOM();
 
    bool getCOM(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &COM, double & mass);
 
    bool getAllVelocities(yarp::sig::Vector &vel);
    
    bool getAllPositions(yarp::sig::Vector &pos);
    
    bool EXPERIMENTAL_getCOMjacobian(iCub::skinDynLib::BodyPart which_part, yarp::sig::Matrix &jac);
 
    bool EXPERIMENTAL_computeCOMjacobian();
 
    bool EXPERIMENTAL_getCOMvelocity(iCub::skinDynLib::BodyPart which_part, yarp::sig::Vector &vel, yarp::sig::Vector &dq);
};
 
 
}// end of namespace iDyn
}//end of namespace iCub
 
#endif