Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3 : : // RDL - Robot Dynamics Library
4 : : // Licensed under the zlib license. See LICENSE for more details.
5 : :
6 : : #ifndef RDL_BODY_HPP
7 : : #define RDL_BODY_HPP
8 : :
9 : : #include <rdl_dynamics/RigidBodyInertia.hpp>
10 : : #include <rdl_dynamics/rdl_mathutils.hpp>
11 : : #include <assert.h>
12 : : #include <iostream>
13 : : #include <rdl_dynamics/RdlExceptions.hpp>
14 : :
15 : : namespace RobotDynamics
16 : : {
17 : : /**
18 : : * @struct A compact struct for storing the drag data for a body
19 : : */
20 : : struct DragData
21 : : {
22 : 2 : DragData(const std::vector<double>& linear_drag, const std::vector<double>& quadratic_drag)
23 : 2 : {
24 : 2 : assert(linear_drag.size() == 6 && quadratic_drag.size() == 6);
25 : 14 : for (unsigned int i = 0; i < 6; i++)
26 : : {
27 : 12 : this->linearDrag[i] = linear_drag[i];
28 : 12 : this->quadraticDrag[i] = quadratic_drag[i];
29 : : }
30 : 2 : }
31 : :
32 : 45 : DragData(const Math::SpatialVector& linear_drag, const Math::SpatialVector& quadratic_drag)
33 : 45 : {
34 : 45 : this->linearDrag = linear_drag;
35 : 45 : this->quadraticDrag = quadratic_drag;
36 : 45 : }
37 : :
38 : 3994 : DragData()
39 : 3994 : {
40 : 3994 : zero();
41 : 3994 : }
42 : :
43 : 15365 : DragData(const DragData& dragData)
44 : 15365 : {
45 : 15365 : this->linearDrag = dragData.linearDrag;
46 : 15365 : this->quadraticDrag = dragData.quadraticDrag;
47 : 15365 : }
48 : :
49 : 7727 : void zero()
50 : : {
51 : 7727 : linearDrag.setZero();
52 : 7727 : quadraticDrag.setZero();
53 : 7727 : }
54 : :
55 : 1383 : DragData& operator=(const DragData& dragData)
56 : : {
57 : 1383 : this->linearDrag = dragData.linearDrag;
58 : 1383 : this->quadraticDrag = dragData.quadraticDrag;
59 : 1383 : return *this;
60 : : }
61 : :
62 : 399 : void operator+=(const DragData& dragData)
63 : : {
64 : 399 : linearDrag += dragData.linearDrag;
65 : 399 : quadraticDrag += dragData.quadraticDrag;
66 : 399 : }
67 : :
68 : : Math::SpatialVector linearDrag; /**< 6 coefficients for the linear portion of the drag effect */
69 : : Math::SpatialVector quadraticDrag; /**< 6 coefficients for the quadratic portion of the drag effect */
70 : : };
71 : :
72 : 399 : inline DragData operator+(DragData dragData1, const DragData& dragData2)
73 : : {
74 : 399 : dragData1 += dragData2;
75 : 399 : return dragData1;
76 : : }
77 : :
78 : : /** \brief Describes all properties of a single body
79 : : *
80 : : * A Body contains information about mass, the location of its center of
81 : : * mass, the ineria tensor in the center of mass as well as information
82 : : * for hydrodynamics such as center of buoyancy, volume, and shape parameters
83 : : * for modeling fluid added mass/drag effects. This class is
84 : : * designed to use the given information and transform it such that it can
85 : : * directly be used by the spatial algebra.
86 : : */
87 : : struct Body
88 : : {
89 : 1244 : Body()
90 : 1244 : : mMass(0.)
91 : 1244 : , mCenterOfMass(0., 0., 0.)
92 : 1244 : , mInertia(Math::Matrix3d::Zero(3, 3))
93 : 1244 : , volume(0.)
94 : 1244 : , mCenterOfBuoyancy(0., 0., 0.)
95 : 1244 : , addedMassMatrix(Math::SpatialMatrixZero)
96 : 1244 : , mIsVirtual(false)
97 : : {
98 : 1244 : dragData.zero();
99 : 1244 : };
100 : :
101 : 14728 : Body(const Body& body)
102 : 14728 : : mMass(body.mMass)
103 : 14728 : , mCenterOfMass(body.mCenterOfMass)
104 : 14728 : , mInertia(body.mInertia)
105 : 14728 : , volume(body.volume)
106 : 14728 : , mCenterOfBuoyancy(body.mCenterOfBuoyancy)
107 : 14728 : , addedMassMatrix(body.addedMassMatrix)
108 : 14728 : , dragData(body.dragData)
109 : 14728 : , mIsVirtual(body.mIsVirtual){};
110 : :
111 : 1151 : Body& operator=(const Body& body)
112 : : {
113 : 1151 : if (this != &body)
114 : : {
115 : 1151 : mMass = body.mMass;
116 : 1151 : mInertia = body.mInertia;
117 : 1151 : mCenterOfMass = body.mCenterOfMass;
118 : 1151 : mIsVirtual = body.mIsVirtual;
119 : 1151 : mCenterOfBuoyancy = body.mCenterOfBuoyancy;
120 : 1151 : volume = body.volume;
121 : 1151 : addedMassMatrix = body.addedMassMatrix;
122 : 1151 : dragData = body.dragData;
123 : : }
124 : :
125 : 1151 : return *this;
126 : : }
127 : :
128 : : /** \brief Constructs a body from mass, center of mass, radii of gyration, the center of buoyancy,
129 : : * added mass matrix, drag data, and the volume taken up by the body
130 : : *
131 : : * This constructor eases the construction of a new body as all the
132 : : * required parameters can be specified as parameters to the
133 : : * constructor. These are then used to generate the spatial inertia
134 : : * matrix which is expressed at the origin.
135 : : *
136 : : * \param mass the mass of the body
137 : : * \param com the position of the center of mass in the bodies coordinates
138 : : * \param gyration_radii the radii of gyration at the center of mass of the body
139 : : * \param centerOfBuoyancy The center of buoyancy for the body
140 : : * \param volume The volume taken up by the body
141 : : * \param addedMassMatrix The hydrodynamic added mass matrix
142 : : * \param dragData Hydrodynamic drag data struct for the body
143 : : */
144 : 53 : Body(const double& mass, const Math::Vector3d& com, const Math::Vector3d& gyration_radii, const Math::Vector3d& centerOfBuoyancy, const double volume,
145 : : const Math::SpatialMatrix& addedMassMatrix, const DragData& dragData)
146 : 53 : : mMass(mass), mCenterOfMass(com), volume(volume), mCenterOfBuoyancy(centerOfBuoyancy), addedMassMatrix(addedMassMatrix), dragData(dragData), mIsVirtual(false)
147 : : {
148 : 53 : mInertia = Math::Matrix3d(gyration_radii[0], 0., 0., 0., gyration_radii[1], 0., 0., 0., gyration_radii[2]);
149 : 53 : }
150 : :
151 : : /** \brief Constructs a body from mass, center of mass, radii of gyration, the center of buoyancy,
152 : : * and hydrodynamic parameters
153 : : *
154 : : * This constructor eases the construction of a new body as all the
155 : : * required parameters can be specified as parameters to the
156 : : * constructor. These are then used to generate the spatial inertia
157 : : * matrix which is expressed at the origin.
158 : : *
159 : : * \param mass the mass of the body
160 : : * \param com the position of the center of mass in the bodies coordinates
161 : : * \param gyration_radii the radii of gyration at the center of mass of the body
162 : : * \param centerOfBuoyancy The center of buoyancy for the body
163 : : * \param volume The volume taken up by the body
164 : : * \param addedMassMatrix The hydrodynamic added mass matrix for the body
165 : : */
166 : 10 : Body(const double& mass, const Math::Vector3d& com, const Math::Vector3d& gyration_radii, const Math::Vector3d& centerOfBuoyancy, const double volume,
167 : : const Math::SpatialMatrix& addedMassMatrix)
168 : 10 : : mMass(mass), mCenterOfMass(com), volume(volume), mCenterOfBuoyancy(centerOfBuoyancy), addedMassMatrix(addedMassMatrix), mIsVirtual(false)
169 : : {
170 : 10 : dragData.zero();
171 : 10 : mInertia = Math::Matrix3d(gyration_radii[0], 0., 0., 0., gyration_radii[1], 0., 0., 0., gyration_radii[2]);
172 : 10 : }
173 : :
174 : : /** \brief Constructs a body from mass, center of mass, radii of gyration. Hydrodynamic terms are set such that
175 : : * they will have no effect on the dynamics
176 : : *
177 : : * This constructor eases the construction of a new body as all the
178 : : * required parameters can be specified as parameters to the
179 : : * constructor. These are then used to generate the spatial inertia
180 : : * matrix which is expressed at the origin.
181 : : *
182 : : * \param mass the mass of the body
183 : : * \param com the position of the center of mass in the bodies coordinates
184 : : * \param gyration_radii the radii of gyration at the center of mass of the body
185 : : */
186 : 1722 : Body(const double& mass, const Math::Vector3d& com, const Math::Vector3d& gyration_radii)
187 : 1722 : : mMass(mass), mCenterOfMass(com), volume(0.), mCenterOfBuoyancy(Math::Vector3dZero), mIsVirtual(false)
188 : : {
189 : 1722 : mInertia = Math::Matrix3d(gyration_radii[0], 0., 0., 0., gyration_radii[1], 0., 0., 0., gyration_radii[2]);
190 : 1722 : dragData.zero();
191 : 1722 : addedMassMatrix.setZero();
192 : 1722 : }
193 : :
194 : : /** \brief Constructs a body from mass, center of mass, a 3x3 inertia matrix, the center of buoyancy,
195 : : * and hydrodynamic parameters
196 : : *
197 : : * This constructor eases the construction of a new body as all the
198 : : * required parameters can simply be specified as parameters to the
199 : : * constructor. These are then used to generate the spatial inertia
200 : : * matrix which is expressed at the origin.
201 : : *
202 : : * \param mass the mass of the body
203 : : * \param com the position of the center of mass in the bodies coordinates
204 : : * \param inertia_C the inertia at the center of mass
205 : : * \param centerOfBuoyancy The center of buoyancy for the body
206 : : * \param volume The volume taken up by the body
207 : : * \param addedMassMatrix Hydrodynamic added mass matrix for the body
208 : : * \param dragData The drag data for this body
209 : : */
210 : 262 : Body(const double& mass, const Math::Vector3d& com, const Math::Matrix3d& inertia_C, const Math::Vector3d& centerOfBuoyancy, const double volume,
211 : : const Math::SpatialMatrix& addedMassMatrix, const DragData& dragData)
212 : 262 : : mMass(mass)
213 : 262 : , mCenterOfMass(com)
214 : 262 : , mInertia(inertia_C)
215 : 262 : , volume(volume)
216 : 262 : , mCenterOfBuoyancy(centerOfBuoyancy)
217 : 262 : , addedMassMatrix(addedMassMatrix)
218 : 262 : , dragData(dragData)
219 : 262 : , mIsVirtual(false)
220 : : {
221 : 262 : }
222 : :
223 : : /** \brief Constructs a body from mass, center of mass, a 3x3 inertia matrix, the center of buoyancy,
224 : : * and hydrodynamic parameters
225 : : *
226 : : * This constructor eases the construction of a new body as all the
227 : : * required parameters can simply be specified as parameters to the
228 : : * constructor. These are then used to generate the spatial inertia
229 : : * matrix which is expressed at the origin.
230 : : *
231 : : * \param mass the mass of the body
232 : : * \param com the position of the center of mass in the bodies coordinates
233 : : * \param inertia_C the inertia at the center of mass
234 : : * \param centerOfBuoyancy The center of buoyancy for the body
235 : : * \param volume The volume taken up by the body
236 : : * \param addedMassMatrix The hydrodynamic added mass matrix for the body
237 : : */
238 : 620 : Body(const double& mass, const Math::Vector3d& com, const Math::Matrix3d& inertia_C, const Math::Vector3d& centerOfBuoyancy, const double volume,
239 : : const Math::SpatialMatrix addedMassMatrix)
240 : 620 : : mMass(mass), mCenterOfMass(com), mInertia(inertia_C), volume(volume), mCenterOfBuoyancy(centerOfBuoyancy), addedMassMatrix(addedMassMatrix), mIsVirtual(false)
241 : : {
242 : 620 : dragData.zero();
243 : 620 : }
244 : :
245 : : /** \brief Constructs a body from mass, center of mass, a 3x3 inertia matrix. Hydrodynamic terms are set such that
246 : : * they will have no dynamic effects
247 : : *
248 : : * This constructor eases the construction of a new body as all the
249 : : * required parameters can simply be specified as parameters to the
250 : : * constructor. These are then used to generate the spatial inertia
251 : : * matrix which is expressed at the origin.
252 : : *
253 : : * \param mass the mass of the body
254 : : * \param com the position of the center of mass in the bodies coordinates
255 : : * \param inertia_C the inertia at the center of mass
256 : : */
257 : 137 : Body(const double& mass, const Math::Vector3d& com, const Math::Matrix3d& inertia_C)
258 : 137 : : mMass(mass), mCenterOfMass(com), mInertia(inertia_C), volume(0.), mCenterOfBuoyancy(Math::Vector3dZero), mIsVirtual(false)
259 : : {
260 : 137 : addedMassMatrix.setZero();
261 : 137 : dragData.zero();
262 : 137 : }
263 : :
264 : : /** \brief Joins inertial parameters of two bodies to create a composite
265 : : * body.
266 : : *
267 : : * This function can be used to join inertial parameters of two bodies
268 : : * to create a composite body that has the inertial properties as if the
269 : : * two bodies were joined by a fixed joint.
270 : : *
271 : : * \note The drag terms and added mass matrices for both bodies will simply be added
272 : : * together
273 : : *
274 : : * \param transform The frame transformation from the origin of the
275 : : * original body to the origin of the added body
276 : : * \param other_body The other body that will be merged with *this.
277 : : */
278 : 491 : void join(const Math::SpatialTransform& transform, const Body& other_body)
279 : : {
280 : : // nothing to do if we join a massles body to the current.
281 : 491 : if (other_body.mMass == 0. && other_body.mInertia == Math::Matrix3d::Zero())
282 : : {
283 : 91 : return;
284 : : }
285 : :
286 : 400 : double other_mass = other_body.mMass;
287 : 400 : double new_mass = mMass + other_mass;
288 : :
289 : 400 : double other_volume = other_body.volume;
290 : 400 : double new_volume = volume + other_volume;
291 : :
292 : 400 : if (new_mass == 0.)
293 : : {
294 : 1 : throw RdlException("Error: cannot join bodies as both have zero mass!");
295 : : }
296 : :
297 : 399 : Math::Vector3d other_com = transform.E.transpose() * other_body.mCenterOfMass + transform.r;
298 : 399 : Math::Vector3d new_com = (1. / new_mass) * (mMass * mCenterOfMass + other_mass * other_com);
299 : 399 : Math::Vector3d new_cob = mCenterOfBuoyancy;
300 : 399 : if (new_volume != 0.)
301 : : {
302 : 267 : Math::Vector3d other_cob = transform.E.transpose() * other_body.mCenterOfBuoyancy + transform.r;
303 : 267 : new_cob = (1. / new_volume) * (volume * mCenterOfBuoyancy + other_volume * other_cob);
304 : : }
305 : :
306 : : // We have to transform the inertia of other_body to the new COM. This
307 : : // is done in 4 steps:
308 : : //
309 : : // 1. Transform the inertia from other origin to other COM
310 : : // 2. Rotate the inertia that it is aligned to the frame of this body
311 : : // 3. Transform inertia of other_body to the origin of the frame of
312 : : // this body
313 : : // 4. Sum the two inertias
314 : : // 5. Transform the summed inertia to the new COM
315 : :
316 : 399 : Math::RigidBodyInertia other_rbi = Math::createFromMassComInertiaC(other_body.mMass, other_body.mCenterOfMass, other_body.mInertia);
317 : 399 : Math::RigidBodyInertia this_rbi = Math::createFromMassComInertiaC(mMass, mCenterOfMass, mInertia);
318 : :
319 : 399 : Math::Matrix3d inertia_other = other_rbi.toMatrix().block<3, 3>(0, 0);
320 : :
321 : : // 1. Transform the inertia from other origin to other COM
322 : 399 : Math::Matrix3d other_com_cross = other_body.mCenterOfMass.toTildeForm();
323 : 399 : Math::Matrix3d inertia_other_com = inertia_other - other_mass * other_com_cross * other_com_cross.transpose();
324 : :
325 : : // 2. Rotate the inertia that it is aligned to the frame of this body
326 : 399 : Math::Matrix3d inertia_other_com_rotated = transform.E.transpose() * inertia_other_com * transform.E;
327 : :
328 : : // 3. Transform inertia of other_body to the origin of the frame of this body
329 : 399 : Math::Matrix3d inertia_other_com_rotated_this_origin = Math::parallel_axis(inertia_other_com_rotated, other_mass, other_com);
330 : :
331 : : // 4. Sum the two inertias
332 : 399 : Math::Matrix3d inertia_summed = Math::Matrix3d(this_rbi.toMatrix().block<3, 3>(0, 0)) + inertia_other_com_rotated_this_origin;
333 : :
334 : : // 5. Transform the summed inertia to the new COM
335 : 399 : Math::Matrix3d new_inertia = inertia_summed - new_mass * new_com.toTildeForm() * new_com.toTildeForm().transpose();
336 : :
337 : 399 : Math::SpatialMatrix new_added_mass_matrix = this->addedMassMatrix + other_body.addedMassMatrix;
338 : 399 : DragData new_drag_data = this->dragData + other_body.dragData;
339 : :
340 : 399 : *this = Body(new_mass, new_com, new_inertia, new_cob, new_volume, new_added_mass_matrix, new_drag_data);
341 : : }
342 : :
343 : 18151 : ~Body(){};
344 : :
345 : : /// \brief The mass of the body
346 : : double mMass;
347 : : /// \brief The position of the center of mass in body coordinates
348 : : Math::Vector3d mCenterOfMass;
349 : : /// \brief Inertia matrix at the center of mass
350 : : Math::Matrix3d mInertia;
351 : :
352 : : double volume; /**< Volume taken up by body. */
353 : : Math::Vector3d mCenterOfBuoyancy; /**< Center of buoyancy for a body */
354 : : Math::SpatialMatrix addedMassMatrix; /**< The hydrodynamic added mass for the body */
355 : : DragData dragData; /**< The hydrodynamic drag data for the body */
356 : : bool mIsVirtual;
357 : : };
358 : :
359 : : /** \brief Keeps the information of a body and how it is attached to another body.
360 : : *
361 : : * When using fixed bodies, i.e. a body that is attached to anothe via a
362 : : * fixed joint, the attached body is merged onto its parent. By doing so
363 : : * adding fixed joints do not have an impact on runtime.
364 : : */
365 : : struct FixedBody
366 : : {
367 : : /// \brief The mass of the body
368 : : double mMass;
369 : : /// \brief The position of the center of mass in body coordinates
370 : : Math::Vector3d mCenterOfMass;
371 : : /// \brief The spatial inertia that contains both mass and inertia information
372 : : Math::Matrix3d mInertia;
373 : : double volume; /**< Volume taken up by body. */
374 : : Math::Vector3d mCenterOfBuoyancy; /**< Center of buoyancy for a body */
375 : : Math::SpatialMatrix addedMassMatrix; /**< The hydrodynamic added mass for the body */
376 : : DragData dragData; /**< The hydrodynamic drag data for the body */
377 : :
378 : : /// \brief Id of the movable body that this fixed body is attached to.
379 : : unsigned int mMovableParent;
380 : : /// \brief Transforms spatial quantities expressed for the parent to the
381 : : // fixed body.
382 : : Math::SpatialTransform mParentTransform;
383 : : Math::SpatialTransform mBaseTransform;
384 : :
385 : 484 : static FixedBody CreateFromBody(const Body& body)
386 : : {
387 : 484 : FixedBody fbody;
388 : :
389 : 484 : fbody.mMass = body.mMass;
390 : 484 : fbody.mCenterOfMass = body.mCenterOfMass;
391 : 484 : fbody.mInertia = body.mInertia;
392 : 484 : fbody.mCenterOfBuoyancy = body.mCenterOfBuoyancy;
393 : 484 : fbody.volume = body.volume;
394 : 484 : fbody.addedMassMatrix = body.addedMassMatrix;
395 : 484 : fbody.dragData = body.dragData;
396 : :
397 : 484 : return fbody;
398 : : }
399 : : };
400 : : } // namespace RobotDynamics
401 : :
402 : : #endif
|
