Rigid Body Dynamics Library
Body.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_BODY_H
9 #define RBDL_BODY_H
10 
11 #include "rbdl/rbdl_math.h"
12 #include "rbdl/rbdl_mathutils.h"
13 #include <assert.h>
14 #include <iostream>
15 #include "rbdl/Logging.h"
16 
17 namespace RigidBodyDynamics {
18 
26 struct RBDL_DLLAPI Body {
27  Body() :
28  mMass (0.),
29  mCenterOfMass (0., 0., 0.),
30  mInertia (Math::Matrix3d::Zero(3,3)),
31  mIsVirtual (false)
32  { };
33  Body(const Body &body) :
34  mMass (body.mMass),
35  mCenterOfMass (body.mCenterOfMass),
36  mInertia (body.mInertia),
37  mIsVirtual (body.mIsVirtual)
38  {};
39  Body& operator= (const Body &body) {
40  if (this != &body) {
41  mMass = body.mMass;
42  mInertia = body.mInertia;
43  mCenterOfMass = body.mCenterOfMass;
44  mIsVirtual = body.mIsVirtual;
45  }
46 
47  return *this;
48  }
49 
61  Body(const double &mass,
62  const Math::Vector3d &com,
63  const Math::Vector3d &gyration_radii) :
64  mMass (mass),
65  mCenterOfMass(com),
66  mIsVirtual (false) {
67  mInertia = Math::Matrix3d (
68  gyration_radii[0], 0., 0.,
69  0., gyration_radii[1], 0.,
70  0., 0., gyration_radii[2]
71  );
72  }
73 
85  Body(const double &mass,
86  const Math::Vector3d &com,
87  const Math::Matrix3d &inertia_C) :
88  mMass (mass),
89  mCenterOfMass(com),
90  mInertia (inertia_C),
91  mIsVirtual (false) { }
92 
107  void Join (const Math::SpatialTransform &transform, const Body &other_body) {
108  // nothing to do if we join a massles body to the current.
109  if (other_body.mMass == 0. && other_body.mInertia == Math::Matrix3d::Zero()) {
110  return;
111  }
112 
113  double other_mass = other_body.mMass;
114  double new_mass = mMass + other_mass;
115 
116  if (new_mass == 0.) {
117  std::cerr << "Error: cannot join bodies as both have zero mass!" << std::endl;
118  assert (false);
119  abort();
120  }
121 
122  Math::Vector3d other_com = transform.E.transpose() * other_body.mCenterOfMass + transform.r;
123  Math::Vector3d new_com = (1 / new_mass ) * (mMass * mCenterOfMass + other_mass * other_com);
124 
125  LOG << "other_com = " << std::endl << other_com.transpose() << std::endl;
126  LOG << "rotation = " << std::endl << transform.E << std::endl;
127 
128  // We have to transform the inertia of other_body to the new COM. This
129  // is done in 4 steps:
130  //
131  // 1. Transform the inertia from other origin to other COM
132  // 2. Rotate the inertia that it is aligned to the frame of this body
133  // 3. Transform inertia of other_body to the origin of the frame of
134  // this body
135  // 4. Sum the two inertias
136  // 5. Transform the summed inertia to the new COM
137 
140 
141  Math::Matrix3d inertia_other = other_rbi.toMatrix().block<3,3>(0,0);
142  LOG << "inertia_other = " << std::endl << inertia_other << std::endl;
143 
144  // 1. Transform the inertia from other origin to other COM
145  Math::Matrix3d other_com_cross = Math::VectorCrossMatrix(other_body.mCenterOfMass);
146  Math::Matrix3d inertia_other_com = inertia_other - other_mass * other_com_cross * other_com_cross.transpose();
147  LOG << "inertia_other_com = " << std::endl << inertia_other_com << std::endl;
148 
149  // 2. Rotate the inertia that it is aligned to the frame of this body
150  Math::Matrix3d inertia_other_com_rotated = transform.E.transpose() * inertia_other_com * transform.E;
151  LOG << "inertia_other_com_rotated = " << std::endl << inertia_other_com_rotated << std::endl;
152 
153  // 3. Transform inertia of other_body to the origin of the frame of this body
154  Math::Matrix3d inertia_other_com_rotated_this_origin = Math::parallel_axis (inertia_other_com_rotated, other_mass, other_com);
155  LOG << "inertia_other_com_rotated_this_origin = " << std::endl << inertia_other_com_rotated_this_origin << std::endl;
156 
157  // 4. Sum the two inertias
158  Math::Matrix3d inertia_summed = Math::Matrix3d (this_rbi.toMatrix().block<3,3>(0,0)) + inertia_other_com_rotated_this_origin;
159  LOG << "inertia_summed = " << std::endl << inertia_summed << std::endl;
160 
161  // 5. Transform the summed inertia to the new COM
162  Math::Matrix3d new_inertia = inertia_summed - new_mass * Math::VectorCrossMatrix (new_com) * Math::VectorCrossMatrix(new_com).transpose();
163 
164  LOG << "new_mass = " << new_mass << std::endl;
165  LOG << "new_com = " << new_com.transpose() << std::endl;
166  LOG << "new_inertia = " << std::endl << new_inertia << std::endl;
167 
168  *this = Body (new_mass, new_com, new_inertia);
169  }
170 
171  ~Body() {};
172 
174  double mMass;
179 
181 };
182 
189 struct RBDL_DLLAPI FixedBody {
191  double mMass;
196 
198  unsigned int mMovableParent;
200  // fixed body.
203 
204  static FixedBody CreateFromBody (const Body& body) {
205  FixedBody fbody;
206 
207  fbody.mMass = body.mMass;
208  fbody.mCenterOfMass = body.mCenterOfMass;
209  fbody.mInertia = body.mInertia;
210 
211  return fbody;
212  }
213 };
214 
215 }
216 
217 /* RBDL_BODY_H */
218 #endif
Compact representation of spatial transformations.
Compact representation for Spatial Inertia.
static SpatialRigidBodyInertia createFromMassComInertiaC(double mass, const Vector3d &com, const Matrix3d &inertia_C)
Math::Vector3d mCenterOfMass
The position of the center of mass in body coordinates.
Definition: Body.h:193
Body(const double &mass, const Math::Vector3d &com, const Math::Vector3d &gyration_radii)
Constructs a body from mass, center of mass and radii of gyration.
Definition: Body.h:61
Matrix3d VectorCrossMatrix(const Vector3d &vector)
Math::SpatialTransform mBaseTransform
Definition: Body.h:202
RBDL_DLLAPI Matrix3d parallel_axis(const Matrix3d &inertia, double mass, const Vector3d &com)
Translates the inertia matrix to a new center.
Namespace for all structures of the RigidBodyDynamics library.
Definition: Constraints.cc:23
unsigned int mMovableParent
Id of the movable body that this fixed body is attached to.
Definition: Body.h:198
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
Body(const Body &body)
Definition: Body.h:33
Math::Matrix3d mInertia
Inertia matrix at the center of mass.
Definition: Body.h:178
Math::Vector3d mCenterOfMass
The position of the center of mass in body coordinates.
Definition: Body.h:176
#define LOG
Definition: Logging.h:23
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
Math::Matrix3d mInertia
The spatial inertia that contains both mass and inertia information.
Definition: Body.h:195
double mMass
The mass of the body.
Definition: Body.h:171
static FixedBody CreateFromBody(const Body &body)
Definition: Body.h:204
double mMass
The mass of the body.
Definition: Body.h:191
Body(const double &mass, const Math::Vector3d &com, const Math::Matrix3d &inertia_C)
Constructs a body from mass, center of mass, and a 3x3 inertia matrix.
Definition: Body.h:85
Describes all properties of a single body.
Definition: Body.h:26