jsim.h

Go to the documentation of this file.

#ifndef IIT_CT_DOUBLEINVERTEDPENDULUM_JSIM_H_
#define IIT_CT_DOUBLEINVERTEDPENDULUM_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 ct_DoubleInvertedPendulum {
namespace dyn {

namespace tpl {

template <typename TRAIT>
class JSIM : public iit::rbd::StateDependentMatrix<iit::ct_DoubleInvertedPendulum::tpl::JointState<typename TRAIT::Scalar>, 2, 2, JSIM<TRAIT>>
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    private:
        typedef iit::rbd::StateDependentMatrix<iit::ct_DoubleInvertedPendulum::tpl::JointState<typename TRAIT::Scalar>, 2, 2, JSIM<TRAIT>> Base;
    public:
        typedef typename TRAIT::Scalar Scalar;
        typedef typename iit::ct_DoubleInvertedPendulum::tpl::JointState<Scalar> JointState;
        typedef iit::rbd::Core<Scalar> CoreS;
        typedef typename Base::Index Index;
        typedef typename iit::rbd::PlainMatrix<Scalar, 2, 2> MatrixType;
        typedef InertiaProperties<TRAIT> IProperties;
        typedef iit::ct_DoubleInvertedPendulum::tpl::ForceTransforms<TRAIT> FTransforms;
        typedef iit::rbd::tpl::InertiaMatrixDense<Scalar> InertiaMatrix;
        typedef typename CoreS::ForceVector ForceVector;

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

        const JSIM& update(const JointState&);


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

    protected:
        void computeLInverse();
    private:
        IProperties& linkInertias;
        FTransforms* frcTransf;

        // The composite-inertia tensor for each link
        InertiaMatrix Link1_Ic;
        const InertiaMatrix& Link2_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;
}


}

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

}
}
}

#include "jsim.impl.h"

#endif