jsim.h

Go to the documentation of this file.

#ifndef IIT_CT_HYA_JSIM_H_
#define IIT_CT_HYA_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_HyA {
namespace dyn {

namespace tpl{

template <class TRAIT>
class JSIM : public iit::rbd::StateDependentMatrix<iit::ct_HyA::tpl::JointState<typename TRAIT::Scalar>, 6, 6, JSIM<TRAIT>>
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    private:
        typedef iit::rbd::StateDependentMatrix<iit::ct_HyA::JointState, 6, 6, JSIM<TRAIT>> Base;
    public:
        typedef typename TRAIT::Scalar SCALAR;
        typedef typename Base::Index Index;
        typedef Eigen::Matrix<SCALAR,6,6> MatrixType;
        typedef InertiaProperties<TRAIT> IProperties;
        typedef iit::ct_HyA::tpl::ForceTransforms<TRAIT> FTransforms;
        typedef iit::rbd::tpl::InertiaMatrixDense<SCALAR> InertiaMatrix;
        typedef iit::ct_HyA::tpl::JointState<SCALAR> JointState;
        typedef typename iit::rbd::Core<SCALAR>::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 Shoulder_AA_Ic;
        InertiaMatrix Shoulder_FE_Ic;
        InertiaMatrix Humerus_R_Ic;
        InertiaMatrix Elbow_FE_Ic;
        InertiaMatrix Wrist_R_Ic;
        const InertiaMatrix& Wrist_FE_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;
}


} // namespace tpl

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

}
}
}

#include "jsim.impl.h"

#endif