wbc
RobotModel.hpp
Go to the documentation of this file.
1#ifndef WBC_CORE_ROBOTMODEL_HPP
2#define WBC_CORE_ROBOTMODEL_HPP
3
4#include <memory>
5#include <map>
9#include "../types/Wrench.hpp"
10#include "RobotModelConfig.hpp"
11#include <urdf_world/world.h>
12
13namespace wbc{
14
27protected:
28 void clear();
29 void resetData();
30 void updateData();
31
47 struct Matrix{
50 Eigen::MatrixXd data;
51 };
52 struct Vector{
55 Eigen::VectorXd data;
56 };
62
63 std::vector<types::Contact> contacts;
64 Eigen::Vector3d gravity;
67 std::string world_frame, base_frame;
69 std::vector<std::string> actuated_joint_names;
70 std::vector<std::string> independent_joint_names;
71 std::vector<std::string> joint_names;
72 urdf::ModelInterfaceSharedPtr robot_urdf;
75 Eigen::MatrixXd selection_matrix;
76 Eigen::VectorXd q, qd, qdd, tau, zero_jnt;
77
78 std::vector<Matrix> com_jac;
79 std::vector<Matrix> joint_space_inertia_mat;
80 std::vector<Vector> bias_forces;
81 std::vector<RigidBodyState> com_rbs;
82 std::map<std::string,Matrix> space_jac_map;
83 std::map<std::string,Matrix> body_jac_map;
84 std::map<std::string,Pose> pose_map;
85 std::map<std::string,Twist> twist_map;
86 std::map<std::string,SpatialAcceleration> acc_map;
87 std::map<std::string,SpatialAcceleration> spatial_acc_bias_map;
89 Eigen::VectorXd joint_weights;
90
93
94public:
95 RobotModel();
96 virtual ~RobotModel(){}
97
103 virtual bool configure(const RobotModelConfig& cfg) = 0;
104
109 void update(const Eigen::VectorXd& joint_positions,
110 const Eigen::VectorXd& joint_velocities);
111
117 virtual void update(const Eigen::VectorXd& joint_positions,
118 const Eigen::VectorXd& joint_velocities,
119 const Eigen::VectorXd& joint_accelerations);
120
128 void update(const Eigen::VectorXd& joint_positions,
129 const Eigen::VectorXd& joint_velocities,
130 const types::Pose& fb_pose,
131 const types::Twist& fb_twist);
132
143 virtual void update(const Eigen::VectorXd& joint_positions,
144 const Eigen::VectorXd& joint_velocities,
145 const Eigen::VectorXd& joint_accelerations,
146 const types::Pose& fb_pose,
147 const types::Twist& fb_twist,
148 const types::SpatialAcceleration& fb_acc) = 0;
149
152
155 virtual const types::Pose &pose(const std::string &frame_id) = 0;
156
159 virtual const types::Twist &twist(const std::string &frame_id) = 0;
160
163 virtual const types::SpatialAcceleration &acceleration(const std::string &frame_id) = 0;
164
165
172 virtual const Eigen::MatrixXd &spaceJacobian(const std::string &frame_id) = 0;
173
178 virtual const Eigen::MatrixXd &bodyJacobian(const std::string &frame_id) = 0;
179
184 virtual const Eigen::MatrixXd &comJacobian() = 0;
185
191 virtual const types::SpatialAcceleration &spatialAccelerationBias(const std::string &frame_id) = 0;
192
194 virtual const Eigen::MatrixXd &jointSpaceInertiaMatrix() = 0;
195
197 virtual const Eigen::VectorXd &biasForces() = 0;
198
203 const std::vector<std::string>& jointNames(){return joint_names;}
204
206 const std::vector<std::string>& actuatedJointNames(){return actuated_joint_names;}
207
209 const std::vector<std::string>& independentJointNames(){return independent_joint_names;}
210
212 uint jointIndex(const std::string &joint_name);
213
215 const std::string& baseFrame(){return base_frame;}
216
218 const std::string& worldFrame(){return world_frame;}
219
222
224 const Eigen::MatrixXd &selectionMatrix(){return selection_matrix;}
225
227 bool hasLink(const std::string& link_name);
228
230 bool hasJoint(const std::string& joint_name);
231
233 bool hasActuatedJoint(const std::string& joint_name);
234
237
239 void setContacts(const std::vector<types::Contact> &contacts);
240
242 const std::vector<types::Contact>& getContacts(){return contacts;}
243
245 uint nj(){return jointNames().size() + (has_floating_base ? 6 : 0);}
246
248 uint na(){return actuatedJointNames().size();}
249
251 uint nfb(){return has_floating_base ? 6 : 0;}
252
254 uint nac();
255
257 uint nc(){return contacts.size();}
258
264 const Eigen::VectorXd &getQ(){return q;}
265
271 const Eigen::VectorXd &getQd(){return qd;}
272
278 const Eigen::VectorXd &getQdd(){return qdd;}
279
281 void setGravityVector(const Eigen::Vector3d& g){gravity=g;}
282
285
288
291
293 urdf::ModelInterfaceSharedPtr loadRobotURDF(const std::string& file_or_string);
294
296 urdf::ModelInterfaceSharedPtr getURDFModel(){return robot_urdf;}
297
301 void setJointWeights(const Eigen::VectorXd& weights);
302
306 const Eigen::VectorXd& getJointWeights() const { return joint_weights; }
307
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;
313
314};
315typedef std::shared_ptr<RobotModel> RobotModelPtr;
316
317template<typename T> RobotModel* createT(){return new T;}
318
320 typedef std::map<std::string, RobotModel*(*)()> RobotModelMap;
321
322 static RobotModel *createInstance(const std::string& name) {
323 RobotModelMap::iterator it = getRobotModelMap()->find(name);
324 if(it == getRobotModelMap()->end())
325 throw std::runtime_error("Failed to create instance of plugin " + name + ". Is the plugin registered?");
326 return it->second();
327 }
328
329 template<typename T>
330 static T* createInstance(const std::string& name){
331 RobotModel* tmp = createInstance(name);
332 T* ret = dynamic_cast<T*>(tmp);
333 return ret;
334 }
335
337 if(!robot_model_map)
338 robot_model_map = new RobotModelMap;
339 return robot_model_map;
340 }
341
342 static void clear(){
343 robot_model_map->clear();
344 }
345private:
346 static RobotModelMap *robot_model_map;
347};
348
349template<typename T>
351 RobotModelRegistry(const std::string& name) {
352 RobotModelMap::iterator it = getRobotModelMap()->find(name);
353 if(it != getRobotModelMap()->end())
354 throw std::runtime_error("Failed to register plugin with name " + name + ". A plugin with the same name is already registered");
355 getRobotModelMap()->insert(std::make_pair(name, &createT<T>));
356 }
357};
358
359}
360
361#endif // WBC_CORE_ROBOTMODEL_HPP
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 Pose.hpp:9
Definition RigidBodyState.hpp:10
Definition SpatialAcceleration.hpp:8
Definition Twist.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