KinematicModel.hpp

Go to the documentation of this file.

#ifndef ODOMETRY_KINEMATIC_MODEL_HPP
#define ODOMETRY_KINEMATIC_MODEL_HPP

#include <string> // std string library
#include <vector> // std vector library
#include <base/Eigen.hpp> //base eigen of rock

namespace odometry
{
    template <typename _Scalar, int _RobotTrees, int _RobotJointDoF, int _SlipDoF, int _ContactDoF>
    class KinematicModel
    {
        public:
            static const unsigned int MAX_CHAIN_DOF = _RobotJointDoF+_SlipDoF+_ContactDoF;

            static const unsigned int MODEL_DOF = _RobotJointDoF+_RobotTrees*(_SlipDoF+_ContactDoF);

        public:
            inline int getNumberOfTrees()
            {
                return _RobotTrees;
            }
            inline int getRobotJointDoF(void)
            {
                return _RobotJointDoF;
            }
            inline int getSlipDoF(void)
            {
                return _SlipDoF;
            }
            inline int getContactDoF()
            {
                return _ContactDoF;
            }
            inline unsigned int getMaxChainDoF()
            {
                return MAX_CHAIN_DOF;
            }
            inline unsigned int getModelDOF()
            {
                return MODEL_DOF;
            }

            virtual std::string name() = 0;

            virtual void contactPointsPerTree (std::vector<unsigned int> &contactPoints) = 0;

            virtual void setPointsInContact (const std::vector<int> &pointsInContact) = 0;

            virtual void fkBody2ContactPointt(const int chainIdx, const std::vector<_Scalar> &positions, Eigen::Affine3d &fkTrans, base::Matrix6d &fkCov) = 0;

            virtual void fkSolver(const std::vector<_Scalar> &positions, std::vector<Eigen::Affine3d> &fkRobot, std::vector<base::Matrix6d> &fkCov) = 0;

            virtual Eigen::Matrix<_Scalar, 6*_RobotTrees, MODEL_DOF> jacobianSolver(const std::vector<_Scalar> &positions) = 0;
    };
}

#endif // ODOMETRY_KINEMATIC_MODEL_HPP