1#ifndef WBC_CORE_ROBOTMODEL_HPP
2#define WBC_CORE_ROBOTMODEL_HPP
11#include <urdf_world/world.h>
86 std::map<std::string,SpatialAcceleration>
acc_map;
109 void update(
const Eigen::VectorXd& joint_positions,
110 const Eigen::VectorXd& joint_velocities);
117 virtual void update(
const Eigen::VectorXd& joint_positions,
118 const Eigen::VectorXd& joint_velocities,
119 const Eigen::VectorXd& joint_accelerations);
128 void update(
const Eigen::VectorXd& joint_positions,
129 const Eigen::VectorXd& joint_velocities,
143 virtual void update(
const Eigen::VectorXd& joint_positions,
144 const Eigen::VectorXd& joint_velocities,
145 const Eigen::VectorXd& joint_accelerations,
172 virtual const Eigen::MatrixXd &
spaceJacobian(
const std::string &frame_id) = 0;
178 virtual const Eigen::MatrixXd &
bodyJacobian(
const std::string &frame_id) = 0;
212 uint
jointIndex(
const std::string &joint_name);
227 bool hasLink(
const std::string& link_name);
230 bool hasJoint(
const std::string& joint_name);
264 const Eigen::VectorXd &
getQ(){
return q;}
293 urdf::ModelInterfaceSharedPtr
loadRobotURDF(
const std::string& file_or_string);
311 virtual const Eigen::VectorXd&
inverseDynamics(
const Eigen::VectorXd& qdd_ref = Eigen::VectorXd(),
312 const std::vector<types::Wrench>& f_ext = std::vector<types::Wrench>()) = 0;
325 throw std::runtime_error(
"Failed to create instance of plugin " + name +
". Is the plugin registered?");
332 T* ret =
dynamic_cast<T*
>(tmp);
339 return robot_model_map;
343 robot_model_map->clear();
354 throw std::runtime_error(
"Failed to register plugin with name " + name +
". A plugin with the same name is already registered");
Interface for all robot models. This has to provide all kinematics and dynamics information that is r...
Definition RobotModel.hpp:26
std::vector< Vector > bias_forces
Definition RobotModel.hpp:80
const RobotModelConfig & getRobotModelConfig()
Get current robot model config.
Definition RobotModel.hpp:287
bool has_floating_base
Definition RobotModel.hpp:73
bool hasActuatedJoint(const std::string &joint_name)
Return True if given joint name is an actuated joint in robot model, false otherwise.
Definition RobotModel.cpp:134
std::string world_frame
Definition RobotModel.hpp:67
types::JointLimits joint_limits
Definition RobotModel.hpp:68
virtual ~RobotModel()
Definition RobotModel.hpp:96
const types::RigidBodyState & floatingBaseState()
Get current status of floating base.
Definition RobotModel.hpp:284
virtual const Eigen::MatrixXd & bodyJacobian(const std::string &frame_id)=0
Returns the Body Jacobian for the given frame. The order of the Jacobian's columns will be the same a...
virtual const types::RigidBodyState & centerOfMass()=0
Return centers of mass expressed in world frame.
RobotModelConfig robot_model_config
Definition RobotModel.hpp:66
std::vector< std::string > actuated_joint_names
Definition RobotModel.hpp:69
std::vector< std::string > independent_joint_names
Definition RobotModel.hpp:70
void resetData()
Definition RobotModel.cpp:58
const Eigen::VectorXd & getQd()
Return full system velocity vector, incl. floating base. Convention [floating_base_vel,...
Definition RobotModel.hpp:271
std::vector< types::Contact > contacts
Definition RobotModel.hpp:63
bool hasFloatingBase()
Is is a floating base robot?
Definition RobotModel.hpp:290
urdf::ModelInterfaceSharedPtr loadRobotURDF(const std::string &file_or_string)
Load URDF model from either file or string.
Definition RobotModel.cpp:139
virtual const types::Pose & pose(const std::string &frame_id)=0
virtual const Eigen::VectorXd & biasForces()=0
Returns the bias force vector, which is nj x 1, where nj is the number of joints + number of floating...
void setGravityVector(const Eigen::Vector3d &g)
Set the current gravity vector. Default is (0,0,-9.81)
Definition RobotModel.hpp:281
void setJointWeights(const Eigen::VectorXd &weights)
set Joint weights by given name
Definition RobotModel.cpp:158
const std::vector< std::string > & independentJointNames()
Return only independent joint names.
Definition RobotModel.hpp:209
std::map< std::string, SpatialAcceleration > spatial_acc_bias_map
Definition RobotModel.hpp:87
std::vector< RigidBodyState > com_rbs
Definition RobotModel.hpp:81
bool fk_needs_recompute
Definition RobotModel.hpp:88
const std::vector< types::Contact > & getContacts()
get current contact points
Definition RobotModel.hpp:242
Eigen::VectorXd joint_weights
Definition RobotModel.hpp:89
virtual const Eigen::MatrixXd & jointSpaceInertiaMatrix()=0
Returns the joint space mass-inertia matrix, which is nj x nj, where nj is the number of joints + num...
Eigen::VectorXd tau
Definition RobotModel.hpp:76
void update(const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities)
Update kinematics/dynamics for fixed base robots. Joint acceleration will be assumed zero.
Definition RobotModel.cpp:11
types::SpatialAcceleration zero_acc
Definition RobotModel.hpp:91
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)=0
Update kinematics/dynamics.
const Eigen::VectorXd & getQdd()
Return full system acceleration vector, incl. floating base. Convention [floating_base_acc,...
Definition RobotModel.hpp:278
void updateData()
Definition RobotModel.cpp:82
virtual const types::SpatialAcceleration & spatialAccelerationBias(const std::string &frame_id)=0
Returns the spatial acceleration bias, i.e. the term Jdot*qdot. The linear part of the acceleration w...
virtual const Eigen::VectorXd & inverseDynamics(const Eigen::VectorXd &qdd_ref=Eigen::VectorXd(), const std::vector< types::Wrench > &f_ext=std::vector< types::Wrench >())=0
Compute tau from internal state.
uint nac()
Definition RobotModel.cpp:148
const Eigen::VectorXd & getJointWeights() const
Get Joint weights as Named vector.
Definition RobotModel.hpp:306
const std::vector< std::string > & jointNames()
Return all joint names excluding the floating base. This will be.
Definition RobotModel.hpp:203
const std::string & worldFrame()
Get the world frame id. Will be "world" in case of floating base robot and equal to base_frame in cas...
Definition RobotModel.hpp:218
const Eigen::MatrixXd & selectionMatrix()
Return current selection matrix, mapping actuated to all dof.
Definition RobotModel.hpp:224
std::map< std::string, Twist > twist_map
Definition RobotModel.hpp:85
Eigen::VectorXd qdd
Definition RobotModel.hpp:76
const std::string & baseFrame()
Get the base frame of the robot, i.e. the root of the URDF model.
Definition RobotModel.hpp:215
RobotModel()
Definition RobotModel.cpp:7
void setContacts(const std::vector< types::Contact > &contacts)
Set new contact points.
Definition RobotModel.cpp:105
virtual const Eigen::MatrixXd & spaceJacobian(const std::string &frame_id)=0
Returns the Space Jacobian for the given frame. The order of the Jacobian's columns will be the same ...
Eigen::Vector3d gravity
Definition RobotModel.hpp:64
virtual const types::SpatialAcceleration & acceleration(const std::string &frame_id)=0
std::map< std::string, Matrix > space_jac_map
Definition RobotModel.hpp:82
uint nfb()
Return dof of the floating base, will be either 0 (floating_base == false) or 6 (floating_base = = fa...
Definition RobotModel.hpp:251
types::RigidBodyState floating_base_state
Definition RobotModel.hpp:65
const types::JointLimits jointLimits()
Return current joint limits.
Definition RobotModel.hpp:221
std::string base_frame
Definition RobotModel.hpp:67
Eigen::MatrixXd selection_matrix
Definition RobotModel.hpp:75
uint nc()
Return total number of contact points.
Definition RobotModel.hpp:257
urdf::ModelInterfaceSharedPtr getURDFModel()
Return current URDF model.
Definition RobotModel.hpp:296
uint na()
Return number of actuated joints.
Definition RobotModel.hpp:248
bool fk_with_zero_acc_needs_recompute
Definition RobotModel.hpp:88
bool hasJoint(const std::string &joint_name)
Return True if given joint name is available in robot model, false otherwise.
Definition RobotModel.cpp:129
virtual const types::Twist & twist(const std::string &frame_id)=0
virtual bool configure(const RobotModelConfig &cfg)=0
This will read the robot model from the given URDF file and initialize all members accordingly.
uint nj()
Return number of joints.
Definition RobotModel.hpp:245
const types::JointState & jointState()
Definition RobotModel.hpp:151
uint jointIndex(const std::string &joint_name)
Get the internal index for a joint name.
Definition RobotModel.cpp:110
urdf::ModelInterfaceSharedPtr robot_urdf
Definition RobotModel.hpp:72
types::JointState joint_state
Definition RobotModel.hpp:74
Eigen::VectorXd qd
Definition RobotModel.hpp:76
std::map< std::string, Pose > pose_map
Definition RobotModel.hpp:84
const Eigen::VectorXd & getQ()
Return full system position vector, incl. floating base. Convention [floating_base_pos,...
Definition RobotModel.hpp:264
const std::vector< std::string > & actuatedJointNames()
Return only actuated joint names.
Definition RobotModel.hpp:206
std::vector< Matrix > com_jac
Definition RobotModel.hpp:78
bool hasLink(const std::string &link_name)
Return True if given link name is available in robot model, false otherwise.
Definition RobotModel.cpp:119
bool updated
Definition RobotModel.hpp:92
std::vector< std::string > joint_names
Definition RobotModel.hpp:71
Eigen::VectorXd zero_jnt
Definition RobotModel.hpp:76
virtual const Eigen::MatrixXd & comJacobian()=0
Returns the CoM Jacobian for the robot, which maps the robot joint velocities to linear spatial veloc...
bool configured
Definition RobotModel.hpp:92
std::map< std::string, SpatialAcceleration > acc_map
Definition RobotModel.hpp:86
Eigen::VectorXd q
Definition RobotModel.hpp:76
std::map< std::string, Matrix > body_jac_map
Definition RobotModel.hpp:83
void clear()
Definition RobotModel.cpp:33
std::vector< Matrix > joint_space_inertia_mat
Definition RobotModel.hpp:79
Definition JointLimits.hpp:29
The JointState class describes either the state or the command for a set of joints,...
Definition JointState.hpp:13
Definition RigidBodyState.hpp:10
Definition SpatialAcceleration.hpp:8
Definition ContactsAccelerationConstraint.cpp:3
QPSolver * createT()
Definition QPSolver.hpp:35
std::shared_ptr< RobotModel > RobotModelPtr
Definition RobotModel.hpp:315
Robot Model configuration class, containts information like urdf file, type of robot model to be load...
Definition RobotModelConfig.hpp:13
Definition RobotModel.hpp:319
static T * createInstance(const std::string &name)
Definition RobotModel.hpp:330
static RobotModel * createInstance(const std::string &name)
Definition RobotModel.hpp:322
std::map< std::string, RobotModel *(*)()> RobotModelMap
Definition RobotModel.hpp:320
static RobotModelMap * getRobotModelMap()
Definition RobotModel.hpp:336
static void clear()
Definition RobotModel.hpp:342
RobotModelRegistry(const std::string &name)
Definition RobotModel.hpp:351
Matrix()
Definition RobotModel.hpp:48
Eigen::MatrixXd data
Definition RobotModel.hpp:50
bool needs_recompute
Definition RobotModel.hpp:49
bool needs_recompute
Definition RobotModel.hpp:34
Pose()
Definition RobotModel.hpp:33
types::Pose data
Definition RobotModel.hpp:35
bool needs_recompute
Definition RobotModel.hpp:59
types::RigidBodyState data
Definition RobotModel.hpp:60
RigidBodyState()
Definition RobotModel.hpp:58
types::SpatialAcceleration data
Definition RobotModel.hpp:45
SpatialAcceleration()
Definition RobotModel.hpp:43
bool needs_recompute
Definition RobotModel.hpp:44
Twist()
Definition RobotModel.hpp:38
bool needs_recompute
Definition RobotModel.hpp:39
types::Twist data
Definition RobotModel.hpp:40
bool needs_recompute
Definition RobotModel.hpp:54
Eigen::VectorXd data
Definition RobotModel.hpp:55
Vector()
Definition RobotModel.hpp:53