Rigid Body Dynamics Library
Model.h
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #ifndef RBDL_MODEL_H
9 #define RBDL_MODEL_H
10 
11 #include "rbdl/rbdl_math.h"
12 #include <map>
13 #include <list>
14 #include <assert.h>
15 #include <iostream>
16 #include <limits>
17 #include <cstring>
18 
19 #include "rbdl/Logging.h"
20 #include "rbdl/Joint.h"
21 #include "rbdl/Body.h"
22 
23 // std::vectors containing any objects that have Eigen matrices or vectors
24 // as members need to have a special allocater. This can be achieved with
25 // the following macro.
26 
27 #ifdef EIGEN_CORE_H
28 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Joint);
29 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Body);
30 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::FixedBody);
31 #endif
32 
35 namespace RigidBodyDynamics {
36 
121 struct RBDL_DLLAPI Model {
122  Model();
123 
124  // Structural information
125 
127  std::vector<unsigned int> lambda;
130  std::vector<unsigned int> lambda_q;
132  std::vector<std::vector<unsigned int> >mu;
133 
139  unsigned int dof_count;
140 
148  unsigned int q_size;
156  unsigned int qdot_size;
157 
160 
163 
164  // State information
166  std::vector<Math::SpatialVector> v;
168  std::vector<Math::SpatialVector> a;
169 
171  // Joints
172 
174 
175  std::vector<Joint> mJoints;
177  std::vector<Math::SpatialVector> S;
178 
179  // Joint state variables
180  std::vector<Math::SpatialTransform> X_J;
181  std::vector<Math::SpatialVector> v_J;
182  std::vector<Math::SpatialVector> c_J;
183 
184  std::vector<unsigned int> mJointUpdateOrder;
185 
187  // It is expressed in the coordinate frame of the parent.
188  std::vector<Math::SpatialTransform> X_T;
191  std::vector<unsigned int> mFixedJointCount;
192 
194  // Special variables for joints with 3 degrees of freedom
196  std::vector<Math::Matrix63> multdof3_S;
197  std::vector<Math::Matrix63> multdof3_U;
198  std::vector<Math::Matrix3d> multdof3_Dinv;
199  std::vector<Math::Vector3d> multdof3_u;
200  std::vector<unsigned int> multdof3_w_index;
201 
202  std::vector<CustomJoint*> mCustomJoints;
203 
205  // Dynamics variables
206 
208  std::vector<Math::SpatialVector> c;
210  std::vector<Math::SpatialMatrix> IA;
212  std::vector<Math::SpatialVector> pA;
214  std::vector<Math::SpatialVector> U;
220  std::vector<Math::SpatialVector> f;
223  std::vector<Math::SpatialRigidBodyInertia> I;
224  std::vector<Math::SpatialRigidBodyInertia> Ic;
225  std::vector<Math::SpatialVector> hc;
226 
228  // Bodies
229 
235  std::vector<Math::SpatialTransform> X_lambda;
237  std::vector<Math::SpatialTransform> X_base;
238 
240  std::vector<FixedBody> mFixedBodies;
254 
262  std::vector<Body> mBodies;
263 
265  std::map<std::string, unsigned int> mBodyNameMap;
266 
297  unsigned int AddBody (
298  const unsigned int parent_id,
299  const Math::SpatialTransform &joint_frame,
300  const Joint &joint,
301  const Body &body,
302  std::string body_name = ""
303  );
304 
305  unsigned int AddBodySphericalJoint (
306  const unsigned int parent_id,
307  const Math::SpatialTransform &joint_frame,
308  const Joint &joint,
309  const Body &body,
310  std::string body_name = ""
311  );
312 
319  unsigned int AppendBody (
320  const Math::SpatialTransform &joint_frame,
321  const Joint &joint,
322  const Body &body,
323  std::string body_name = ""
324  );
325 
326  unsigned int AddBodyCustomJoint (
327  const unsigned int parent_id,
328  const Math::SpatialTransform &joint_frame,
329  CustomJoint *custom_joint,
330  const Body &body,
331  std::string body_name = ""
332  );
333 
358  unsigned int SetFloatingBaseBody (const Body &body);
359 
371  unsigned int GetBodyId (const char *body_name) const {
372  if (mBodyNameMap.count(body_name) == 0) {
373  return std::numeric_limits<unsigned int>::max();
374  }
375 
376  return mBodyNameMap.find(body_name)->second;
377  }
378 
380  std::string GetBodyName (unsigned int body_id) const {
381  std::map<std::string, unsigned int>::const_iterator iter
382  = mBodyNameMap.begin();
383 
384  while (iter != mBodyNameMap.end()) {
385  if (iter->second == body_id)
386  return iter->first;
387 
388  iter++;
389  }
390 
391  return "";
392  }
393 
396  bool IsFixedBodyId (unsigned int body_id) {
397  if (body_id >= fixed_body_discriminator
398  && body_id < std::numeric_limits<unsigned int>::max()
399  && body_id - fixed_body_discriminator < mFixedBodies.size()) {
400  return true;
401  }
402  return false;
403  }
404 
405  bool IsBodyId (unsigned int id) {
406  if (id > 0 && id < mBodies.size())
407  return true;
408  if (id >= fixed_body_discriminator
409  && id < std::numeric_limits<unsigned int>::max()) {
410  if (id - fixed_body_discriminator < mFixedBodies.size())
411  return true;
412  }
413  return false;
414  }
415 
423  unsigned int GetParentBodyId (unsigned int id) {
424  if (id >= fixed_body_discriminator) {
425  return mFixedBodies[id - fixed_body_discriminator].mMovableParent;
426  }
427 
428  unsigned int parent_id = lambda[id];
429 
430  while (mBodies[parent_id].mIsVirtual) {
431  parent_id = lambda[parent_id];
432  }
433 
434  return parent_id;
435  }
436 
441  if (id >= fixed_body_discriminator) {
442  return mFixedBodies[id - fixed_body_discriminator].mParentTransform;
443  }
444 
445  unsigned int child_id = id;
446  unsigned int parent_id = lambda[id];
447  if (mBodies[parent_id].mIsVirtual) {
448  while (mBodies[parent_id].mIsVirtual) {
449  child_id = parent_id;
450  parent_id = lambda[child_id];
451  }
452  return X_T[child_id];
453  } else
454  return X_T[id];
455  }
456 
460  void SetJointFrame (unsigned int id,
461  const Math::SpatialTransform &transform) {
462  if (id >= fixed_body_discriminator) {
463  std::cerr << "Error: setting of parent transform "
464  << "not supported for fixed bodies!" << std::endl;
465  abort();
466  }
467 
468  unsigned int child_id = id;
469  unsigned int parent_id = lambda[id];
470  if (mBodies[parent_id].mIsVirtual) {
471  while (mBodies[parent_id].mIsVirtual) {
472  child_id = parent_id;
473  parent_id = lambda[child_id];
474  }
475  X_T[child_id] = transform;
476  } else if (id > 0) {
477  X_T[id] = transform;
478  }
479  }
480 
486  Math::Quaternion GetQuaternion (unsigned int i,
487  const Math::VectorNd &Q) const {
488  assert (mJoints[i].mJointType == JointTypeSpherical);
489  unsigned int q_index = mJoints[i].q_index;
490  return Math::Quaternion ( Q[q_index],
491  Q[q_index + 1],
492  Q[q_index + 2],
493  Q[multdof3_w_index[i]]);
494  }
495 
501  void SetQuaternion (unsigned int i,
502  const Math::Quaternion &quat,
503  Math::VectorNd &Q) const {
504  assert (mJoints[i].mJointType == JointTypeSpherical);
505  unsigned int q_index = mJoints[i].q_index;
506 
507  Q[q_index] = quat[0];
508  Q[q_index + 1] = quat[1];
509  Q[q_index + 2] = quat[2];
510  Q[multdof3_w_index[i]] = quat[3];
511  }
512 };
513 
515 }
516 
517 /* _MODEL_H */
518 #endif
std::vector< Math::SpatialVector > hc
Definition: Model.h:225
Compact representation of spatial transformations.
std::vector< unsigned int > lambda
The id of the parents body.
Definition: Model.h:127
Math::Quaternion GetQuaternion(unsigned int i, const Math::VectorNd &Q) const
Definition: Model.h:486
unsigned int q_size
The size of the -vector. For models without spherical joints the value is the same as Model::dof_coun...
Definition: Model.h:148
Math::SpatialTransform GetJointFrame(unsigned int id)
Definition: Model.h:440
std::string GetBodyName(unsigned int body_id) const
Returns the name of a body for a given body id.
Definition: Model.h:380
Quaternion that are used for singularity free joints.
Definition: Quaternion.h:21
bool IsFixedBodyId(unsigned int body_id)
Checks whether the body is rigidly attached to another body.
Definition: Model.h:396
std::vector< Math::SpatialVector > v_J
Definition: Model.h:181
Contains all information about the rigid body model.
Definition: Model.h:121
std::vector< Math::Matrix63 > multdof3_U
Definition: Model.h:197
std::vector< Math::SpatialVector > v
The spatial velocity of the bodies.
Definition: Model.h:166
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
Definition: Model.h:262
std::vector< CustomJoint * > mCustomJoints
Definition: Model.h:202
void SetQuaternion(unsigned int i, const Math::Quaternion &quat, Math::VectorNd &Q) const
Definition: Model.h:501
std::vector< Math::SpatialRigidBodyInertia > Ic
Definition: Model.h:224
unsigned int previously_added_body_id
Id of the previously added body, required for Model::AppendBody()
Definition: Model.h:159
Namespace for all structures of the RigidBodyDynamics library.
Definition: Contacts.cc:22
std::vector< Math::SpatialVector > f
Internal forces on the body (used only InverseDynamics())
Definition: Model.h:220
Math::Vector3d gravity
the cartesian vector of the gravity
Definition: Model.h:162
std::vector< Math::SpatialRigidBodyInertia > I
The spatial inertia of body i (used only in CompositeRigidBodyAlgorithm())
Definition: Model.h:223
std::vector< Math::SpatialTransform > X_lambda
Transformation from the parent body to the current body .
Definition: Model.h:235
std::vector< unsigned int > mJointUpdateOrder
Definition: Model.h:184
std::vector< unsigned int > lambda_q
The index of the parent degree of freedom that is directly influencing the current one...
Definition: Model.h:130
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
Definition: Model.h:240
Math::VectorNd d
Temporary variable D_i (RBDA p. 130)
Definition: Model.h:216
std::vector< unsigned int > mFixedJointCount
The number of fixed joints that have been declared before each joint.
Definition: Model.h:191
unsigned int GetParentBodyId(unsigned int id)
Definition: Model.h:423
unsigned int qdot_size
The size of the.
Definition: Model.h:156
void SetJointFrame(unsigned int id, const Math::SpatialTransform &transform)
Definition: Model.h:460
std::vector< Math::Vector3d > multdof3_u
Definition: Model.h:199
std::vector< unsigned int > multdof3_w_index
Definition: Model.h:200
std::vector< Math::SpatialVector > c
The velocity dependent spatial acceleration.
Definition: Model.h:208
std::vector< std::vector< unsigned int > > mu
Contains the ids of all the children of a given body.
Definition: Model.h:132
std::vector< Math::SpatialTransform > X_J
Definition: Model.h:180
Keeps the information of a body and how it is attached to another body.
Definition: Body.h:189
std::vector< Math::SpatialVector > a
The spatial acceleration of the bodies.
Definition: Model.h:168
unsigned int fixed_body_discriminator
Value that is used to discriminate between fixed and movable bodies.
Definition: Model.h:253
std::vector< Math::SpatialVector > pA
The spatial bias force.
Definition: Model.h:212
Math::VectorNd u
Temporary variable u (RBDA p. 130)
Definition: Model.h:218
unsigned int dof_count
number of degrees of freedoms of the model
Definition: Model.h:139
std::vector< Math::SpatialVector > S
The joint axis for joint i.
Definition: Model.h:177
std::vector< Math::SpatialVector > U
Temporary variable U_i (RBDA p. 130)
Definition: Model.h:214
std::vector< Math::SpatialTransform > X_base
Transformation from the base to bodies reference frame.
Definition: Model.h:237
std::vector< Math::SpatialVector > c_J
Definition: Model.h:182
3 DoF joint using Quaternions for joint positional variables and angular velocity for joint velocity ...
Definition: Joint.h:186
std::vector< Math::Matrix3d > multdof3_Dinv
Definition: Model.h:198
Describes a joint relative to the predecessor body.
Definition: Joint.h:208
unsigned int GetBodyId(const char *body_name) const
Returns the id of a body that was passed to AddBody()
Definition: Model.h:371
std::map< std::string, unsigned int > mBodyNameMap
Human readable names for the bodies.
Definition: Model.h:265
bool IsBodyId(unsigned int id)
Definition: Model.h:405
std::vector< Math::Matrix63 > multdof3_S
Motion subspace for joints with 3 degrees of freedom.
Definition: Model.h:196
std::vector< Math::SpatialMatrix > IA
The spatial inertia of the bodies.
Definition: Model.h:210
std::vector< Joint > mJoints
All joints.
Definition: Model.h:175
Describes all properties of a single body.
Definition: Body.h:26
std::vector< Math::SpatialTransform > X_T
Transformations from the parent body to the frame of the joint.
Definition: Model.h:188