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;
 
    Eigen::VectorXd q, qd, qdd;
    pinocchio::Model model;
    typedef std::shared_ptr<pinocchio::Data> DataPtr;
    DataPtr data;
 
    void clear();
public:
    RobotModelPinocchio();
    ~RobotModelPinocchio();
 
    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::Acceleration &spatialAccelerationBias(const std::string &root_frame, const std::string &tip_frame);
 
    virtual const base::MatrixXd &jacobianDot(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