inverse_dynamics.h

Go to the documentation of this file.

#ifndef IIT_TESTHYQ_INVERSE_DYNAMICS_H_
#define IIT_TESTHYQ_INVERSE_DYNAMICS_H_

#include <iit/rbd/rbd.h>
#include <iit/rbd/InertiaMatrix.h>
#include <iit/rbd/utils.h>
#include <iit/rbd/robcogen_commons.h>
#include <iit/rbd/traits/DoubleTrait.h>

#include "declarations.h"
#include "inertia_properties.h"
#include "transforms.h"
#include "link_data_map.h"

namespace iit {
namespace TestHyQ {
namespace dyn {

namespace tpl {

template <typename TRAIT>
class InverseDynamics {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef typename TRAIT::Scalar Scalar;

    typedef iit::rbd::Core<Scalar> CoreS;

    typedef typename CoreS::ForceVector Force;
    typedef LinkDataMap<Force> ExtForces;
    typedef typename CoreS::VelocityVector Velocity;
    typedef typename CoreS::VelocityVector Acceleration;
    typedef iit::rbd::tpl::InertiaMatrixDense<Scalar> InertiaMatrix;
    typedef iit::TestHyQ::tpl::JointState<Scalar> JointState;
    typedef typename CoreS::Matrix66 Matrix66s;
    typedef iit::TestHyQ::tpl::MotionTransforms<TRAIT> MTransforms;
    typedef InertiaProperties<TRAIT> IProperties;
            
public:
    InverseDynamics(IProperties& in, MTransforms& tr);
 
    void id(
        JointState& jForces, Acceleration& trunk_a,
        const Acceleration& g, const Velocity& trunk_v,
        const JointState& q, const JointState& qd, const JointState& qdd,
        const ExtForces& fext = zeroExtForces);
    void id(
        JointState& jForces, Acceleration& trunk_a,
        const Acceleration& g, const Velocity& trunk_v,
        const JointState& qd, const JointState& qdd,
        const ExtForces& fext = zeroExtForces);
 
    void id_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Acceleration& g, const Velocity& trunk_v, const Acceleration& baseAccel,
        const JointState& q, const JointState& qd, const JointState& qdd, const ExtForces& fext = zeroExtForces);
    void id_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Acceleration& g, const Velocity& trunk_v, const Acceleration& baseAccel,
        const JointState& qd, const JointState& qdd, const ExtForces& fext = zeroExtForces);

    void G_terms_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Acceleration& g, const JointState& q);
    void G_terms_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Acceleration& g);

    void C_terms_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Velocity& trunk_v, const JointState& q, const JointState& qd);
    void C_terms_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Velocity& trunk_v, const JointState& qd);

    void setJointStatus(const JointState& q) const;

