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 tau;
    RigidBodyDynamics::Math::MatrixNd J, H_q;
 
    std::vector<std::string> jointNamesInRBDLOrder(const std::string &urdf_file);
    void clear();
    void updateFK(Eigen::VectorXd &_q,Eigen::VectorXd &_qd,Eigen::VectorXd &_qdd);
    void updateFK(Eigen::VectorXd &_q,Eigen::VectorXd &_qd);
 
public:
    RobotModelRBDL();
    virtual ~RobotModelRBDL();

    virtual bool configure(const RobotModelConfig& cfg);

    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);

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

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

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

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

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

    virtual const Eigen::MatrixXd &comJacobian();

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

    virtual const Eigen::MatrixXd &jointSpaceInertiaMatrix();

    virtual const Eigen::VectorXd &biasForces();

    virtual const types::RigidBodyState& centerOfMass();

    virtual const Eigen::VectorXd& inverseDynamics(const Eigen::VectorXd& qdd_ref = Eigen::VectorXd(),
                                                   const std::vector<types::Wrench>& f_ext = std::vector<types::Wrench>());
};
 
}
 
#endif