RobotModelRBDL.hpp

Go to the documentation of this file.

#ifndef ROBOT_MODEL_RBDL_HPP
#define ROBOT_MODEL_RBDL_HPP
 
#include "../../core/RobotModel.hpp"
#include <rbdl/rbdl.h>
 
namespace wbc {
 
class RobotModelRBDL : public RobotModel{
protected:
    static RobotModelRegistry<RobotModelRBDL> reg;
 
    std::shared_ptr<RigidBodyDynamics::Model> rbdl_model;
    Eigen::VectorXd q, qd, qdd, tau;
    RigidBodyDynamics::Math::MatrixNd J, H_q;
 
    std::vector<std::string> jointNamesInRBDLOrder(const std::string &urdf_file);
    void updateFloatingBase(const base::samples::RigidBodyStateSE3& floating_base_state_in);
    void clear();
 
public:
    RobotModelRBDL();
    virtual ~RobotModelRBDL();
 
    virtual bool configure(const RobotModelConfig& cfg);
 
    virtual void update(const base::samples::Joints& joint_state,
                        const base::samples::RigidBodyStateSE3& floating_base_state = base::samples::RigidBodyStateSE3());
 
    virtual void systemState(base::VectorXd &q, base::VectorXd &qd, base::VectorXd &qdd);
 
    virtual const base::samples::RigidBodyStateSE3 &rigidBodyState(const std::string &root_frame, const std::string &tip_frame);
 
    virtual const base::MatrixXd &spaceJacobian(const std::string &root_frame, const std::string &tip_frame);
 
    virtual const base::MatrixXd &bodyJacobian(const std::string &root_frame, const std::string &tip_frame);
 
    virtual const base::MatrixXd &comJacobian();
 
    virtual const base::MatrixXd &jacobianDot(const std::string &root_frame, const std::string &tip_frame);
 
    virtual const base::Acceleration &spatialAccelerationBias(const std::string &root_frame, const std::string &tip_frame);
 
    virtual const base::MatrixXd &jointSpaceInertiaMatrix();
 
    virtual const base::VectorXd &biasForces();
 
    virtual const base::samples::RigidBodyStateSE3& centerOfMass();
 
    virtual void computeInverseDynamics(base::commands::Joints &solver_output);
};
 
}
 
#endif