forward_dynamics.h

Go to the documentation of this file.

#ifndef IIT_ROBOT_CT_QUADROTOR_FORWARD_DYNAMICS_H_
#define IIT_ROBOT_CT_QUADROTOR_FORWARD_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 "declarations.h"
#include "transforms.h"
#include "inertia_properties.h"
#include "link_data_map.h"

namespace iit {
namespace ct_quadrotor {
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 LinkDataMap<typename CoreS::ForceVector> ExtForces;
    typedef typename CoreS::ForceVector Force;
    typedef typename CoreS::VelocityVector Velocity;
    typedef typename CoreS::VelocityVector Acceleration;
    typedef typename CoreS::Column6D Column6DS;
    typedef typename iit::ct_quadrotor::tpl::JointState<SCALAR> JointState;
    typedef typename CoreS::Matrix66 Matrix66S;
    
    typedef iit::rbd::tpl::InertiaMatrixDense<SCALAR> InertiaMatrix;
    typedef iit::ct_quadrotor::tpl::MotionTransforms<TRAIT> MTransforms;

public:
    ForwardDynamics(iit::ct_quadrotor::dyn::tpl::InertiaProperties<TRAIT>& in, MotionTransforms& tr); 

    void fd(
       JointState& qdd, Acceleration& body_a, // output parameters,
       const Velocity& body_v, const Acceleration& g,
       const JointState& q, const JointState& qd, const JointState& tau, const ExtForces& fext = zeroExtForces);
    void fd(
        JointState& qdd, Acceleration& body_a, // output parameters,
        const Velocity& body_v, const Acceleration& g,
        const JointState& qd, const JointState& tau, const ExtForces& fext = zeroExtForces);

    void setJointStatus(const JointState& q) const;

private:
    iit::ct_quadrotor::dyn::tpl::InertiaProperties<TRAIT>* inertiaProps;
    MTransforms* motionTransforms;

    Matrix66S vcross; // support variable
    Matrix66S Ia_r;   // support variable, articulated inertia in the case of a revolute joint
    // Link 'body'
    Matrix66S body_AI;
    Force body_p;

    // 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_body)(q);
    (motionTransforms-> fr_link2_X_fr_link1)(q);
}

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

} // namespace tpl

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

}
}
}

#include "forward_dynamics.impl.h"

#endif