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-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
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  double &mass,
158  Math::Vector3d &com,
159  Math::Vector3d *com_velocity,
160  Vector3d *angular_momentum,
161  bool update_kinematics) {
162  if (update_kinematics)
163  UpdateKinematicsCustom (model, &q, &qdot, NULL);
164 
165  for (size_t i = 1; i < model.mBodies.size(); i++) {
166  model.Ic[i] = model.I[i];
167  model.hc[i] = model.Ic[i].toMatrix() * model.v[i];
168  }
169 
170  SpatialRigidBodyInertia Itot (0., Vector3d (0., 0., 0.), Matrix3d::Zero(3,3));
171  SpatialVector htot (SpatialVector::Zero(6));
172 
173  for (size_t i = model.mBodies.size() - 1; i > 0; i--) {
174  unsigned int lambda = model.lambda[i];
175 
176  if (lambda != 0) {
177  model.Ic[lambda] = model.Ic[lambda] + model.X_lambda[i].applyTranspose (model.Ic[i]);
178  model.hc[lambda] = model.hc[lambda] + model.X_lambda[i].applyTranspose (model.hc[i]);
179  } else {
180  Itot = Itot + model.X_lambda[i].applyTranspose (model.Ic[i]);
181  htot = htot + model.X_lambda[i].applyTranspose (model.hc[i]);
182  }
183  }
184 
185  mass = Itot.m;
186  com = Itot.h / mass;
187  LOG << "mass = " << mass << " com = " << com.transpose() << " htot = " << htot.transpose() << std::endl;
188 
189  if (com_velocity)
190  *com_velocity = Vector3d (htot[3] / mass, htot[4] / mass, htot[5] / mass);
191 
192  if (angular_momentum) {
193  htot = Xtrans (com).applyAdjoint (htot);
194  angular_momentum->set (htot[0], htot[1], htot[2]);
195  }
196 }
197 
198 RBDL_DLLAPI double CalcPotentialEnergy (
199  Model &model,
200  const Math::VectorNd &q,
201  bool update_kinematics) {
202  double mass;
203  Vector3d com;
204  CalcCenterOfMass (model, q, VectorNd::Zero (model.qdot_size), mass, com, NULL, NULL, update_kinematics);
205 
206  Vector3d g = - Vector3d (model.gravity[0], model.gravity[1], model.gravity[2]);
207  LOG << "pot_energy: " << " mass = " << mass << " com = " << com.transpose() << std::endl;
208 
209  return mass * com.dot(g);
210 }
211 
212 RBDL_DLLAPI double CalcKineticEnergy (
213  Model &model,
214  const Math::VectorNd &q,
215  const Math::VectorNd &qdot,
216  bool update_kinematics) {
217  if (update_kinematics)
218  UpdateKinematicsCustom (model, &q, &qdot, NULL);
219 
220  double result = 0.;
221 
222  for (size_t i = 1; i < model.mBodies.size(); i++) {
223  result += 0.5 * model.v[i].transpose() * (model.I[i] * model.v[i]);
224  }
225  return result;
226 }
227 
228 }
229 }
std::vector< Math::SpatialVector > hc
Definition: Model.h:225
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:88
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:212
std::string GetBodyName(unsigned int body_id) const
Returns the name of a body for a given body id.
Definition: Model.h:380
RBDL_DLLAPI std::string GetModelHierarchy(const Model &model)
Creates a human readable overview of the model.
Definition: rbdl_utils.cc:125
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:198
Contains all information about the rigid body model.
Definition: Model.h:121
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
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:168
STL namespace.
std::vector< Math::SpatialRigidBodyInertia > Ic
Definition: Model.h:224
RBDL_DLLAPI void CalcCenterOfMass(Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, double &mass, Math::Vector3d &com, Math::Vector3d *com_velocity, Vector3d *angular_momentum, bool update_kinematics)
Computes the Center of Mass (COM) and optionally its linear velocity.
Definition: rbdl_utils.cc:153
Namespace for all structures of the RigidBodyDynamics library.
Definition: Contacts.cc:22
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
SpatialTransform Xtrans(const Vector3d &r)
std::vector< FixedBody > mFixedBodies
All bodies that are attached to a body via a fixed joint.
Definition: Model.h:240
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:24
unsigned int fixed_body_discriminator
Value that is used to discriminate between fixed and movable bodies.
Definition: Model.h:253
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
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.