Rigid Body Dynamics Library
Model.cc
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 #include <iostream>
9 #include <limits>
10 #include <assert.h>
11 
12 #include "rbdl/rbdl_mathutils.h"
13 
14 #include "rbdl/Logging.h"
15 
16 #include "rbdl/Model.h"
17 #include "rbdl/Body.h"
18 #include "rbdl/Joint.h"
19 
20 using namespace RigidBodyDynamics;
21 using namespace RigidBodyDynamics::Math;
22 
24  Body root_body;
25  Joint root_joint;
26 
27  Vector3d zero_position (0., 0., 0.);
28  SpatialVector zero_spatial (0., 0., 0., 0., 0., 0.);
29 
30  // structural information
31  lambda.push_back(0);
32  lambda_q.push_back(0);
33  mu.push_back(std::vector<unsigned int>());
34  dof_count = 0;
35  q_size = 0;
36  qdot_size = 0;
37  previously_added_body_id = 0;
38 
39  gravity = Vector3d (0., -9.81, 0.);
40 
41  // state information
42  v.push_back(zero_spatial);
43  a.push_back(zero_spatial);
44 
45  // Joints
46  mJoints.push_back(root_joint);
47  S.push_back (zero_spatial);
48  X_T.push_back(SpatialTransform());
49 
50  X_J.push_back (SpatialTransform());
51  v_J.push_back (zero_spatial);
52  c_J.push_back (zero_spatial);
53 
54  // Spherical joints
55  multdof3_S.push_back (Matrix63::Zero());
56  multdof3_U.push_back (Matrix63::Zero());
57  multdof3_Dinv.push_back (Matrix3d::Zero());
58  multdof3_u.push_back (Vector3d::Zero());
59  multdof3_w_index.push_back (0);
60 
61  // Dynamic variables
62  c.push_back(zero_spatial);
63  IA.push_back(SpatialMatrix::Identity());
64  pA.push_back(zero_spatial);
65  U.push_back(zero_spatial);
66 
67  u = VectorNd::Zero(1);
68  d = VectorNd::Zero(1);
69 
70  f.push_back (zero_spatial);
72  Vector3d (0., 0., 0.),
73  Matrix3d::Zero(3,3));
74  Ic.push_back (rbi);
75  I.push_back(rbi);
76  hc.push_back (zero_spatial);
77  hdotc.push_back (zero_spatial);
78 
79  // Bodies
80  X_lambda.push_back(SpatialTransform());
81  X_base.push_back(SpatialTransform());
82 
83  mBodies.push_back(root_body);
84  mBodyNameMap["ROOT"] = 0;
85 
86  fixed_body_discriminator = std::numeric_limits<unsigned int>::max() / 2;
87 }
88 
89 unsigned int AddBodyFixedJoint (
90  Model &model,
91  const unsigned int parent_id,
92  const SpatialTransform &joint_frame,
93  const Joint &joint,
94  const Body &body,
95  std::string body_name) {
96  FixedBody fbody = FixedBody::CreateFromBody (body);
97  fbody.mMovableParent = parent_id;
98  fbody.mParentTransform = joint_frame;
99 
100  if (model.IsFixedBodyId(parent_id)) {
101  FixedBody fixed_parent =
102  model.mFixedBodies[parent_id - model.fixed_body_discriminator];
103 
104  fbody.mMovableParent = fixed_parent.mMovableParent;
105  fbody.mParentTransform = joint_frame * fixed_parent.mParentTransform;
106  }
107 
108  // merge the two bodies
109  Body parent_body = model.mBodies[fbody.mMovableParent];
110  parent_body.Join (fbody.mParentTransform, body);
111  model.mBodies[fbody.mMovableParent] = parent_body;
112  model.I[fbody.mMovableParent] =
114  parent_body.mMass,
115  parent_body.mCenterOfMass,
116  parent_body.mInertia);
117 
118  model.mFixedBodies.push_back (fbody);
119 
120  if (model.mFixedBodies.size() >
121  std::numeric_limits<unsigned int>::max() - model.fixed_body_discriminator) {
122  std::cerr << "Error: cannot add more than "
123  << std::numeric_limits<unsigned int>::max()
124  - model.mFixedBodies.size()
125  << " fixed bodies. You need to modify "
126  << "Model::fixed_body_discriminator for this."
127  << std::endl;
128  assert (0);
129  abort();
130  }
131 
132  if (body_name.size() != 0) {
133  if (model.mBodyNameMap.find(body_name) != model.mBodyNameMap.end()) {
134  std::cerr << "Error: Body with name '"
135  << body_name
136  << "' already exists!"
137  << std::endl;
138  assert (0);
139  abort();
140  }
141  model.mBodyNameMap[body_name] = model.mFixedBodies.size()
142  + model.fixed_body_discriminator - 1;
143  }
144 
145  return model.mFixedBodies.size() + model.fixed_body_discriminator - 1;
146 }
147 
148 unsigned int AddBodyMultiDofJoint (
149  Model &model,
150  const unsigned int parent_id,
151  const SpatialTransform &joint_frame,
152  const Joint &joint,
153  const Body &body,
154  std::string body_name) {
155  // Here we emulate multi DoF joints by simply adding nullbodies. This
156  // allows us to use fixed size elements for S,v,a, etc. which is very
157  // fast in Eigen.
158  unsigned int joint_count = 0;
159  if (joint.mJointType == JointType1DoF)
160  joint_count = 1;
161  else if (joint.mJointType == JointType2DoF)
162  joint_count = 2;
163  else if (joint.mJointType == JointType3DoF)
164  joint_count = 3;
165  else if (joint.mJointType == JointType4DoF)
166  joint_count = 4;
167  else if (joint.mJointType == JointType5DoF)
168  joint_count = 5;
169  else if (joint.mJointType == JointType6DoF)
170  joint_count = 6;
171  else if (joint.mJointType == JointTypeFloatingBase)
172  // no action required
173  {}
174  else {
175  std::cerr << "Error: Invalid joint type: "
176  << joint.mJointType
177  << std::endl;
178 
179  assert (0 && !"Invalid joint type!");
180  }
181 
182  Body null_body (0., Vector3d (0., 0., 0.), Vector3d (0., 0., 0.));
183  null_body.mIsVirtual = true;
184 
185  unsigned int null_parent = parent_id;
186  SpatialTransform joint_frame_transform;
187 
188  if (joint.mJointType == JointTypeFloatingBase) {
189  null_parent = model.AddBody (parent_id,
190  joint_frame,
192  null_body);
193 
194  return model.AddBody (null_parent,
195  SpatialTransform(),
197  body,
198  body_name);
199  }
200 
201  Joint single_dof_joint;
202  unsigned int j;
203 
204  // Here we add multiple virtual bodies that have no mass or inertia for
205  // which each is attached to the model with a single degree of freedom
206  // joint.
207  for (j = 0; j < joint_count; j++) {
208  single_dof_joint = Joint (joint.mJointAxes[j]);
209 
210  if (single_dof_joint.mJointType == JointType1DoF) {
211  Vector3d rotation (
212  joint.mJointAxes[j][0],
213  joint.mJointAxes[j][1],
214  joint.mJointAxes[j][2]);
215  Vector3d translation (
216  joint.mJointAxes[j][3],
217  joint.mJointAxes[j][4],
218  joint.mJointAxes[j][5]);
219 
220  if (rotation == Vector3d (0., 0., 0.)) {
221  single_dof_joint = Joint (JointTypePrismatic, translation);
222  } else if (translation == Vector3d (0., 0., 0.)) {
223  single_dof_joint = Joint (JointTypeRevolute, rotation);
224  }
225  }
226 
227  // the first joint has to be transformed by joint_frame, all the
228  // others must have a null transformation
229  if (j == 0)
230  joint_frame_transform = joint_frame;
231  else
232  joint_frame_transform = SpatialTransform();
233 
234  if (j == joint_count - 1)
235  // if we are at the last we must add the real body
236  break;
237  else {
238  // otherwise we just add an intermediate body
239  null_parent = model.AddBody (null_parent,
240  joint_frame_transform,
241  single_dof_joint,
242  null_body);
243  }
244  }
245 
246  return model.AddBody (null_parent,
247  joint_frame_transform,
248  single_dof_joint,
249  body,
250  body_name);
251 }
252 
253 unsigned int Model::AddBody(
254  const unsigned int parent_id,
255  const SpatialTransform &joint_frame,
256  const Joint &joint,
257  const Body &body,
258  std::string body_name) {
259  assert (lambda.size() > 0);
260  assert (joint.mJointType != JointTypeUndefined);
261 
262  if (joint.mJointType == JointTypeFixed) {
263  previously_added_body_id = AddBodyFixedJoint (*this,
264  parent_id,
265  joint_frame,
266  joint,
267  body,
268  body_name);
269 
270  return previously_added_body_id;
271  } else if ( (joint.mJointType == JointTypeSpherical)
272  || (joint.mJointType == JointTypeEulerZYX)
273  || (joint.mJointType == JointTypeEulerXYZ)
274  || (joint.mJointType == JointTypeEulerYXZ)
275  || (joint.mJointType == JointTypeTranslationXYZ)
276  || (joint.mJointType == JointTypeCustom)
277  ) {
278  // no action required
279  } else if (joint.mJointType != JointTypePrismatic
280  && joint.mJointType != JointTypeRevolute
281  && joint.mJointType != JointTypeRevoluteX
282  && joint.mJointType != JointTypeRevoluteY
283  && joint.mJointType != JointTypeRevoluteZ
284  && joint.mJointType != JointTypeHelical
285  ) {
286  previously_added_body_id = AddBodyMultiDofJoint (*this,
287  parent_id,
288  joint_frame,
289  joint,
290  body,
291  body_name);
292  return previously_added_body_id;
293  }
294 
295  // If we add the body to a fixed body we have to make sure that we
296  // actually add it to its movable parent.
297  unsigned int movable_parent_id = parent_id;
298  SpatialTransform movable_parent_transform;
299 
300  if (IsFixedBodyId(parent_id)) {
301  unsigned int fbody_id = parent_id - fixed_body_discriminator;
302  movable_parent_id = mFixedBodies[fbody_id].mMovableParent;
303  movable_parent_transform = mFixedBodies[fbody_id].mParentTransform;
304  }
305 
306  // structural information
307  lambda.push_back(movable_parent_id);
308  unsigned int lambda_q_last = mJoints[mJoints.size() - 1].q_index;
309 
310  if (mJoints[mJoints.size() - 1].mDoFCount > 0
311  && mJoints[mJoints.size() - 1].mJointType != JointTypeCustom) {
312  lambda_q_last = lambda_q_last + mJoints[mJoints.size() - 1].mDoFCount;
313  } else if (mJoints[mJoints.size() - 1].mJointType == JointTypeCustom) {
314  unsigned int custom_index = mJoints[mJoints.size() - 1].custom_joint_index;
315  lambda_q_last = lambda_q_last
316  + mCustomJoints[mCustomJoints.size() - 1]->mDoFCount;
317  }
318 
319  for (unsigned int i = 0; i < joint.mDoFCount; i++) {
320  lambda_q.push_back(lambda_q_last + i);
321  }
322  mu.push_back(std::vector<unsigned int>());
323  mu.at(movable_parent_id).push_back(mBodies.size());
324 
325  // Bodies
326  X_lambda.push_back(SpatialTransform());
327  X_base.push_back(SpatialTransform());
328  mBodies.push_back(body);
329 
330  if (body_name.size() != 0) {
331  if (mBodyNameMap.find(body_name) != mBodyNameMap.end()) {
332  std::cerr << "Error: Body with name '"
333  << body_name
334  << "' already exists!"
335  << std::endl;
336  assert (0);
337  abort();
338  }
339  mBodyNameMap[body_name] = mBodies.size() - 1;
340  }
341 
342  // state information
343  v.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
344  a.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
345 
346  // Joints
347  unsigned int prev_joint_index = mJoints.size() - 1;
348  mJoints.push_back(joint);
349 
350  if (mJoints[prev_joint_index].mJointType != JointTypeCustom) {
351  mJoints[mJoints.size() - 1].q_index =
352  mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount;
353  } else {
354  mJoints[mJoints.size() - 1].q_index =
355  mJoints[prev_joint_index].q_index + mJoints[prev_joint_index].mDoFCount;
356  }
357 
358  S.push_back (joint.mJointAxes[0]);
359 
360  // Joint state variables
361  X_J.push_back (SpatialTransform());
362  v_J.push_back (joint.mJointAxes[0]);
363  c_J.push_back (SpatialVector(0., 0., 0., 0., 0., 0.));
364 
365  // workspace for joints with 3 dof
366  multdof3_S.push_back (Matrix63::Zero(6,3));
367  multdof3_U.push_back (Matrix63::Zero());
368  multdof3_Dinv.push_back (Matrix3d::Zero());
369  multdof3_u.push_back (Vector3d::Zero());
370  multdof3_w_index.push_back (0);
371 
372  dof_count = dof_count + joint.mDoFCount;
373 
374  // update the w components of the Quaternions. They are stored at the end
375  // of the q vector
376  int multdof3_joint_counter = 0;
377  int mCustomJoint_joint_counter = 0;
378  for (unsigned int i = 1; i < mJoints.size(); i++) {
379  if (mJoints[i].mJointType == JointTypeSpherical) {
380  multdof3_w_index[i] = dof_count + multdof3_joint_counter;
381  multdof3_joint_counter++;
382  }
383  }
384 
385  q_size = dof_count
386  + multdof3_joint_counter;
387 
388  qdot_size = qdot_size + joint.mDoFCount;
389 
390  // we have to invert the transformation as it is later always used from the
391  // child bodies perspective.
392  X_T.push_back(joint_frame * movable_parent_transform);
393 
394  // Dynamic variables
395  c.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
396  IA.push_back(SpatialMatrix::Zero(6,6));
397  pA.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
398  U.push_back(SpatialVector(0., 0., 0., 0., 0., 0.));
399 
400  d = VectorNd::Zero (mBodies.size());
401  u = VectorNd::Zero (mBodies.size());
402 
403  f.push_back (SpatialVector (0., 0., 0., 0., 0., 0.));
404 
407  body.mCenterOfMass,
408  body.mInertia);
409 
410  Ic.push_back (rbi);
411  I.push_back (rbi);
412  hc.push_back (SpatialVector(0., 0., 0., 0., 0., 0.));
413  hdotc.push_back (SpatialVector(0., 0., 0., 0., 0., 0.));
414 
415  if (mBodies.size() == fixed_body_discriminator) {
416  std::cerr << "Error: cannot add more than "
417  << fixed_body_discriminator
418  << " movable bodies. You need to modify "
419  "Model::fixed_body_discriminator for this."
420  << std::endl;
421  assert (0);
422  abort();
423  }
424 
425  previously_added_body_id = mBodies.size() - 1;
426 
427  mJointUpdateOrder.clear();
428 
429  // update the joint order computation
430  std::vector<std::pair<JointType, unsigned int> > joint_types;
431  for (unsigned int i = 0; i < mJoints.size(); i++) {
432  joint_types.push_back(
433  std::pair<JointType, unsigned int> (mJoints[i].mJointType,i));
434  mJointUpdateOrder.push_back (mJoints[i].mJointType);
435  }
436 
437  mJointUpdateOrder.clear();
438  JointType current_joint_type = JointTypeUndefined;
439  while (joint_types.size() != 0) {
440  current_joint_type = joint_types[0].first;
441 
442  std::vector<std::pair<JointType, unsigned int> >::iterator type_iter =
443  joint_types.begin();
444 
445  while (type_iter != joint_types.end()) {
446  if (type_iter->first == current_joint_type) {
447  mJointUpdateOrder.push_back (type_iter->second);
448  type_iter = joint_types.erase (type_iter);
449  } else {
450  ++type_iter;
451  }
452  }
453  }
454 
455  // for (unsigned int i = 0; i < mJointUpdateOrder.size(); i++) {
456  // std::cout << "i = " << i << ": joint_id = " << mJointUpdateOrder[i]
457  // << " joint_type = " << mJoints[mJointUpdateOrder[i]].mJointType << std::endl;
458  // }
459 
460  return previously_added_body_id;
461 }
462 
463 unsigned int Model::AppendBody (
464  const Math::SpatialTransform &joint_frame,
465  const Joint &joint,
466  const Body &body,
467  std::string body_name) {
468  return Model::AddBody (previously_added_body_id,
469  joint_frame,
470  joint,
471  body,
472  body_name);
473 }
474 
476  const unsigned int parent_id,
477  const Math::SpatialTransform &joint_frame,
478  CustomJoint *custom_joint,
479  const Body &body,
480  std::string body_name) {
481  Joint proxy_joint (JointTypeCustom, custom_joint->mDoFCount);
482  proxy_joint.custom_joint_index = mCustomJoints.size();
483  //proxy_joint.mDoFCount = custom_joint->mDoFCount; //MM added. Otherwise
484  //model.q_size = 0, which is not good.
485 
486  mCustomJoints.push_back (custom_joint);
487 
488  unsigned int body_id = AddBody (parent_id,
489  joint_frame,
490  proxy_joint,
491  body,
492  body_name);
493 
494  return body_id;
495 }
496 
Compact representation of spatial transformations.
Compact representation for Spatial Inertia.
static SpatialRigidBodyInertia createFromMassComInertiaC(double mass, const Vector3d &com, const Matrix3d &inertia_C)
Emulated 2 DoF joint.
Definition: Joint.h:195
bool IsFixedBodyId(unsigned int body_id)
Checks whether the body is rigidly attached to another body.
Definition: Model.h:371
Contains all information about the rigid body model.
Definition: Model.h:121
Emulated 4 DoF joint.
Definition: Joint.h:197
SpatialVector_t SpatialVector
Definition: rbdl_math.h:59
std::vector< Body > mBodies
All bodies 0 ... N_B, including the base.
Definition: Model.h:263
unsigned int AddBody(const unsigned int parent_id, const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Connects a given body to the model.
Definition: Model.cc:253
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
Fixed joint which causes the inertial properties to be merged with the parent body.
Definition: Joint.h:192
std::vector< Math::SpatialRigidBodyInertia > I
The spatial inertia of body i (used only in CompositeRigidBodyAlgorithm())
Definition: Model.h:223
unsigned int AddBodyFixedJoint(Model &model, const unsigned int parent_id, const SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name)
Definition: Model.cc:89
unsigned int mMovableParent
Id of the movable body that this fixed body is attached to.
Definition: Body.h:198
unsigned int AddBodyMultiDofJoint(Model &model, const unsigned int parent_id, const SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name)
Definition: Model.cc:148
JointType
General types of joints.
Definition: Joint.h:179
void Join(const Math::SpatialTransform &transform, const Body &other_body)
Joins inertial parameters of two bodies to create a composite body.
Definition: Body.h:107
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
Definition: Model.h:241
3 DoF joint that uses Euler ZYX convention (faster than emulated multi DoF joints).
Definition: Joint.h:187
unsigned int AppendBody(const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
Adds a Body to the model such that the previously added Body is the Parent.
Definition: Model.cc:463
Math::Matrix3d mInertia
Inertia matrix at the center of mass.
Definition: Body.h:178
3 DoF joint that uses Euler XYZ convention (faster than emulated multi DoF joints).
Definition: Joint.h:188
Math::Vector3d mCenterOfMass
The position of the center of mass in body coordinates.
Definition: Body.h:176
unsigned int mDoFCount
Number of degrees of freedom of the joint. Note: CustomJoints.
Definition: Joint.h:631
Math types such as vectors and matrices and utility functions.
unsigned int AddBodyCustomJoint(const unsigned int parent_id, const Math::SpatialTransform &joint_frame, CustomJoint *custom_joint, const Body &body, std::string body_name="")
Definition: Model.cc:475
Math::SpatialTransform mParentTransform
Transforms spatial quantities expressed for the parent to the.
Definition: Body.h:201
Keeps the information of a body and how it is attached to another body.
Definition: Body.h:189
A 6-DoF joint for floating-base (or freeflyer) systems.
Definition: Joint.h:191
unsigned int fixed_body_discriminator
Value that is used to discriminate between fixed and movable bodies.
Definition: Model.h:254
double mMass
The mass of the body.
Definition: Body.h:171
Emulated 6 DoF joint.
Definition: Joint.h:199
static FixedBody CreateFromBody(const Body &body)
Definition: Body.h:204
Emulated 3 DoF joint.
Definition: Joint.h:196
JointType mJointType
Type of joint.
Definition: Joint.h:627
3 DoF joint using Quaternions for joint positional variables and angular velocity for joint velocity ...
Definition: Joint.h:186
Describes a joint relative to the predecessor body.
Definition: Joint.h:209
3 DoF joint that uses Euler YXZ convention (faster than emulated multi DoF joints).
Definition: Joint.h:189
std::map< std::string, unsigned int > mBodyNameMap
Human readable names for the bodies.
Definition: Model.h:266
Math::SpatialVector * mJointAxes
The spatial axes of the joint.
Definition: Joint.h:625
Emulated 5 DoF joint.
Definition: Joint.h:198
Describes all properties of a single body.
Definition: Body.h:26
User defined joints of varying size.
Definition: Joint.h:200