jsim.h

Go to the documentation of this file.

#ifndef IIT_CT_QUADROTOR_JSIM_H_
#define IIT_CT_QUADROTOR_JSIM_H_

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

#include "declarations.h"
#include "transforms.h"
#include "inertia_properties.h"
#include <iit/rbd/traits/DoubleTrait.h>


namespace iit {
namespace ct_quadrotor {
namespace dyn {

namespace tpl{

template <class TRAIT>
class JSIM : public iit::rbd::StateDependentMatrix<iit::ct_quadrotor::JointState, 8, 8, JSIM<TRAIT>>
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    private:
        typedef iit::rbd::StateDependentMatrix<iit::ct_quadrotor::JointState, 8, 8, JSIM<TRAIT>> Base;
    public:
        typedef typename TRAIT::Scalar SCALAR;
        typedef typename Base::Index Index;
        typedef Eigen::Matrix<SCALAR,8,8> MatrixType;
        typedef const Eigen::Block<const MatrixType,6,2> BlockF_t;
        typedef const Eigen::Block<const MatrixType,2,2> BlockFixedBase_t;
        typedef InertiaProperties<TRAIT> IProperties;
        typedef iit::ct_quadrotor::tpl::ForceTransforms<TRAIT> FTransforms;
        typedef iit::rbd::tpl::InertiaMatrixDense<SCALAR> InertiaMatrix;

    public:
        JSIM(IProperties&, FTransforms&);
        ~JSIM() {}

        const JSIM& update(const iit::ct_quadrotor::JointState&);


        void computeL();
        void computeInverse();
        const MatrixType& getL() const;
        const MatrixType& getInverse() const;

        const InertiaMatrix& getWholeBodyInertia() const;
        const BlockF_t getF() const;
        const BlockFixedBase_t getFixedBaseBlock() const;
    protected:
        void computeLInverse();
    private:
        IProperties& linkInertias;
        FTransforms* frcTransf;

        // The composite-inertia tensor for each link
        InertiaMatrix body_Ic;
        InertiaMatrix link1_Ic;
        const InertiaMatrix& link2_Ic;
        InertiaMatrix Ic_spare;

        MatrixType L;
        MatrixType Linv;
        MatrixType inverse;
};

template <class TRAIT>
inline const typename JSIM<TRAIT>::MatrixType& JSIM<TRAIT>::getL() const {
    return L;
}

template <class TRAIT>
inline const typename JSIM<TRAIT>::MatrixType& JSIM<TRAIT>::getInverse() const {
    return inverse;
}

template <class TRAIT>
inline const typename JSIM<TRAIT>::InertiaMatrix& JSIM<TRAIT>::getWholeBodyInertia() const {
    return body_Ic;
}

template <class TRAIT>
inline const typename JSIM<TRAIT>::BlockF_t JSIM<TRAIT>::getF() const {
    return JSIM<TRAIT>:: template block<6,2>(0,6);
}

template <class TRAIT>
inline const typename JSIM<TRAIT>::BlockFixedBase_t JSIM<TRAIT>::getFixedBaseBlock() const{
    return JSIM<TRAIT>:: template block<2,2>(6,6);
}

} // namespace tpl

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

}
}
}

#include "jsim.impl.h"

#endif