RobotModelHyrodyn.hpp

Go to the documentation of this file.

#ifndef ROBOTMODELHYRODYN_HPP
#define ROBOTMODELHYRODYN_HPP
 
#include "../../core/RobotModel.hpp"
 
#include <hyrodyn/robot_model_hyrodyn.hpp>
#include <wbc/types/JointCommand.hpp>
 
namespace wbc{
 
class RobotModelHyrodyn : public RobotModel{
private:
    static RobotModelRegistry<RobotModelHyrodyn> reg;
 
protected:
    hyrodyn::RobotModel_HyRoDyn hyrodyn;
    Eigen::VectorXd tmp_ydd;
 
    void clear();
    void addFloatingBaseToURDF(urdf::ModelInterfaceSharedPtr& robot_urdf, const std::string &world_frame_id = "world");
public:
    RobotModelHyrodyn();
    virtual ~RobotModelHyrodyn();

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

    hyrodyn::RobotModel_HyRoDyn *hyrodynHandle(){return &hyrodyn;}

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