RobotModel.hpp

Go to the documentation of this file.

#ifndef WBC_CORE_ROBOTMODEL_HPP
#define WBC_CORE_ROBOTMODEL_HPP
 
#include <memory>
#include <map>
#include "../types/JointLimits.hpp"
#include "../types/RigidBodyState.hpp"
#include "../types/JointState.hpp"
#include "../types/Wrench.hpp"
#include "RobotModelConfig.hpp"
#include <urdf_world/world.h>
#include "../tools/filter.hpp"
 
namespace wbc{

class RobotModel{
protected:
    void clear();
    void resetData();
    void updateData();
 
    struct Pose{
        Pose(){needs_recompute = true;}
        bool needs_recompute;
        types::Pose data;
    };
    struct Twist{
        Twist(){needs_recompute = true;}
        bool needs_recompute;
        types::Twist data;
    };
    struct SpatialAcceleration{
        SpatialAcceleration(){needs_recompute = true;}
        bool needs_recompute;
        types::SpatialAcceleration data;
    };
    struct Matrix{
        Matrix(){needs_recompute = true;}
        bool needs_recompute;
        Eigen::MatrixXd data;
    };
    struct Vector{
        Vector(){needs_recompute = true;}
        bool needs_recompute;
        Eigen::VectorXd data;
    };
    struct RigidBodyState{
        RigidBodyState(){needs_recompute = true;}
        bool needs_recompute;
        types::RigidBodyState data;
    };
 
    std::vector<types::Contact> contacts;
    Eigen::Vector3d gravity;
    types::RigidBodyState floating_base_state;
    RobotModelConfig robot_model_config;
    std::string world_frame, base_frame;
    types::JointLimits joint_limits;
    std::vector<std::string> actuated_joint_names;
    std::vector<std::string> independent_joint_names;
    std::vector<std::string> joint_names;
    urdf::ModelInterfaceSharedPtr robot_urdf;
    bool has_floating_base;
    types::JointState joint_state;
    Eigen::MatrixXd selection_matrix;
    Eigen::VectorXd q, qd, qdd, tau, zero_jnt;
 
    std::vector<Matrix> com_jac;
    std::vector<Matrix> joint_space_inertia_mat;
    std::vector<Vector> bias_forces;
    std::vector<RigidBodyState> com_rbs;
    std::map<std::string,Matrix> space_jac_map;
    std::map<std::string,Matrix> body_jac_map;
    std::map<std::string,Pose> pose_map;
    std::map<std::string,Twist> twist_map;
    std::map<std::string,SpatialAcceleration> acc_map;
    std::map<std::string,SpatialAcceleration> spatial_acc_bias_map;
    bool fk_needs_recompute, fk_with_zero_acc_needs_recompute;
    Eigen::VectorXd joint_weights;
 
    types::SpatialAcceleration zero_acc;
    bool configured,updated;

    std::unique_ptr<LowPassFilter> joint_vel_filter;
    std::unique_ptr<LowPassFilter> fb_vel_filter;
 
public:
    RobotModel();
    virtual ~RobotModel(){}

    virtual bool configure(const RobotModelConfig& cfg) = 0;

    void update(const Eigen::VectorXd& joint_positions,
                const Eigen::VectorXd& joint_velocities);

    virtual void update(const Eigen::VectorXd& joint_positions,
                const Eigen::VectorXd& joint_velocities,
                const Eigen::VectorXd& joint_accelerations);

    void update(const Eigen::VectorXd& joint_positions,
                const Eigen::VectorXd& joint_velocities,
                const types::Pose& fb_pose,
                const types::Twist& fb_twist);

    virtual void update(const Eigen::VectorXd& joint_positions,
                        const Eigen::VectorXd& joint_velocities,
                        const Eigen::VectorXd& joint_accelerations,
                        const types::Pose& fb_pose,
                        const types::Twist& fb_twist,
                        const types::SpatialAcceleration& fb_acc) = 0;

    const types::JointState& jointState(){return joint_state;}

    virtual const types::Pose &pose(const std::string &frame_id) = 0;

    virtual const types::Twist &twist(const std::string &frame_id) = 0;

    virtual const types::SpatialAcceleration &acceleration(const std::string &frame_id) = 0;
 

    virtual const Eigen::MatrixXd &spaceJacobian(const std::string &frame_id) = 0;

    virtual const Eigen::MatrixXd &bodyJacobian(const std::string &frame_id) = 0;

    virtual const Eigen::MatrixXd &comJacobian() = 0;

    virtual const types::SpatialAcceleration &spatialAccelerationBias(const std::string &frame_id) = 0;

    virtual const Eigen::MatrixXd &jointSpaceInertiaMatrix() = 0;

    virtual const Eigen::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 types::JointLimits jointLimits(){return joint_limits;}

    const Eigen::MatrixXd &selectionMatrix(){return selection_matrix;}

    bool hasLink(const std::string& link_name);

    bool hasJoint(const std::string& joint_name);

    bool hasActuatedJoint(const std::string& joint_name);

    virtual const types::RigidBodyState& centerOfMass() = 0;

    void setContacts(const std::vector<types::Contact> &contacts);

    const std::vector<types::Contact>& getContacts(){return contacts;}

    uint nj(){return jointNames().size() + (has_floating_base ? 6 : 0);}

    uint na(){return actuatedJointNames().size();}

    uint nfb(){return has_floating_base ? 6 : 0;}

    uint nac();

    uint nc(){return contacts.size();}

    const Eigen::VectorXd &getQ(){return q;}

    const Eigen::VectorXd &getQd(){return qd;}

    const Eigen::VectorXd &getQdd(){return qdd;}

    void setGravityVector(const Eigen::Vector3d& g){gravity=g;}

    const types::RigidBodyState& floatingBaseState(){return floating_base_state;}

    const RobotModelConfig& getRobotModelConfig(){return robot_model_config;}

    bool hasFloatingBase(){return has_floating_base;}

    urdf::ModelInterfaceSharedPtr loadRobotURDF(const std::string& file_or_string);

    urdf::ModelInterfaceSharedPtr getURDFModel(){return robot_urdf;}

    void setJointWeights(const Eigen::VectorXd& weights);

    const Eigen::VectorXd& getJointWeights() const { return joint_weights; }

    virtual const Eigen::VectorXd& inverseDynamics(const Eigen::VectorXd& qdd_ref = Eigen::VectorXd(),
                                                   const std::vector<types::Wrench>& f_ext = std::vector<types::Wrench>()) = 0;
 
};
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 // WBC_CORE_ROBOTMODEL_HPP