Rigid Body Dynamics Library
rbdl_utils.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 "rbdl/rbdl_utils.h"
9 
10 #include "rbdl/rbdl_math.h"
11 #include "rbdl/Model.h"
12 #include "rbdl/Kinematics.h"
13 
14 #include <sstream>
15 #include <iomanip>
16 
17 namespace RigidBodyDynamics {
18 
19 namespace Utils {
20 
21 using namespace std;
22 using namespace Math;
23 
24 string get_dof_name (const SpatialVector &joint_dof) {
25  if (joint_dof == SpatialVector (1., 0., 0., 0., 0., 0.))
26  return "RX";
27  else if (joint_dof == SpatialVector (0., 1., 0., 0., 0., 0.))
28  return "RY";
29  else if (joint_dof == SpatialVector (0., 0., 1., 0., 0., 0.))
30  return "RZ";
31  else if (joint_dof == SpatialVector (0., 0., 0., 1., 0., 0.))
32  return "TX";
33  else if (joint_dof == SpatialVector (0., 0., 0., 0., 1., 0.))
34  return "TY";
35  else if (joint_dof == SpatialVector (0., 0., 0., 0., 0., 1.))
36  return "TZ";
37 
38  ostringstream dof_stream(ostringstream::out);
39  dof_stream << "custom (" << joint_dof.transpose() << ")";
40  return dof_stream.str();
41 }
42 
43 string get_body_name (const RigidBodyDynamics::Model &model, unsigned int body_id) {
44  if (model.mBodies[body_id].mIsVirtual) {
45  // if there is not a unique child we do not know what to do...
46  if (model.mu[body_id].size() != 1)
47  return "";
48 
49  return get_body_name (model, model.mu[body_id][0]);
50  }
51 
52  return model.GetBodyName(body_id);
53 }
54 
55 RBDL_DLLAPI std::string GetModelDOFOverview (const Model &model) {
56  stringstream result ("");
57 
58  unsigned int q_index = 0;
59  for (unsigned int i = 1; i < model.mBodies.size(); i++) {
60  if (model.mJoints[i].mDoFCount == 1) {
61  result << setfill(' ') << setw(3) << q_index << ": " << get_body_name(model, i) << "_" << get_dof_name (model.S[i]) << endl;
62  q_index++;
63  } else {
64  for (unsigned int j = 0; j < model.mJoints[i].mDoFCount; j++) {
65  result << setfill(' ') << setw(3) << q_index << ": " << get_body_name(model, i) << "_" << get_dof_name (model.mJoints[i].mJointAxes[j]) << endl;
66  q_index++;
67  }
68  }
69  }
70 
71  return result.str();
72 }
73 
74 std::string print_hierarchy (const RigidBodyDynamics::Model &model, unsigned int body_index = 0, int indent = 0) {
75  stringstream result ("");
76 
77  for (int j = 0; j < indent; j++)
78  result << " ";
79 
80  result << get_body_name (model, body_index);
81 
82  if (body_index > 0)
83  result << " [ ";
84 
85  while (model.mBodies[body_index].mIsVirtual) {
86  if (model.mu[body_index].size() == 0) {
87  result << " end";
88  break;
89  } else if (model.mu[body_index].size() > 1) {
90  cerr << endl << "Error: Cannot determine multi-dof joint as massless body with id " << body_index << " (name: " << model.GetBodyName(body_index) << ") has more than one child:" << endl;
91  for (unsigned int ci = 0; ci < model.mu[body_index].size(); ci++) {
92  cerr << " id: " << model.mu[body_index][ci] << " name: " << model.GetBodyName(model.mu[body_index][ci]) << endl;
93  }
94  abort();
95  }
96 
97  result << get_dof_name(model.S[body_index]) << ", ";
98 
99  body_index = model.mu[body_index][0];
100  }
101 
102  if (body_index > 0)
103  result << get_dof_name(model.S[body_index]) << " ]";
104  result << endl;
105 
106  unsigned int child_index = 0;
107  for (child_index = 0; child_index < model.mu[body_index].size(); child_index++) {
108  result << print_hierarchy (model, model.mu[body_index][child_index], indent + 1);
109  }
110 
111  // print fixed children
112  for (unsigned int fbody_index = 0; fbody_index < model.mFixedBodies.size(); fbody_index++) {
113  if (model.mFixedBodies[fbody_index].mMovableParent == body_index) {
114  for (int j = 0; j < indent + 1; j++)
115  result << " ";
116 
117  result << model.GetBodyName(model.fixed_body_discriminator + fbody_index) << " [fixed]" << endl;
118  }
119  }
120 
121 
122  return result.str();
123 }
124 
125 RBDL_DLLAPI std::string GetModelHierarchy (const Model &model) {
126  stringstream result ("");
127 
128  result << print_hierarchy (model);
129 
130  return result.str();
131 }
132 
133 RBDL_DLLAPI std::string GetNamedBodyOriginsOverview (Model &model) {
134  stringstream result ("");
135 
136  VectorNd Q (VectorNd::Zero(model.dof_count));
137  UpdateKinematicsCustom (model, &Q, NULL, NULL);
138 
139  for (unsigned int body_id = 0; body_id < model.mBodies.size(); body_id++) {
140  std::string body_name = model.GetBodyName (body_id);
141 
142  if (body_name.size() == 0)
143  continue;
144 
145  Vector3d position = CalcBodyToBaseCoordinates (model, Q, body_id, Vector3d (0., 0., 0.), false);
146 
147  result << body_name << ": " << position.transpose() << endl;
148  }
149 
150  return result.str();
151 }
152 
153 RBDL_DLLAPI void CalcCenterOfMass (
154  Model &model,
155  const Math::VectorNd &q,
156  const Math::VectorNd &qdot,
157  const Math::VectorNd *qddot,
158  double &mass,
159  Math::Vector3d &com,
160  Math::Vector3d *com_velocity,
161  Math::Vector3d *com_acceleration,
162  Math::Vector3d *angular_momentum,
163  Math::Vector3d *change_of_angular_momentum,
164  bool update_kinematics) {
165  // If we want to compute com_acceleration or change of angular momentum
166  // we must have qddot provided.
167  assert( (com_acceleration == NULL && change_of_angular_momentum == NULL)
168  || (qddot != NULL) );
169 
170  if (update_kinematics)
171  UpdateKinematicsCustom (model, &q, &qdot, qddot);
172 
173  for (size_t i = 1; i < model.mBodies.size(); i++) {
174  model.Ic[i] = model.I[i];
175  model.hc[i] = model.Ic[i].toMatrix() * model.v[i];
176  model.hdotc[i] = model.Ic[i] * model.a[i] + crossf(model.v[i], model.Ic[i] * model.v[i]);
177  }
178 
179  if (qddot && (com_acceleration || change_of_angular_momentum)) {
180  for (size_t i = 1; i < model.mBodies.size(); i++) {
181  model.hdotc[i] = model.Ic[i] * model.a[i] + crossf(model.v[i], model.Ic[i] * model.v[i]);
182  }
183  }
184 
185  SpatialRigidBodyInertia Itot (0., Vector3d (0., 0., 0.), Matrix3d::Zero(3,3));
186  SpatialVector htot (SpatialVector::Zero(6));
187  SpatialVector hdot_tot (SpatialVector::Zero(6));
188 
189  for (size_t i = model.mBodies.size() - 1; i > 0; i--) {
190  unsigned int lambda = model.lambda[i];
191 
192  if (lambda != 0) {
193  model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].applyTranspose (model.Ic[i]);
194  model.hc[lambda] = model.hc[lambda] + model.X_lambda[i].applyTranspose (model.hc[i]);
195  } else {
196  Itot = Itot + model.X_lambda[i].applyTranspose (model.Ic[i]);
197  htot = htot + model.X_lambda[i].applyTranspose (model.hc[i]);
198  }
199  }
200 
201  if (qddot && (com_acceleration || change_of_angular_momentum)) {
202  for (size_t i = model.mBodies.size() - 1; i > 0; i--) {
203  unsigned int lambda = model.lambda[i];
204 
205  if (lambda != 0) {
206  model.hdotc[lambda] = model.hdotc[lambda] + model.X_lambda[i].applyTranspose (model.hdotc[i]);
207  } else {
208  hdot_tot = hdot_tot + model.X_lambda[i].applyTranspose (model.hdotc[i]);
209  }
210  }
211  }
212 
213  mass = Itot.m;
214  com = Itot.h / mass;
215  LOG << "mass = " << mass << " com = " << com.transpose() << " htot = " << htot.transpose() << std::endl;
216 
217  if (com_velocity) {
218  *com_velocity = Vector3d (htot[3] / mass, htot[4] / mass, htot[5] / mass);
219  }
220 
221  if (angular_momentum) {
222  htot = Xtrans (com).applyAdjoint (htot);
223  angular_momentum->set (htot[0], htot[1], htot[2]);
224  }
225 
226  if (com_acceleration) {
227  *com_acceleration = Vector3d (hdot_tot[3] / mass, hdot_tot[4] / mass, hdot_tot[5] / mass);
228  }
229 
230  if (change_of_angular_momentum) {
231  hdot_tot = Xtrans (com).applyAdjoint (hdot_tot);
232  change_of_angular_momentum->set (hdot_tot[0], hdot_tot[1], hdot_tot[2]);
233  }
234 }
235 
236 RBDL_DLLAPI void CalcZeroMomentPoint (
237  Model &model,
238  const Math::VectorNd &q,
239  const Math::VectorNd &qdot,
240  const Math::VectorNd &qddot,
241  Vector3d* zmp,
242  const Math::Vector3d &normal,
243  const Math::Vector3d &point,
244  bool update_kinematics
245 ) {
246  if (zmp == NULL) {
247  cerr << "ZMP (output) is 'NULL'!" << endl;
248  abort();
249  }
250 
251  // update kinematics if required
252  // NOTE UpdateKinematics computes model.a[i] and model.v[i] required for
253  // change of momentum
254  if (update_kinematics) {
255  UpdateKinematicsCustom (model, &q, &qdot, &qddot);
256  }
257 
258  // compute change of momentum of each single body (same as in RNEA/InverseDynamics)
259  for (size_t i = 1; i < model.mBodies.size(); i++) {
260  model.Ic[i] = model.I[i];
261  model.hdotc[i] = model.Ic[i] * model.a[i] + crossf(model.v[i], model.Ic[i] * model.v[i]);
262  }
263 
264  SpatialRigidBodyInertia I_tot (0., Vector3d (0., 0., 0.), Matrix3d::Zero(3,3));
265  SpatialVector h_tot (SpatialVector::Zero(6));
266  SpatialVector hdot_tot (SpatialVector::Zero(6));
267 
268  // compute total change of momentum and CoM wrt to root body (idx = 0)
269  // by recursively summing up local change of momentum
270  for (size_t i = model.mBodies.size() - 1; i > 0; i--) {
271  unsigned int lambda = model.lambda[i];
272 
273  if (lambda != 0) {
274  model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].applyTranspose (model.Ic[i]);
275  model.hc[lambda] = model.hc[lambda] + model.X_lambda[i].applyTranspose (model.hc[i]);
276  model.hdotc[lambda] = model.hdotc[lambda] + model.X_lambda[i].applyTranspose (model.hdotc[i]);
277  } else {
278  I_tot = I_tot + model.X_lambda[i].applyTranspose (model.Ic[i]);
279  h_tot = h_tot + model.X_lambda[i].applyTranspose (model.hc[i]);
280  hdot_tot = hdot_tot + model.X_lambda[i].applyTranspose (model.hdotc[i]);
281  }
282  }
283 
284  // compute CoM from mass and total inertia
285  const double mass = I_tot.m;
286  const Vector3d com = I_tot.h / mass;
287 
288  // project angular momentum onto CoM
289  SpatialTransform Xcom = Xtrans (com);
290  hdot_tot = Xcom.applyAdjoint (hdot_tot);
291 
292  // compute net external force at CoM by removing effects due to gravity
293  hdot_tot = hdot_tot - mass * SpatialVector (0., 0., 0., model.gravity[0], model.gravity[1], model.gravity[2]);
294 
295  // express total change of momentum in world coordinates
296  hdot_tot = Xcom.inverse().applyAdjoint (hdot_tot);
297 
298  // project purified change of momentum onto surface
299  // z = n x n_0
300  // -------
301  // n * f
302  Vector3d n_0 = hdot_tot.block<3,1>(0,0);
303  Vector3d f = hdot_tot.block<3,1>(3,0);
304  *zmp = normal.cross(n_0) / normal.dot(f);
305 
306  // double distance = (hdot_tot - point).dot(normal);
307  // zmp = hdot_tot - distance * normal;
308 
309  return;
310 }
311 
312 RBDL_DLLAPI double CalcPotentialEnergy (
313  Model &model,
314  const Math::VectorNd &q,
315  bool update_kinematics) {
316  double mass;
317  Vector3d com;
319  model,
320  q,
321  VectorNd::Zero (model.qdot_size),
322  NULL,
323  mass,
324  com,
325  NULL,
326  NULL,
327  NULL,
328  NULL,
329  update_kinematics);
330 
331  Vector3d g = - Vector3d (model.gravity[0], model.gravity[1], model.gravity[2]);
332  LOG << "pot_energy: " << " mass = " << mass << " com = " << com.transpose() << std::endl;
333 
334  return mass * com.dot(g);
335 }
336 
337 RBDL_DLLAPI double CalcKineticEnergy (
338  Model &model,
339  const Math::VectorNd &q,
340  const Math::VectorNd &qdot,
341  bool update_kinematics) {
342  if (update_kinematics)
343  UpdateKinematicsCustom (model, &q, &qdot, NULL);
344 
345  double result = 0.;
346 
347  for (size_t i = 1; i < model.mBodies.size(); i++) {
348  result += 0.5 * model.v[i].transpose() * (model.I[i] * model.v[i]);
349  }
350  return result;
351 }
352 
353 }
354 }
std::vector< Math::SpatialVector > hc
Definition: Model.h:225
SpatialMatrix crossf(const SpatialVector &v)
Compact representation of spatial transformations.
string get_dof_name(const SpatialVector &joint_dof)
Definition: rbdl_utils.cc:24
std::vector< unsigned int > lambda
The id of the parents body.
Definition: Model.h:127
RBDL_DLLAPI void UpdateKinematicsCustom(Model &model, const VectorNd *Q, const VectorNd *QDot, const VectorNd *QDDot)
Selectively updates model internal states of body positions, velocities and/or accelerations.
Definition: Kinematics.cc:78
Compact representation for Spatial Inertia.
SpatialVector applyAdjoint(const SpatialVector &f_sp)
RBDL_DLLAPI double CalcKineticEnergy(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics)
Computes the kinetic energy of the full model.
Definition: rbdl_utils.cc:337
RBDL_DLLAPI std::string GetModelHierarchy(const Model &model)
Creates a human readable overview of the model.
Definition: rbdl_utils.cc:125
std::string GetBodyName(unsigned int body_id) const
Returns the name of a body for a given body id.
Definition: Model.h:355
RBDL_DLLAPI void CalcCenterOfMass(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, const Math::VectorNd *qddot, double &mass, Math::Vector3d &com, Math::Vector3d *com_velocity, Math::Vector3d *com_acceleration, Math::Vector3d *angular_momentum, Math::Vector3d *change_of_angular_momentum, bool update_kinematics)
Computes the Center of Mass (COM) and optionally its linear velocity.
Definition: rbdl_utils.cc:153
RBDL_DLLAPI double CalcPotentialEnergy(Model &model, const Math::VectorNd &q, bool update_kinematics)
Computes the potential energy of the full model.
Definition: rbdl_utils.cc:312
Contains all information about the rigid body model.
Definition: Model.h:121
SpatialVector_t SpatialVector
Definition: rbdl_math.h:59
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
RBDL_DLLAPI Vector3d CalcBodyToBaseCoordinates(Model &model, const VectorNd &Q, unsigned int body_id, const Vector3d &point_body_coordinates, bool update_kinematics)
Returns the base coordinates of a point given in body coordinates.
Definition: Kinematics.cc:159
STL namespace.
std::vector< Math::SpatialRigidBodyInertia > Ic
Definition: Model.h:224
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
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
SpatialTransform Xtrans(const Vector3d &r)
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
Definition: Model.h:241
RBDL_DLLAPI std::string GetModelDOFOverview(const Model &model)
Creates a human readable overview of the Degrees of Freedom.
Definition: rbdl_utils.cc:55
unsigned int qdot_size
The size of the.
Definition: Model.h:156
RBDL_DLLAPI std::string GetNamedBodyOriginsOverview(Model &model)
Creates a human readable overview of the locations of all bodies that have names. ...
Definition: rbdl_utils.cc:133
std::vector< std::vector< unsigned int > > mu
Contains the ids of all the children of a given body.
Definition: Model.h:132
#define LOG
Definition: Logging.h:23
std::vector< Math::SpatialVector > hdotc
Definition: Model.h:226
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::string print_hierarchy(const RigidBodyDynamics::Model &model, unsigned int body_index=0, int indent=0)
Definition: rbdl_utils.cc:74
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
RBDL_DLLAPI void CalcZeroMomentPoint(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, const Math::VectorNd &qddot, Vector3d *zmp, const Math::Vector3d &normal, const Math::Vector3d &point, bool update_kinematics)
Computes the Zero-Moment-Point (ZMP) on a given contact surface.
Definition: rbdl_utils.cc:236
string get_body_name(const RigidBodyDynamics::Model &model, unsigned int body_id)
Definition: rbdl_utils.cc:43
std::vector< Joint > mJoints
All joints.
Definition: Model.h:175
void set(const double &v0, const double &v1, const double &v2)
Vector3d h
Coordinates of the center of mass.