Gazebo Math

API Reference

8.1.0
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::math
29{
30 // Inline bracket to help doxygen filtering.
31 inline namespace GZ_MATH_VERSION_NAMESPACE {
32 //
49 template<typename T>
51 {
53 public: Inertial()
54 {}
55
66 const Pose3<T> &_pose)
67 : massMatrix(_massMatrix), pose(_pose)
68 {}
69
82 const Pose3<T> &_pose,
84 : massMatrix(_massMatrix), pose(_pose), addedMass(_addedMass)
85 {}
86
97 public: bool SetMassMatrix(const MassMatrix3<T> &_m,
99 {
100 this->massMatrix = _m;
101 return this->massMatrix.IsValid(_tolerance);
102 }
103
108 public: const MassMatrix3<T> &MassMatrix() const
109 {
110 return this->massMatrix;
111 }
112
116 public: bool SetPose(const Pose3<T> &_pose)
117 {
118 this->pose = _pose;
119 return this->massMatrix.IsValid();
120 }
121
125 public: const Pose3<T> &Pose() const
126 {
127 return this->pose;
128 }
129
138 public: bool SetFluidAddedMass(const Matrix6<T> &_m)
139 {
140 this->addedMass = _m;
141 return this->addedMass.value() == this->addedMass.value().Transposed();
142 }
143
147 public: const std::optional< Matrix6<T> > &FluidAddedMass() const
148 {
149 return this->addedMass;
150 }
151
157 public: Matrix3<T> Moi() const
158 {
159 auto R = Matrix3<T>(this->pose.Rot());
160 return R * this->massMatrix.Moi() * R.Transposed();
161 }
162
178 public: Matrix6<T> BodyMatrix() const
179 {
181
183 Matrix3<T>::Identity * this->massMatrix.Mass());
184
186
187 auto x = this->pose.Pos().X();
188 auto y = this->pose.Pos().Y();
189 auto z = this->pose.Pos().Z();
190 auto skew = Matrix3<T>(
191 0, -z, y,
192 z, 0, -x,
193 -y, x, 0) * this->massMatrix.Mass();
196
197 return result;
198 }
199
207 public: Matrix6<T> SpatialMatrix() const
208 {
209 return this->addedMass.has_value() ?
210 this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
211 }
212
218 {
219 auto moi = this->Moi();
220 this->pose.Rot() = _q;
221 auto R = Matrix3<T>(_q);
222 return this->massMatrix.SetMoi(R.Transposed() * moi * R);
223 }
224
238 const T _tol = 1e-6)
239 {
240 this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
241 _q.Inverse();
242 const auto moments = this->MassMatrix().PrincipalMoments(_tol);
243 const auto diag = Matrix3<T>(
244 moments[0], 0, 0,
245 0, moments[1], 0,
246 0, 0, moments[2]);
247 const auto R = Matrix3<T>(_q);
248 return this->massMatrix.SetMoi(R * diag * R.Transposed());
249 }
250
255 public: bool operator==(const Inertial<T> &_inertial) const
256 {
257 return (this->pose == _inertial.Pose()) &&
258 (this->massMatrix == _inertial.MassMatrix()) &&
259 (this->addedMass == _inertial.FluidAddedMass());
260 }
261
265 public: bool operator!=(const Inertial<T> &_inertial) const
266 {
267 return !(*this == _inertial);
268 }
269
276 {
277 T m1 = this->massMatrix.Mass();
278 T m2 = _inertial.MassMatrix().Mass();
279
280 // Total mass
281 T mass = m1 + m2;
282
283 // Only continue if total mass is positive
284 if (mass <= 0)
285 {
286 return *this;
287 }
288
289 auto com1 = this->Pose().Pos();
290 auto com2 = _inertial.Pose().Pos();
291 // New center of mass location in base frame
292 auto com = (m1*com1 + m2*com2) / mass;
293
294 // Components of new moment of inertia matrix
297 // First add matrices in base frame
298 {
299 auto moi = this->Moi() + _inertial.Moi();
300 ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
301 ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
302 }
303 // Then account for parallel axis theorem
304 {
305 auto dc = com1 - com;
306 ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
307 ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
308 ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
309 ixyxzyz.X() -= m1 * dc[0] * dc[1];
310 ixyxzyz.Y() -= m1 * dc[0] * dc[2];
311 ixyxzyz.Z() -= m1 * dc[1] * dc[2];
312 }
313 {
314 auto dc = com2 - com;
315 ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
316 ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
317 ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
318 ixyxzyz.X() -= m2 * dc[0] * dc[1];
319 ixyxzyz.Y() -= m2 * dc[0] * dc[2];
320 ixyxzyz.Z() -= m2 * dc[1] * dc[2];
321 }
322 this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
323 this->pose = Pose3<T>(com, Quaternion<T>::Identity);
324
325 return *this;
326 }
327
334 {
335 T m = this->massMatrix.Mass();
336 T m2 = _inertial.MassMatrix().Mass();
337
338 // Remaining mass
339 T m1 = m - m2;
340
341 // Only continue if remaining mass is positive
342 if (m1 <= 0)
343 {
344 return *this;
345 }
346
347 auto com = this->Pose().Pos();
348 auto com2 = _inertial.Pose().Pos();
349 // Remaining center of mass location in base frame
350 auto com1 = (m*com - m2*com2)/m1;
351
352 // Components of new moment of inertia matrix
355
356 // First subtract matrices in base frame
357 {
358 auto moi = this->Moi() - _inertial.Moi();
359 ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
360 ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
361 }
362 // Then account for parallel axis theorem
363 {
364 auto dc = com1 - com;
365 ixxyyzz.X() -= m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
366 ixxyyzz.Y() -= m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
367 ixxyyzz.Z() -= m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
368 ixyxzyz.X() += m1 * dc[0] * dc[1];
369 ixyxzyz.Y() += m1 * dc[0] * dc[2];
370 ixyxzyz.Z() += m1 * dc[1] * dc[2];
371 }
372 {
373 auto dc = com2 - com;
374 ixxyyzz.X() -= m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
375 ixxyyzz.Y() -= m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
376 ixxyyzz.Z() -= m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
377 ixyxzyz.X() += m2 * dc[0] * dc[1];
378 ixyxzyz.Y() += m2 * dc[0] * dc[2];
379 ixyxzyz.Z() += m2 * dc[1] * dc[2];
380 }
381 this->massMatrix = MassMatrix3<T>(m1, ixxyyzz, ixyxzyz);
383
384 return *this;
385 }
386
392 public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
393 {
394 return Inertial<T>(*this) += _inertial;
395 }
396
402 public: const Inertial<T> operator-(const Inertial<T> &_inertial) const
403 {
404 return Inertial<T>(*this) -= _inertial;
405 }
406
409 private: MassMatrix3<T> massMatrix;
410
413 private: Pose3<T> pose;
414
416 private: std::optional<Matrix6<T>> addedMass;
417 };
418
421 } // namespace GZ_MATH_VERSION_NAMESPACE
422} // namespace gz::math
423#endif // GZ_MATH_INERTIAL_HH_