Rigid Body Dynamics Library
Model.h
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
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  std::vector<Math::SpatialVector> hdotc;
227 
229  // Bodies
230 
236  std::vector<Math::SpatialTransform> X_lambda;
238  std::vector<Math::SpatialTransform> X_base;
239 
241  std::vector<FixedBody> mFixedBodies;
255 
263  std::vector<Body> mBodies;
264 
266  std::map<std::string, unsigned int> mBodyNameMap;
267 
298  unsigned int AddBody (
299  const unsigned int parent_id,
300  const Math::SpatialTransform &joint_frame,
301  const Joint &joint,
302  const Body &body,
303  std::string body_name = ""
304  );
305 
306  unsigned int AddBodySphericalJoint (
307  const unsigned int parent_id,
308  const Math::SpatialTransform &joint_frame,
309  const Joint &joint,
310  const Body &body,
311  std::string body_name = ""
312  );
313 
320  unsigned int AppendBody (
321  const Math::SpatialTransform &joint_frame,
322  const Joint &joint,
323  const Body &body,
324  std::string body_name = ""
325  );
326 
327  unsigned int AddBodyCustomJoint (
328  const unsigned int parent_id,
329  const Math::SpatialTransform &joint_frame,
330  CustomJoint *custom_joint,
331  const Body &body,
332  std::string body_name = ""
333  );
334 
346  unsigned int GetBodyId (const char *body_name) const {
347  if (mBodyNameMap.count(body_name) == 0) {
348  return std::numeric_limits<unsigned int>::max();
349  }
350 
351  return mBodyNameMap.find(body_name)->second;
352  }
353 
355  std::string GetBodyName (unsigned int body_id) const {
356  std::map<std::string, unsigned int>::const_iterator iter
357  = mBodyNameMap.begin();
358 
359  while (iter != mBodyNameMap.end()) {
360  if (iter->second == body_id)
361  return iter->first;
362 
363  iter++;
364  }
365 
366  return "";
367  }
368 
371  bool IsFixedBodyId (unsigned int body_id) {
372  if (body_id >= fixed_body_discriminator
373  && body_id < std::numeric_limits<unsigned int>::max()
374  && body_id - fixed_body_discriminator < mFixedBodies.size()) {
375  return true;
376  }
377  return false;
378  }
379 
380  bool IsBodyId (unsigned int id) {
381  if (id > 0 && id < mBodies.size())
382  return true;
383  if (id >= fixed_body_discriminator
384  && id < std::numeric_limits<unsigned int>::max()) {
385  if (id - fixed_body_discriminator < mFixedBodies.size())
386  return true;
387  }
388  return false;
389  }
390 
398  unsigned int GetParentBodyId (unsigned int id) {
399  if (id >= fixed_body_discriminator) {
400  return mFixedBodies[id - fixed_body_discriminator].mMovableParent;
401  }
402 
403  unsigned int parent_id = lambda[id];
404 
405  while (mBodies[parent_id].mIsVirtual) {
406  parent_id = lambda[parent_id];
407  }
408 
409  return parent_id;
410  }
411 
416  if (id >= fixed_body_discriminator) {
417  return mFixedBodies[id - fixed_body_discriminator].mParentTransform;
418  }
419 
420  unsigned int child_id = id;
421  unsigned int parent_id = lambda[id];
422  if (mBodies[parent_id].mIsVirtual) {
423  while (mBodies[parent_id].mIsVirtual) {
424  child_id = parent_id;
425  parent_id = lambda[child_id];
426  }
427  return X_T[child_id];
428  } else
429  return X_T[id];
430  }
431 
435  void SetJointFrame (unsigned int id,
436  const Math::SpatialTransform &transform) {
437  if (id >= fixed_body_discriminator) {
438  std::cerr << "Error: setting of parent transform "
439  << "not supported for fixed bodies!" << std::endl;
440  abort();
441  }
442 
443  unsigned int child_id = id;
444  unsigned int parent_id = lambda[id];
445  if (mBodies[parent_id].mIsVirtual) {
446  while (mBodies[parent_id].mIsVirtual) {
447  child_id = parent_id;
448  parent_id = lambda[child_id];
449  }
450  X_T[child_id] = transform;
451  } else if (id > 0) {
452  X_T[id] = transform;
453  }
454  }
455 
461  Math::Quaternion GetQuaternion (unsigned int i,
462  const Math::VectorNd &Q) const {
463  assert (mJoints[i].mJointType == JointTypeSpherical);
464  unsigned int q_index = mJoints[i].q_index;
465  return Math::Quaternion ( Q[q_index],
466  Q[q_index + 1],
467  Q[q_index + 2],
468  Q[multdof3_w_index[i]]);
469  }
470 
476  void SetQuaternion (unsigned int i,
477  const Math::Quaternion &quat,
478  Math::VectorNd &Q) const {
479  assert (mJoints[i].mJointType == JointTypeSpherical);
480  unsigned int q_index = mJoints[i].q_index;
481 
482  Q[q_index] = quat[0];
483  Q[q_index + 1] = quat[1];
484  Q[q_index + 2] = quat[2];
485  Q[multdof3_w_index[i]] = quat[3];
486  }
487 };
488 
490 }
491 
492 /* _MODEL_H */
493 #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
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:415
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:371
unsigned int GetBodyId(const char *body_name) const
Returns the id of a body that was passed to AddBody()
Definition: Model.h:346
std::vector< Math::SpatialVector > v_J
Definition: Model.h:181
std::string GetBodyName(unsigned int body_id) const
Returns the name of a body for a given body id.
Definition: Model.h:355
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:263
std::vector< CustomJoint * > mCustomJoints
Definition: Model.h:202
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
void SetQuaternion(unsigned int i, const Math::Quaternion &quat, Math::VectorNd &Q) const
Definition: Model.h:476
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
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:236
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:241
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:398
Math::Quaternion GetQuaternion(unsigned int i, const Math::VectorNd &Q) const
Definition: Model.h:461
unsigned int qdot_size
The size of the.
Definition: Model.h:156
void SetJointFrame(unsigned int id, const Math::SpatialTransform &transform)
Definition: Model.h:435
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::SpatialVector > hdotc
Definition: Model.h:226
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:254
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:238
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:209
std::map< std::string, unsigned int > mBodyNameMap
Human readable names for the bodies.
Definition: Model.h:266
bool IsBodyId(unsigned int id)
Definition: Model.h:380
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