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#include "rbdl/rbdl_errors.h"
23
24// std::vectors containing any objects that have Eigen matrices or vectors
25// as members need to have a special allocater. This can be achieved with
26// the following macro.
27
28#ifndef RBDL_USE_CASADI_MATH
29EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Joint)
30EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::Body)
31EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(RigidBodyDynamics::FixedBody)
32#endif
33
36namespace RigidBodyDynamics
37{
38
123struct RBDL_DLLAPI Model {
124 Model();
125
126 // Structural information
127
129 std::vector<unsigned int> lambda;
132 std::vector<unsigned int> lambda_q;
134 std::vector<std::vector<unsigned int> >mu;
135
141 unsigned int dof_count;
142
150 unsigned int q_size;
158 unsigned int qdot_size;
159
161 unsigned int previously_added_body_id;
162
164 Math::Vector3d gravity;
165
166 // State information
168 std::vector<Math::SpatialVector> v;
170 std::vector<Math::SpatialVector> a;
171
173 // Joints
174
176
177 std::vector<Joint> mJoints;
179 std::vector<Math::SpatialVector> S;
180
181 // Joint state variables
182 std::vector<Math::SpatialVector> v_J;
183 std::vector<Math::SpatialVector> c_J;
184
185 std::vector<unsigned int> mJointUpdateOrder;
186
188 // It is expressed in the coordinate frame of the parent.
189 std::vector<Math::SpatialTransform> X_T;
192 std::vector<unsigned int> mFixedJointCount;
193
195 // Special variables for joints with 3 degrees of freedom
197 std::vector<Math::Matrix63> multdof3_S;
198 std::vector<Math::Matrix63> multdof3_U;
199 std::vector<Math::Matrix3d> multdof3_Dinv;
200 std::vector<Math::Vector3d> multdof3_u;
201 std::vector<unsigned int> multdof3_w_index;
202
203 std::vector<CustomJoint*> mCustomJoints;
204
206 // Dynamics variables
207
209 std::vector<Math::SpatialVector> c;
211 std::vector<Math::SpatialMatrix> IA;
213 std::vector<Math::SpatialVector> pA;
215 std::vector<Math::SpatialVector> U;
217 Math::VectorNd d;
219 Math::VectorNd u;
221 std::vector<Math::SpatialVector> f;
224 std::vector<Math::SpatialRigidBodyInertia> I;
225 std::vector<Math::SpatialRigidBodyInertia> Ic;
226 std::vector<Math::SpatialVector> hc;
227 std::vector<Math::SpatialVector> hdotc;
228
230 // Bodies
231
237 std::vector<Math::SpatialTransform> X_lambda;
239 std::vector<Math::SpatialTransform> X_base;
240
242 std::vector<FixedBody> mFixedBodies;
255 unsigned int fixed_body_discriminator;
256
264 std::vector<Body> mBodies;
265
267 std::map<std::string, unsigned int> mBodyNameMap;
268
299 unsigned int AddBody (
300 const unsigned int parent_id,
301 const Math::SpatialTransform &joint_frame,
302 const Joint &joint,
303 const Body &body,
304 std::string body_name = ""
305 );
306
307 unsigned int AddBodySphericalJoint (
308 const unsigned int parent_id,
309 const Math::SpatialTransform &joint_frame,
310 const Joint &joint,
311 const Body &body,
312 std::string body_name = ""
313 );
314
321 unsigned int AppendBody (
322 const Math::SpatialTransform &joint_frame,
323 const Joint &joint,
324 const Body &body,
325 std::string body_name = ""
326 );
327
328 unsigned int AddBodyCustomJoint (
329 const unsigned int parent_id,
330 const Math::SpatialTransform &joint_frame,
331 CustomJoint *custom_joint,
332 const Body &body,
333 std::string body_name = ""
334 );
335
347 unsigned int GetBodyId (const char *body_name) const
348 {
349 if (mBodyNameMap.count(body_name) == 0) {
350 return std::numeric_limits<unsigned int>::max();
351 }
352
353 return mBodyNameMap.find(body_name)->second;
354 }
355
357 std::string GetBodyName (unsigned int body_id) const
358 {
359 std::map<std::string, unsigned int>::const_iterator iter
360 = mBodyNameMap.begin();
361
362 while (iter != mBodyNameMap.end()) {
363 if (iter->second == body_id) {
364 return iter->first;
365 }
366
367 iter++;
368 }
369
370 return "";
371 }
372
375 bool IsFixedBodyId (unsigned int body_id)
376 {
377 if (body_id >= fixed_body_discriminator
378 && body_id < std::numeric_limits<unsigned int>::max()
379 && body_id - fixed_body_discriminator < mFixedBodies.size()) {
380 return true;
381 }
382 return false;
383 }
384
385 bool IsBodyId (unsigned int id)
386 {
387 if (id > 0 && id < mBodies.size()) {
388 return true;
389 }
390 if (id >= fixed_body_discriminator
391 && id < std::numeric_limits<unsigned int>::max()) {
392 if (id - fixed_body_discriminator < mFixedBodies.size()) {
393 return true;
394 }
395 }
396 return false;
397 }
398
406 unsigned int GetParentBodyId (unsigned int id)
407 {
408 if (id >= fixed_body_discriminator) {
409 return mFixedBodies[id - fixed_body_discriminator].mMovableParent;
410 }
411
412 unsigned int parent_id = lambda[id];
413
414 while (mBodies[parent_id].mIsVirtual) {
415 parent_id = lambda[parent_id];
416 }
417
418 return parent_id;
419 }
420
424 Math::SpatialTransform GetJointFrame (unsigned int id)
425 {
426 if (id >= fixed_body_discriminator) {
427 return mFixedBodies[id - fixed_body_discriminator].mParentTransform;
428 }
429
430 unsigned int child_id = id;
431 unsigned int parent_id = lambda[id];
432 if (mBodies[parent_id].mIsVirtual) {
433 while (mBodies[parent_id].mIsVirtual) {
434 child_id = parent_id;
435 parent_id = lambda[child_id];
436 }
437 return X_T[child_id];
438 } else {
439 return X_T[id];
440 }
441 }
442
446 void SetJointFrame (unsigned int id,
447 const Math::SpatialTransform &transform)
448 {
449 if (id >= fixed_body_discriminator) {
450 throw Errors::RBDLError("Error: setting of parent transform not supported for fixed bodies!");
451 }
452
453 unsigned int child_id = id;
454 unsigned int parent_id = lambda[id];
455 if (mBodies[parent_id].mIsVirtual) {
456 while (mBodies[parent_id].mIsVirtual) {
457 child_id = parent_id;
458 parent_id = lambda[child_id];
459 }
460 X_T[child_id] = transform;
461 } else if (id > 0) {
462 X_T[id] = transform;
463 }
464 }
465
471 Math::Quaternion GetQuaternion (unsigned int i,
472 const Math::VectorNd &Q) const
473 {
474 assert (mJoints[i].mJointType == JointTypeSpherical);
475 unsigned int q_index = mJoints[i].q_index;
476 return Math::Quaternion ( Q[q_index],
477 Q[q_index + 1],
478 Q[q_index + 2],
479 Q[multdof3_w_index[i]]);
480 }
481
487 void SetQuaternion (unsigned int i,
488 const Math::Quaternion &quat,
489 Math::VectorNd &Q) const
490 {
491 assert (mJoints[i].mJointType == JointTypeSpherical);
492 unsigned int q_index = mJoints[i].q_index;
493
494 Q[q_index] = quat[0];
495 Q[q_index + 1] = quat[1];
496 Q[q_index + 2] = quat[2];
497 Q[multdof3_w_index[i]] = quat[3];
498 }
499};
500
502}
503
504/* _MODEL_H */
505#endif
Body.h
RigidBodyDynamics::Math::VectorNd
VectorN_t VectorNd
Definition: rbdl_math.h:66
RigidBodyDynamics::Math::Vector3d
Vector3_t Vector3d
Definition: rbdl_math.h:57
RigidBodyDynamics
Definition: Joint.cc:45
rbdl_errors.h
rbdl_math.h
RigidBodyDynamics::Body
Describes all properties of a single body.
Definition: Body.h:28
RigidBodyDynamics::FixedBody
Keeps the information of a body and how it is attached to another body.
Definition: Body.h:208