forward_dynamics.h

Go to the documentation of this file.

#ifndef IIT_ROBOT_CT_DOUBLEINVERTEDPENDULUM_FORWARD_DYNAMICS_H_
#define IIT_ROBOT_CT_DOUBLEINVERTEDPENDULUM_FORWARD_DYNAMICS_H_

#include <iit/rbd/rbd.h>
#include <iit/rbd/utils.h>
#include <iit/rbd/robcogen_commons.h>

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

namespace iit {
namespace ct_DoubleInvertedPendulum {
namespace dyn {

namespace tpl {

template <typename TRAIT>
class ForwardDynamics {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // Convenient type aliases:

    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::Column6D Column6DS;
    typedef typename CoreS::Matrix66 Matrix66S;
    typedef LinkDataMap<Force> ExtForces;
    typedef typename iit::ct_DoubleInvertedPendulum::tpl::JointState<Scalar> JointState;
    typedef iit::ct_DoubleInvertedPendulum::tpl::MotionTransforms<TRAIT> MTransforms;

public:
    ForwardDynamics(InertiaProperties<TRAIT>& in, MTransforms& tr); 

    void fd(
        JointState& qdd, // output parameter
        const JointState& q, const JointState& qd, const JointState& tau, const ExtForces& fext = zeroExtForces);
    void fd(
        JointState& qdd, // output parameter
        const JointState& qd, const JointState& tau, const ExtForces& fext = zeroExtForces);

    void setJointStatus(const JointState& q) const;

private:
    InertiaProperties<TRAIT>* inertiaProps;
    MTransforms* motionTransforms;

    Matrix66S vcross; // support variable
    Matrix66S Ia_r;   // support variable, articulated inertia in the case of a revolute joint

    // Link 'Link1' :
    Matrix66S Link1_AI;
    Velocity Link1_a;
    Velocity Link1_v;
    Velocity Link1_c;
    Force    Link1_p;

    Column6DS Link1_U;
    Scalar Link1_D;
    Scalar Link1_u;
    // Link 'Link2' :
    Matrix66S Link2_AI;
    Velocity Link2_a;
    Velocity Link2_v;
    Velocity Link2_c;
    Force    Link2_p;

    Column6DS Link2_U;
    Scalar Link2_D;
    Scalar Link2_u;
private:
    static const ExtForces zeroExtForces;
};

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

template <typename TRAIT>
inline void ForwardDynamics<TRAIT>::fd(
    JointState& qdd,
    const JointState& q,
    const JointState& qd,
    const JointState& tau,
    const ExtForces& fext/* = zeroExtForces */)
{
    setJointStatus(q);
    fd(qdd, qd, tau, fext);
}

}

typedef tpl::ForwardDynamics<iit::rbd::DoubleTrait> ForwardDynamics;

}
}
}

#include "forward_dynamics.impl.h"

#endif