RobotModel.hpp

Go to the documentation of this file.

#ifndef ROBOTMODEL_HPP
#define ROBOTMODEL_HPP
 
#include <memory>
#include <base/Eigen.hpp>
#include <base/samples/RigidBodyStateSE3.hpp>
#include <base/samples/Joints.hpp>
#include <base/Acceleration.hpp>
#include <base/JointLimits.hpp>
#include <base/samples/Wrenches.hpp>
#include <base/commands/Joints.hpp>
#include "RobotModelConfig.hpp"
#include <urdf_world/world.h>
 
namespace wbc{
 
std::vector<std::string> operator+(std::vector<std::string> a, std::vector<std::string> b);
 
class RobotModel{
protected:
    void clear();
 
    const std::string chainID(const std::string& root, const std::string& tip){return root + "_" + tip;}
 
    ActiveContacts active_contacts;
    base::Vector3d gravity;
    base::samples::RigidBodyStateSE3 floating_base_state;
    base::samples::Wrenches contact_wrenches;
    RobotModelConfig robot_model_config;
    std::string world_frame, base_frame;
    base::JointLimits joint_limits;
    std::vector<std::string> actuated_joint_names;
    std::vector<std::string> independent_joint_names;
    std::vector<std::string> joint_names;
    std::vector<std::string> joint_names_floating_base;
    urdf::ModelInterfaceSharedPtr robot_urdf;
    bool has_floating_base;
    base::samples::Joints joint_state;
    base::MatrixXd joint_space_inertia_mat;
    base::MatrixXd com_jac;
    base::VectorXd bias_forces;
    base::Acceleration spatial_acc_bias;
    base::MatrixXd selection_matrix;
    base::samples::RigidBodyStateSE3 com_rbs;
    base::samples::RigidBodyStateSE3 rbs;
 
    typedef std::map<std::string, base::MatrixXd > JacobianMap;
    JacobianMap space_jac_map;
    JacobianMap body_jac_map;
    JacobianMap jac_dot_map;
 
    // Helper
    base::samples::Joints joint_state_out;
 
public:
    RobotModel();
    virtual ~RobotModel(){}
 
    virtual bool configure(const RobotModelConfig& cfg) = 0;
 
    virtual void update(const base::samples::Joints& joint_state,
                        const base::samples::RigidBodyStateSE3& floating_base_state = base::samples::RigidBodyStateSE3()) = 0;
 
    const base::samples::Joints& jointState(const std::vector<std::string> &joint_names);
 
    virtual void systemState(base::VectorXd &q, base::VectorXd &qd, base::VectorXd &qdd) = 0;
 
    virtual const base::samples::RigidBodyStateSE3 &rigidBodyState(const std::string &root_frame, const std::string &tip_frame) = 0;
 
    virtual const base::MatrixXd &spaceJacobian(const std::string &root_frame, const std::string &tip_frame) = 0;
 
    virtual const base::MatrixXd &bodyJacobian(const std::string &root_frame, const std::string &tip_frame) = 0;
 
    virtual const base::MatrixXd &comJacobian() = 0;
 
    virtual const base::Acceleration &spatialAccelerationBias(const std::string &root_frame, const std::string &tip_frame) = 0;
 
    virtual const base::MatrixXd &jacobianDot(const std::string &root_frame, const std::string &tip_frame) = 0;
 
    virtual const base::MatrixXd &jointSpaceInertiaMatrix() = 0;
 
    virtual const base::VectorXd &biasForces() = 0;
 
    const std::vector<std::string>& jointNames(){return joint_names;}
 
    const std::vector<std::string>& actuatedJointNames(){return actuated_joint_names;}
 
    const std::vector<std::string>& independentJointNames(){return independent_joint_names;}
 
    uint jointIndex(const std::string &joint_name);
 
    const std::string& baseFrame(){return base_frame;}
 
    const std::string& worldFrame(){return world_frame;}
 
    const base::JointLimits& jointLimits(){return joint_limits;}
 
    bool hasLink(const std::string& link_name);
 
    bool hasJoint(const std::string& joint_name);
 
    bool hasActuatedJoint(const std::string& joint_name);
 
    const base::MatrixXd& selectionMatrix(){return selection_matrix;}
 
    virtual const base::samples::RigidBodyStateSE3& centerOfMass() = 0;
 
    void setActiveContacts(const ActiveContacts &contacts);
 
    const ActiveContacts& getActiveContacts(){return active_contacts;}
 
    uint noOfJoints(){return jointNames().size();}
 
    uint noOfActuatedJoints(){return actuatedJointNames().size();}
 
    void setGravityVector(const base::Vector3d& g){gravity=g;}
 
    const base::samples::RigidBodyStateSE3& floatingBaseState(){return floating_base_state;}
 
    void setContactWrenches(const base::samples::Wrenches& wrenches){contact_wrenches = wrenches;}
 
    virtual void computeInverseDynamics(base::commands::Joints &solver_output) = 0;
 
    const RobotModelConfig& getRobotModelConfig(){return robot_model_config;}
 
    bool hasFloatingBase(){return has_floating_base;}
 
    urdf::ModelInterfaceSharedPtr loadRobotURDF(const std::string& file_or_string);
 
};
typedef std::shared_ptr<RobotModel> RobotModelPtr;
 
template<typename T> RobotModel* createT(){return new T;}
 
struct RobotModelFactory{
    typedef std::map<std::string, RobotModel*(*)()> RobotModelMap;
 
    static RobotModel *createInstance(const std::string& name) {
        RobotModelMap::iterator it = getRobotModelMap()->find(name);
        if(it == getRobotModelMap()->end())
            throw std::runtime_error("Failed to create instance of plugin " + name + ". Is the plugin registered?");
        return it->second();
    }
 
    template<typename T>
    static T* createInstance(const std::string& name){
        RobotModel* tmp = createInstance(name);
        T* ret = dynamic_cast<T*>(tmp);
        return ret;
    }
 
    static RobotModelMap *getRobotModelMap(){
        if(!robot_model_map)
            robot_model_map = new RobotModelMap;
        return robot_model_map;
    }
 
    static void clear(){
        robot_model_map->clear();
    }
private:
    static RobotModelMap *robot_model_map;
};
 
template<typename T>
struct RobotModelRegistry : RobotModelFactory{
    RobotModelRegistry(const std::string& name) {
        RobotModelMap::iterator it = getRobotModelMap()->find(name);
        if(it != getRobotModelMap()->end())
            throw std::runtime_error("Failed to register plugin with name " + name + ". A plugin with the same name is already registered");
        getRobotModelMap()->insert(std::make_pair(name, &createT<T>));
    }
};
 
}
 
#endif // ROBOTMODEL_HPP