RobotModelPinocchio.hpp

Go to the documentation of this file.

#ifndef ROBOT_MODEL_PINOCCHIO_HPP
#define ROBOT_MODEL_PINOCCHIO_HPP
 
#include "../../core/RobotModel.hpp"
#include <pinocchio/multibody/fwd.hpp>
#include <pinocchio/parsers/urdf.hpp>
 
namespace wbc {
 
class RobotModelPinocchio : public RobotModel{
protected:
    static RobotModelRegistry<RobotModelPinocchio> reg;
 
    pinocchio::Model model;
    typedef std::shared_ptr<pinocchio::Data> DataPtr;
    DataPtr data;

    void clear();
    void updateFK(Eigen::VectorXd &_q,Eigen::VectorXd &_qd,Eigen::VectorXd &_qdd);
    void updateFK(Eigen::VectorXd &_q,Eigen::VectorXd &_qd);
 
public:
    RobotModelPinocchio();
    ~RobotModelPinocchio();

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

    pinocchio::Model *getInternalModel();
};
 
}
 
#endif