public:
    const Force& getForce_trunk() const { return trunk_f; }
    const Velocity& getVelocity_LF_hipassembly() const { return LF_hipassembly_v; }
    const Acceleration& getAcceleration_LF_hipassembly() const { return LF_hipassembly_a; }
    const Force& getForce_LF_hipassembly() const { return LF_hipassembly_f; }
    const Velocity& getVelocity_LF_upperleg() const { return LF_upperleg_v; }
    const Acceleration& getAcceleration_LF_upperleg() const { return LF_upperleg_a; }
    const Force& getForce_LF_upperleg() const { return LF_upperleg_f; }
    const Velocity& getVelocity_LF_lowerleg() const { return LF_lowerleg_v; }
    const Acceleration& getAcceleration_LF_lowerleg() const { return LF_lowerleg_a; }
    const Force& getForce_LF_lowerleg() const { return LF_lowerleg_f; }
    const Velocity& getVelocity_RF_hipassembly() const { return RF_hipassembly_v; }
    const Acceleration& getAcceleration_RF_hipassembly() const { return RF_hipassembly_a; }
    const Force& getForce_RF_hipassembly() const { return RF_hipassembly_f; }
    const Velocity& getVelocity_RF_upperleg() const { return RF_upperleg_v; }
    const Acceleration& getAcceleration_RF_upperleg() const { return RF_upperleg_a; }
    const Force& getForce_RF_upperleg() const { return RF_upperleg_f; }
    const Velocity& getVelocity_RF_lowerleg() const { return RF_lowerleg_v; }
    const Acceleration& getAcceleration_RF_lowerleg() const { return RF_lowerleg_a; }
    const Force& getForce_RF_lowerleg() const { return RF_lowerleg_f; }
    const Velocity& getVelocity_LH_hipassembly() const { return LH_hipassembly_v; }
    const Acceleration& getAcceleration_LH_hipassembly() const { return LH_hipassembly_a; }
    const Force& getForce_LH_hipassembly() const { return LH_hipassembly_f; }
    const Velocity& getVelocity_LH_upperleg() const { return LH_upperleg_v; }
    const Acceleration& getAcceleration_LH_upperleg() const { return LH_upperleg_a; }
    const Force& getForce_LH_upperleg() const { return LH_upperleg_f; }
    const Velocity& getVelocity_LH_lowerleg() const { return LH_lowerleg_v; }
    const Acceleration& getAcceleration_LH_lowerleg() const { return LH_lowerleg_a; }
    const Force& getForce_LH_lowerleg() const { return LH_lowerleg_f; }
    const Velocity& getVelocity_RH_hipassembly() const { return RH_hipassembly_v; }
    const Acceleration& getAcceleration_RH_hipassembly() const { return RH_hipassembly_a; }
    const Force& getForce_RH_hipassembly() const { return RH_hipassembly_f; }
    const Velocity& getVelocity_RH_upperleg() const { return RH_upperleg_v; }
    const Acceleration& getAcceleration_RH_upperleg() const { return RH_upperleg_a; }
    const Force& getForce_RH_upperleg() const { return RH_upperleg_f; }
    const Velocity& getVelocity_RH_lowerleg() const { return RH_lowerleg_v; }
    const Acceleration& getAcceleration_RH_lowerleg() const { return RH_lowerleg_a; }
    const Force& getForce_RH_lowerleg() const { return RH_lowerleg_f; }
protected:
    void secondPass_fullyActuated(JointState& jForces);

private:
    IProperties* inertiaProps;
    MTransforms* xm;
private:
    Matrix66s vcross; // support variable
    // Link 'LF_hipassembly' :
    const InertiaMatrix& LF_hipassembly_I;
    Velocity      LF_hipassembly_v;
    Acceleration  LF_hipassembly_a;
    Force         LF_hipassembly_f;
    // Link 'LF_upperleg' :
    const InertiaMatrix& LF_upperleg_I;
    Velocity      LF_upperleg_v;
    Acceleration  LF_upperleg_a;
    Force         LF_upperleg_f;
    // Link 'LF_lowerleg' :
    const InertiaMatrix& LF_lowerleg_I;
    Velocity      LF_lowerleg_v;
    Acceleration  LF_lowerleg_a;
    Force         LF_lowerleg_f;
    // Link 'RF_hipassembly' :
    const InertiaMatrix& RF_hipassembly_I;
    Velocity      RF_hipassembly_v;
    Acceleration  RF_hipassembly_a;
    Force         RF_hipassembly_f;
    // Link 'RF_upperleg' :
    const InertiaMatrix& RF_upperleg_I;
    Velocity      RF_upperleg_v;
    Acceleration  RF_upperleg_a;
    Force         RF_upperleg_f;
    // Link 'RF_lowerleg' :
    const InertiaMatrix& RF_lowerleg_I;
    Velocity      RF_lowerleg_v;
    Acceleration  RF_lowerleg_a;
    Force         RF_lowerleg_f;
    // Link 'LH_hipassembly' :
    const InertiaMatrix& LH_hipassembly_I;
    Velocity      LH_hipassembly_v;
    Acceleration  LH_hipassembly_a;
    Force         LH_hipassembly_f;
    // Link 'LH_upperleg' :
    const InertiaMatrix& LH_upperleg_I;
    Velocity      LH_upperleg_v;
    Acceleration  LH_upperleg_a;
    Force         LH_upperleg_f;
    // Link 'LH_lowerleg' :
    const InertiaMatrix& LH_lowerleg_I;
    Velocity      LH_lowerleg_v;
    Acceleration  LH_lowerleg_a;
    Force         LH_lowerleg_f;
    // Link 'RH_hipassembly' :
    const InertiaMatrix& RH_hipassembly_I;
    Velocity      RH_hipassembly_v;
    Acceleration  RH_hipassembly_a;
    Force         RH_hipassembly_f;
    // Link 'RH_upperleg' :
    const InertiaMatrix& RH_upperleg_I;
    Velocity      RH_upperleg_v;
    Acceleration  RH_upperleg_a;
    Force         RH_upperleg_f;
    // Link 'RH_lowerleg' :
    const InertiaMatrix& RH_lowerleg_I;
    Velocity      RH_lowerleg_v;
    Acceleration  RH_lowerleg_a;
    Force         RH_lowerleg_f;

    // The robot base
    const InertiaMatrix& trunk_I;
    InertiaMatrix trunk_Ic;
    Force         trunk_f;
    // The composite inertia tensors
    InertiaMatrix LF_hipassembly_Ic;
    InertiaMatrix LF_upperleg_Ic;
    const InertiaMatrix& LF_lowerleg_Ic;
    InertiaMatrix RF_hipassembly_Ic;
    InertiaMatrix RF_upperleg_Ic;
    const InertiaMatrix& RF_lowerleg_Ic;
    InertiaMatrix LH_hipassembly_Ic;
    InertiaMatrix LH_upperleg_Ic;
    const InertiaMatrix& LH_lowerleg_Ic;
    InertiaMatrix RH_hipassembly_Ic;
    InertiaMatrix RH_upperleg_Ic;
    const InertiaMatrix& RH_lowerleg_Ic;

