Gazebo Math

API Reference

7.4.0
gz/math/Inertial.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2016 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17#ifndef GZ_MATH_INERTIAL_HH_
18#define GZ_MATH_INERTIAL_HH_
19
20#include <optional>
21
22#include <gz/math/config.hh>
24#include "gz/math/Matrix3.hh"
25#include "gz/math/Matrix6.hh"
26#include "gz/math/Pose3.hh"
27
28namespace gz
29{
30 namespace math
31 {
32 // Inline bracket to help doxygen filtering.
33 inline namespace GZ_MATH_VERSION_NAMESPACE {
34 //
51 template<typename T>
53 {
55 public: Inertial()
56 {}
57
67 public: Inertial(const MassMatrix3<T> &_massMatrix,
68 const Pose3<T> &_pose)
69 : massMatrix(_massMatrix), pose(_pose)
70 {}
71
83 public: Inertial(const MassMatrix3<T> &_massMatrix,
84 const Pose3<T> &_pose,
85 const Matrix6<T> &_addedMass)
86 : massMatrix(_massMatrix), pose(_pose), addedMass(_addedMass)
87 {}
88
91 public: Inertial(const Inertial<T> &_inertial) = default;
92
94 public: ~Inertial() = default;
95
106 public: bool SetMassMatrix(const MassMatrix3<T> &_m,
107 const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
108 {
109 this->massMatrix = _m;
110 return this->massMatrix.IsValid(_tolerance);
111 }
112
117 public: const MassMatrix3<T> &MassMatrix() const
118 {
119 return this->massMatrix;
120 }
121
125 public: bool SetPose(const Pose3<T> &_pose)
126 {
127 this->pose = _pose;
128 return this->massMatrix.IsValid();
129 }
130
134 public: const Pose3<T> &Pose() const
135 {
136 return this->pose;
137 }
138
147 public: bool SetFluidAddedMass(const Matrix6<T> &_m)
148 {
149 this->addedMass = _m;
150 return this->addedMass.value() == this->addedMass.value().Transposed();
151 }
152
156 public: const std::optional< Matrix6<T> > &FluidAddedMass() const
157 {
158 return this->addedMass;
159 }
160
166 public: Matrix3<T> Moi() const
167 {
168 auto R = Matrix3<T>(this->pose.Rot());
169 return R * this->massMatrix.Moi() * R.Transposed();
170 }
171
187 public: Matrix6<T> BodyMatrix() const
188 {
189 Matrix6<T> result;
190
192 Matrix3<T>::Identity * this->massMatrix.Mass());
193
194 result.SetSubmatrix(Matrix6<T>::BOTTOM_RIGHT, this->Moi());
195
196 auto x = this->pose.Pos().X();
197 auto y = this->pose.Pos().Y();
198 auto z = this->pose.Pos().Z();
199 auto skew = Matrix3<T>(
200 0, -z, y,
201 z, 0, -x,
202 -y, x, 0) * this->massMatrix.Mass();
205
206 return result;
207 }
208
216 public: Matrix6<T> SpatialMatrix() const
217 {
218 return this->addedMass.has_value() ?
219 this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
220 }
221
226 public: bool SetInertialRotation(const Quaternion<T> &_q)
227 {
228 auto moi = this->Moi();
229 this->pose.Rot() = _q;
230 auto R = Matrix3<T>(_q);
231 return this->massMatrix.SetMoi(R.Transposed() * moi * R);
232 }
233
246 public: bool SetMassMatrixRotation(const Quaternion<T> &_q,
247 const T _tol = 1e-6)
248 {
249 this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
250 _q.Inverse();
251 const auto moments = this->MassMatrix().PrincipalMoments(_tol);
252 const auto diag = Matrix3<T>(
253 moments[0], 0, 0,
254 0, moments[1], 0,
255 0, 0, moments[2]);
256 const auto R = Matrix3<T>(_q);
257 return this->massMatrix.SetMoi(R * diag * R.Transposed());
258 }
259
263 public: Inertial &operator=(const Inertial<T> &_inertial) = default;
264
269 public: bool operator==(const Inertial<T> &_inertial) const
270 {
271 return (this->pose == _inertial.Pose()) &&
272 (this->massMatrix == _inertial.MassMatrix()) &&
273 (this->addedMass == _inertial.FluidAddedMass());
274 }
275
279 public: bool operator!=(const Inertial<T> &_inertial) const
280 {
281 return !(*this == _inertial);
282 }
283
289 public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
290 {
291 T m1 = this->massMatrix.Mass();
292 T m2 = _inertial.MassMatrix().Mass();
293
294 // Total mass
295 T mass = m1 + m2;
296
297 // Only continue if total mass is positive
298 if (mass <= 0)
299 {
300 return *this;
301 }
302
303 auto com1 = this->Pose().Pos();
304 auto com2 = _inertial.Pose().Pos();
305 // New center of mass location in base frame
306 auto com = (m1*com1 + m2*com2) / mass;
307
308 // Components of new moment of inertia matrix
309 Vector3<T> ixxyyzz;
310 Vector3<T> ixyxzyz;
311 // First add matrices in base frame
312 {
313 auto moi = this->Moi() + _inertial.Moi();
314 ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
315 ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
316 }
317 // Then account for parallel axis theorem
318 {
319 auto dc = com1 - com;
320 ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
321 ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
322 ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
323 ixyxzyz.X() -= m1 * dc[0] * dc[1];
324 ixyxzyz.Y() -= m1 * dc[0] * dc[2];
325 ixyxzyz.Z() -= m1 * dc[1] * dc[2];
326 }
327 {
328 auto dc = com2 - com;
329 ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
330 ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
331 ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
332 ixyxzyz.X() -= m2 * dc[0] * dc[1];
333 ixyxzyz.Y() -= m2 * dc[0] * dc[2];
334 ixyxzyz.Z() -= m2 * dc[1] * dc[2];
335 }
336 this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
337 this->pose = Pose3<T>(com, Quaternion<T>::Identity);
338
339 return *this;
340 }
341
347 public: Inertial<T> &operator-=(const Inertial<T> &_inertial)
348 {
349 T m = this->massMatrix.Mass();
350 T m2 = _inertial.MassMatrix().Mass();
351
352 // Remaining mass
353 T m1 = m - m2;
354
355 // Only continue if remaining mass is positive
356 if (m1 <= 0)
357 {
358 return *this;
359 }
360
361 auto com = this->Pose().Pos();
362 auto com2 = _inertial.Pose().Pos();
363 // Remaining center of mass location in base frame
364 auto com1 = (m*com - m2*com2)/m1;
365
366 // Components of new moment of inertia matrix
367 Vector3<T> ixxyyzz;
368 Vector3<T> ixyxzyz;
369
370 // First subtract matrices in base frame
371 {
372 auto moi = this->Moi() - _inertial.Moi();
373 ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
374 ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
375 }
376 // Then account for parallel axis theorem
377 {
378 auto dc = com1 - com;
379 ixxyyzz.X() -= m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
380 ixxyyzz.Y() -= m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
381 ixxyyzz.Z() -= m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
382 ixyxzyz.X() += m1 * dc[0] * dc[1];
383 ixyxzyz.Y() += m1 * dc[0] * dc[2];
384 ixyxzyz.Z() += m1 * dc[1] * dc[2];
385 }
386 {
387 auto dc = com2 - com;
388 ixxyyzz.X() -= m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
389 ixxyyzz.Y() -= m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
390 ixxyyzz.Z() -= m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
391 ixyxzyz.X() += m2 * dc[0] * dc[1];
392 ixyxzyz.Y() += m2 * dc[0] * dc[2];
393 ixyxzyz.Z() += m2 * dc[1] * dc[2];
394 }
395 this->massMatrix = MassMatrix3<T>(m1, ixxyyzz, ixyxzyz);
396 this->pose = Pose3<T>(com1, Quaternion<T>::Identity);
397
398 return *this;
399 }
400
406 public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
407 {
408 return Inertial<T>(*this) += _inertial;
409 }
410
416 public: const Inertial<T> operator-(const Inertial<T> &_inertial) const
417 {
418 return Inertial<T>(*this) -= _inertial;
419 }
420
423 private: MassMatrix3<T> massMatrix;
424
427 private: Pose3<T> pose;
428
430 private: std::optional<Matrix6<T>> addedMass;
431 };
432
435 }
436 }
437}
438#endif