robot.h

#pragma once
 
#include <vector>
#include <string>
#include <memory>
#include <Eigen/Dense>
#include <utility>
 
#include "log.h"
#include "matrix_math.h"
#include "link.h"
#include "joint_params.h"
#include "joint.h"
 
namespace bmpf {
    class BaseRobot {
    public:
        virtual ~BaseRobot() = default;
 
        BaseRobot();
 
        virtual void loadFromFile(std::string path) = 0;
 
        virtual std::vector<Eigen::Vector3d> getJointAxes(std::vector<double> state);
 
        virtual std::vector<Eigen::Matrix4d> getTransformMatrices(std::vector<double> state);
 
        virtual Eigen::Matrix4d getEndEffectorTransformMatrix(std::vector<double> state);
 
        Eigen::Matrix4d getEndEffectorDiffTransformMatrix(std::vector<double> state, int iVal);
 
        Eigen::Matrix4d getEndEffectorDiff2TransformMatrix(std::vector<double> state, int iVal, int jVal);
 
        bool isStateEnabled(std::vector<double> state);
 
        std::vector<double> getWorldTranslation() const {
            return {_worldTransformVector.begin(), _worldTransformVector.begin() + 3};
        }
 
        std::vector<double> getWorldRotation() const {
            return {_worldTransformVector.begin() + 3, _worldTransformVector.begin() + 6};
        }
 
        std::vector<double> getWorldScale() const {
            return {_worldTransformVector.begin() + 6, _worldTransformVector.begin() + 9};
        }
 
        std::vector<double> getWorldTransformVector() const { return _worldTransformVector; }
 
        const std::shared_ptr<Eigen::Matrix4d> &getWorldTransformMatrix() const { return _worldTransformMatrix; }
 
        unsigned long getJointCnt() const { return _jointParams.size(); }
 
 
        std::vector<double> getMaxSpeedModules() { return _maxSpeedModules; }
 
        std::vector<double> getMaxAccelerationModules() { return _maxAccelerationModules; }
 
        unsigned long getLinkCnt() const { return _links.size(); }
 
        std::vector<std::shared_ptr<JointParams>> getJointParamsList() const { return _jointParams; }
 
        std::vector<std::shared_ptr<Link>> getLinks() const { return _links; }
 
        std::string getPath() const { return _path; }
 
        void setWorldTranslation(const std::vector<double> &translation);
 
        void setWorldRotation(const std::vector<double> &rotation);
 
        void setWorldScale(const std::vector<double> &scale);
 
        void setWorldTransformVector(const std::vector<double> &vec);
 
        std::vector<std::string> getModelPaths() const;
 
        std::vector<double> getEndEffectorPos(std::vector<double> state);
 
        std::vector<double> getEndEffectorDiffPos(std::vector<double> state, int iVal);
 
        std::vector<double> getEndEffectorDiff2Pos(std::vector<double> state, int iVal, int jVal);
 
        std::vector<double> getEndEffectorRGVector(std::vector<double> state);
 
        std::vector<double> getEndEffectorDiffRGVector(std::vector<double> state, int iVal);
 
        std::vector<double> getEndEffectorDiff2RGVector(std::vector<double> state, int iVal, int jVal);
 
        std::vector<double> getAllLinkPositions(std::vector<double> state);
 
        std::vector<double> getAllLinkRGVectors(std::vector<double> state);
 
        std::vector<double> getMassCenterAbsolutePositions(std::vector<double> state);
 
        virtual std::vector<double> getMassCenterLocalPositions();
 
        virtual std::vector<Eigen::Matrix3d> getInertias();
 
        std::vector<double> getRandomState();
 
    protected:
        void _fillWorldTransformMatrix();
 
        template<typename F>
        void _forEachJoint(std::vector<double> state, const F &consumer);
 
        template<typename F>
        void _forEachDiffJoint(std::vector<double> state, int iVal, const F &consumer);
 
        template<typename F>
        void _forEachDiff2Joint(std::vector<double> state, int iVal, int jVal, const F &consumer);
 
        std::vector<std::shared_ptr<Joint>> _joints;
        std::vector<std::shared_ptr<JointParams>> _jointParams;
        std::vector<double> _maxSpeedModules;
        std::vector<double> _maxAccelerationModules;
        std::vector<std::shared_ptr<Link>> _links;
        std::vector<std::shared_ptr<Link>> _nonHierarchicalLinks;
        std::string _path;
        std::vector<double> _worldTransformVector;
        std::shared_ptr<Eigen::Matrix4d> _worldTransformMatrix;
 
    };
}