1#ifndef ROBOTMODELHYRODYN_HPP
2#define ROBOTMODELHYRODYN_HPP
6#include <hyrodyn/robot_model_hyrodyn.hpp>
7#include <wbc/types/JointCommand.hpp>
36 const base::samples::RigidBodyStateSE3&
floating_base_state = base::samples::RigidBodyStateSE3());
39 virtual void systemState(base::VectorXd &
q, base::VectorXd &
qd, base::VectorXd &
qdd);
47 virtual const base::samples::RigidBodyStateSE3 &
rigidBodyState(
const std::string &root_frame,
const std::string &tip_frame);
54 virtual const base::MatrixXd &
spaceJacobian(
const std::string &root_frame,
const std::string &tip_frame);
61 virtual const base::MatrixXd &
bodyJacobian(
const std::string &root_frame,
const std::string &tip_frame);
77 virtual const base::MatrixXd &
jacobianDot(
const std::string &root_frame,
const std::string &tip_frame);
84 virtual const base::Acceleration &
spatialAccelerationBias(
const std::string &root_frame,
const std::string &tip_frame);
93 virtual const base::samples::RigidBodyStateSE3&
centerOfMass();
virtual const base::samples::RigidBodyStateSE3 & centerOfMass()
Return Current center of gravity in expressed base frame.
Definition RobotModelHyrodyn.cpp:389
virtual const base::samples::RigidBodyStateSE3 & rigidBodyState(const std::string &root_frame, const std::string &tip_frame)
Computes and returns the relative transform between the two given frames. By convention this is the p...
Definition RobotModelHyrodyn.cpp:218
virtual void systemState(base::VectorXd &q, base::VectorXd &qd, base::VectorXd &qdd)
Definition RobotModelHyrodyn.cpp:201
hyrodyn::RobotModel_HyRoDyn * hyrodynHandle()
Return pointer to the internal hyrodyn model.
Definition RobotModelHyrodyn.hpp:96
virtual void computeInverseDynamics(base::commands::Joints &solver_output)
Compute and return the inverse dynamics solution.
Definition RobotModelHyrodyn.cpp:403
virtual bool configure(const RobotModelConfig &cfg)
Load and configure the robot model.
Definition RobotModelHyrodyn.cpp:23
virtual const base::MatrixXd & spaceJacobian(const std::string &root_frame, const std::string &tip_frame)
Returns the Space Jacobian for the kinematic chain between root and the tip frame as full body Jacobi...
Definition RobotModelHyrodyn.cpp:243
virtual ~RobotModelHyrodyn()
Definition RobotModelHyrodyn.cpp:13
virtual const base::VectorXd & biasForces()
Definition RobotModelHyrodyn.cpp:369
virtual const base::MatrixXd & bodyJacobian(const std::string &root_frame, const std::string &tip_frame)
Returns the Body Jacobian for the kinematic chain between root and the tip frame as full body Jacobia...
Definition RobotModelHyrodyn.cpp:284
RobotModelHyrodyn()
Definition RobotModelHyrodyn.cpp:10
hyrodyn::RobotModel_HyRoDyn hyrodyn
Definition RobotModelHyrodyn.hpp:16
virtual const base::Acceleration & spatialAccelerationBias(const std::string &root_frame, const std::string &tip_frame)
Returns the spatial acceleration bias, i.e. the term Jdot*qdot.
Definition RobotModelHyrodyn.cpp:344
virtual const base::MatrixXd & jointSpaceInertiaMatrix()
Definition RobotModelHyrodyn.cpp:350
virtual const base::MatrixXd & jacobianDot(const std::string &root_frame, const std::string &tip_frame)
Returns the derivative of the Jacobian for the kinematic chain between root and the tip frame as full...
Definition RobotModelHyrodyn.cpp:339
void clear()
Definition RobotModelHyrodyn.cpp:16
virtual void update(const base::samples::Joints &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state=base::samples::RigidBodyStateSE3())
Update the robot configuration.
Definition RobotModelHyrodyn.cpp:110
virtual const base::MatrixXd & comJacobian()
Returns the CoM Jacobian for the entire robot, which maps the robot joint velocities to linear spatia...
Definition RobotModelHyrodyn.cpp:327
Eigen::VectorXd qdd
Definition RobotModel.hpp:76
RobotModel()
Definition RobotModel.cpp:7
types::RigidBodyState floating_base_state
Definition RobotModel.hpp:65
types::JointState joint_state
Definition RobotModel.hpp:74
Eigen::VectorXd qd
Definition RobotModel.hpp:76
Eigen::VectorXd q
Definition RobotModel.hpp:76
Definition ContactsAccelerationConstraint.cpp:3
Robot Model configuration class, containts information like urdf file, type of robot model to be load...
Definition RobotModelConfig.hpp:13
Definition RobotModel.hpp:350