inverse_dynamics.h

Go to the documentation of this file.

#ifndef IIT_CT_QUADROTOR_INVERSE_DYNAMICS_H_
#define IIT_CT_QUADROTOR_INVERSE_DYNAMICS_H_

#include <Eigen/Dense>
#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_quadrotor {
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::ct_quadrotor::tpl::JointState<SCALAR> JointState;
    typedef typename CoreS::Matrix66 Matrix66s;
    typedef iit::ct_quadrotor::tpl::MotionTransforms<TRAIT> MTransforms;
    typedef InertiaProperties<TRAIT> IProperties;            

public:
    InverseDynamics(IProperties& in, MTransforms& tr);
 
    void id(
        JointState& jForces, Acceleration& body_a,
        const Acceleration& g, const Velocity& body_v,
        const JointState& q, const JointState& qd, const JointState& qdd,
        const ExtForces& fext = zeroExtForces);
    void id(
        JointState& jForces, Acceleration& body_a,
        const Acceleration& g, const Velocity& body_v,
        const JointState& qd, const JointState& qdd,
        const ExtForces& fext = zeroExtForces);
 
    void id_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Acceleration& g, const Velocity& body_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& body_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& body_v, const JointState& q, const JointState& qd);
    void C_terms_fully_actuated(
        Force& baseWrench, JointState& jForces,
        const Velocity& body_v, const JointState& qd);

    void setJointStatus(const JointState& q) const;

public:
    const Force& getForce_body() const { return body_f; }
    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 secondPass_fullyActuated(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;

    // The robot base
    const InertiaMatrix& body_I;
    InertiaMatrix body_Ic;
    Force         body_f;
    // The composite inertia tensors
    InertiaMatrix link1_Ic;
    const InertiaMatrix& link2_Ic;

private:
    static const ExtForces zeroExtForces;
};

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

template <typename TRAIT>
inline void InverseDynamics<TRAIT>::id(
    JointState& jForces, Acceleration& body_a,
    const Acceleration& g, const Velocity& body_v,
    const JointState& q, const JointState& qd, const JointState& qdd,
    const ExtForces& fext)
{
    setJointStatus(q);
    id(jForces, body_a, g, body_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& body_v, const JointState& q, const JointState& qd)
{
    setJointStatus(q);
    C_terms_fully_actuated(baseWrench, jForces, body_v, qd);
}

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

} // namespace tpl

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

}
}

}

#include "inverse_dynamics.impl.h"

#endif