inverse_dynamics.h

Go to the documentation of this file.

#ifndef IIT_CT_DOUBLEINVERTEDPENDULUM_INVERSE_DYNAMICS_H_
#define IIT_CT_DOUBLEINVERTEDPENDULUM_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 ct_DoubleInvertedPendulum {
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 typename CoreS::VelocityVector Velocity;
    typedef typename CoreS::VelocityVector Acceleration;
    typedef typename CoreS::Matrix66 Matrix66s;
    typedef iit::rbd::tpl::InertiaMatrixDense<Scalar> InertiaMatrix;
    typedef iit::ct_DoubleInvertedPendulum::tpl::JointState<Scalar> JointState;
    typedef LinkDataMap<Force> ExtForces;
    typedef iit::ct_DoubleInvertedPendulum::tpl::MotionTransforms<TRAIT> MTransforms;
    typedef InertiaProperties<TRAIT> IProperties;

public:
    InverseDynamics(IProperties& in, MTransforms& tr);

    void id(
        JointState& jForces,
        const JointState& q, const JointState& qd, const JointState& qdd,
        const ExtForces& fext = zeroExtForces);
    void id(
        JointState& jForces,
        const JointState& qd, const JointState& qdd,
        const ExtForces& fext = zeroExtForces);

    void G_terms(JointState& jForces, const JointState& q);
    void G_terms(JointState& jForces);

    void C_terms(JointState& jForces, const JointState& q, const JointState& qd);
    void C_terms(JointState& jForces, const JointState& qd);

    void setJointStatus(const JointState& q) const;

public:
    const Velocity& getVelocity_Link1() const { return Link1_v; }
    const Acceleration& getAcceleration_Link1() const { return Link1_a; }
    const Force& getForce_Link1() const { return Link1_f; }
    const Velocity& getVelocity_Link2() const { return Link2_v; }
    const Acceleration& getAcceleration_Link2() const { return Link2_a; }
    const Force& getForce_Link2() const { return Link2_f; }
protected:
    void firstPass(const JointState& qd, const JointState& qdd, const ExtForces& fext);
    void secondPass(JointState& jForces);

private:
    IProperties* inertiaProps;
    MTransforms* xm;
private:
    Matrix66s vcross; // support variable
    // Link 'Link1' :
    const InertiaMatrix& Link1_I;
    Velocity      Link1_v;
    Acceleration  Link1_a;
    Force         Link1_f;
    // Link 'Link2' :
    const InertiaMatrix& Link2_I;
    Velocity      Link2_v;
    Acceleration  Link2_a;
    Force         Link2_f;


private:
    static const ExtForces zeroExtForces;
};

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::setJointStatus(const JointState& q) const
{
    (xm->fr_Link1_X_fr_DoubleInvertedPendulumBase)(q);
    (xm->fr_Link2_X_fr_Link1)(q);
}

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::G_terms(JointState& jForces, const JointState& q)
{
    setJointStatus(q);
    G_terms(jForces);
}

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::C_terms(JointState& jForces, const JointState& q, const JointState& qd)
{
    setJointStatus(q);
    C_terms(jForces, qd);
}

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

}

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

}
}

}

#include "inverse_dynamics.impl.h"

#endif