urdf_robot.h

#pragma once
 
#include <base/robot.h>
#include <Eigen/Dense>
#include <utility>
#include <vector>
#include <string>
 
#include "urdf/model.h"
#include "base/joint.h"
 
 
namespace bmpf {
    class URDFRobot : public BaseRobot {
    public:
 
        URDFRobot() = default;
 
        void loadFromFile(std::string path) override;
 
    private:
 
        constexpr static const double EFFORT_TO_ACCELERATION = 0.01;
 
        void _applyURDF(std::vector<std::shared_ptr<urdf::Joint>> jointList,
                        std::map<std::string, urdf::LinkSharedPtr> linkMap);
 
        static Eigen::Matrix4d _getMatrixFromPose(urdf::Pose pose);
 
        static std::shared_ptr<Link>
        _createLink(const std::shared_ptr<urdf::Link> &urdfLink, const Eigen::Matrix4d &linkTransform);
 
        static Eigen::Matrix4d _createLinkTransform(const std::shared_ptr<urdf::Link> &urdfLink);
    };
}