private:
    static const ExtForces zeroExtForces;
};

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::setJointStatus(const JointState& q) const
{
    (xm->fr_LF_hipassembly_X_fr_trunk)(q);
    (xm->fr_LF_upperleg_X_fr_LF_hipassembly)(q);
    (xm->fr_LF_lowerleg_X_fr_LF_upperleg)(q);
    (xm->fr_RF_hipassembly_X_fr_trunk)(q);
    (xm->fr_RF_upperleg_X_fr_RF_hipassembly)(q);
    (xm->fr_RF_lowerleg_X_fr_RF_upperleg)(q);
    (xm->fr_LH_hipassembly_X_fr_trunk)(q);
    (xm->fr_LH_upperleg_X_fr_LH_hipassembly)(q);
    (xm->fr_LH_lowerleg_X_fr_LH_upperleg)(q);
    (xm->fr_RH_hipassembly_X_fr_trunk)(q);
    (xm->fr_RH_upperleg_X_fr_RH_hipassembly)(q);
    (xm->fr_RH_lowerleg_X_fr_RH_upperleg)(q);
}

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::id(
    JointState& jForces, Acceleration& trunk_a,
    const Acceleration& g, const Velocity& trunk_v,
    const JointState& q, const JointState& qd, const JointState& qdd,
    const ExtForces& fext)
{
    setJointStatus(q);
    id(jForces, trunk_a, g, trunk_v,
       qd, qdd, fext);
}

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::G_terms_fully_actuated(
    Force& baseWrench, JointState& jForces,
    const Acceleration& g, const JointState& q)
{
    setJointStatus(q);
    G_terms_fully_actuated(baseWrench, jForces, g);
}

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::C_terms_fully_actuated(
    Force& baseWrench, JointState& jForces,
    const Velocity& trunk_v, const JointState& q, const JointState& qd)
{
    setJointStatus(q);
    C_terms_fully_actuated(baseWrench, jForces, trunk_v, qd);
}

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::id_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Acceleration& g, const Velocity& trunk_v, const Acceleration& baseAccel,
        const JointState& q, const JointState& qd, const JointState& qdd, const ExtForces& fext)
{
    setJointStatus(q);
    id_fully_actuated(baseWrench, jForces, g, trunk_v,
        baseAccel, qd, qdd, fext);
}

}

typedef tpl::InverseDynamics<rbd::DoubleTrait> InverseDynamics;

}
}

}

#include "inverse_dynamics.impl.h"

#endif