jsim.h

Go to the documentation of this file.

#ifndef IIT_TESTHYQ_JSIM_H_
#define IIT_TESTHYQ_JSIM_H_

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

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


namespace iit {
namespace TestHyQ {
namespace dyn {

namespace tpl {

template <typename TRAIT>
class JSIM : public iit::rbd::StateDependentMatrix<iit::TestHyQ::tpl::JointState<typename TRAIT::Scalar>, 18, 18, JSIM<TRAIT>>
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    private:
        typedef iit::rbd::StateDependentMatrix<iit::TestHyQ::tpl::JointState<typename TRAIT::Scalar>, 18, 18, JSIM<TRAIT>> Base;
    public:
        typedef typename TRAIT::Scalar Scalar;
        typedef typename iit::TestHyQ::tpl::JointState<Scalar> JointState;
        typedef iit::rbd::Core<Scalar> CoreS;
        typedef typename Base::Index Index;
        typedef typename iit::rbd::PlainMatrix<Scalar, 18, 18> MatrixType;
        typedef const Eigen::Block<const MatrixType,6,12> BlockF_t;
        typedef const Eigen::Block<const MatrixType,12,12> BlockFixedBase_t;
        typedef InertiaProperties<TRAIT> IProperties;
        typedef iit::TestHyQ::tpl::ForceTransforms<TRAIT> FTransforms;
        typedef iit::rbd::tpl::InertiaMatrixDense<Scalar> InertiaMatrix;

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

        const JSIM& update(const 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 trunk_Ic;
        InertiaMatrix LF_hipassembly_Ic;
        InertiaMatrix LF_upperleg_Ic;
        const InertiaMatrix& LF_lowerleg_Ic;
        InertiaMatrix RF_hipassembly_Ic;
        InertiaMatrix RF_upperleg_Ic;
        const InertiaMatrix& RF_lowerleg_Ic;
        InertiaMatrix LH_hipassembly_Ic;
        InertiaMatrix LH_upperleg_Ic;
        const InertiaMatrix& LH_lowerleg_Ic;
        InertiaMatrix RH_hipassembly_Ic;
        InertiaMatrix RH_upperleg_Ic;
        const InertiaMatrix& RH_lowerleg_Ic;
        InertiaMatrix Ic_spare;

        MatrixType L;
        MatrixType Linv;
        MatrixType inverse;
};

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

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

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

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

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

}

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

}
}
}

#include "jsim.impl.h"

#endif