Gazebo Math

API Reference

7.5.1
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>
23 #include "gz/math/MassMatrix3.hh"
24 #include "gz/math/Matrix3.hh"
25 #include "gz/math/Matrix6.hh"
26 #include "gz/math/Pose3.hh"
27 
28 namespace gz::math
29 {
30  // Inline bracket to help doxygen filtering.
31  inline namespace GZ_MATH_VERSION_NAMESPACE {
32  //
49  template<typename T>
50  class Inertial
51  {
53  public: Inertial()
54  {}
55 
65  public: Inertial(const MassMatrix3<T> &_massMatrix,
66  const Pose3<T> &_pose)
67  : massMatrix(_massMatrix), pose(_pose)
68  {}
69 
81  public: Inertial(const MassMatrix3<T> &_massMatrix,
82  const Pose3<T> &_pose,
83  const Matrix6<T> &_addedMass)
84  : massMatrix(_massMatrix), pose(_pose), addedMass(_addedMass)
85  {}
86 
89  public: Inertial(const Inertial<T> &_inertial) = default;
90 
92  public: ~Inertial() = default;
93 
104  public: bool SetMassMatrix(const MassMatrix3<T> &_m,
105  const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
106  {
107  this->massMatrix = _m;
108  return this->massMatrix.IsValid(_tolerance);
109  }
110 
115  public: const MassMatrix3<T> &MassMatrix() const
116  {
117  return this->massMatrix;
118  }
119 
123  public: bool SetPose(const Pose3<T> &_pose)
124  {
125  this->pose = _pose;
126  return this->massMatrix.IsValid();
127  }
128 
132  public: const Pose3<T> &Pose() const
133  {
134  return this->pose;
135  }
136 
145  public: bool SetFluidAddedMass(const Matrix6<T> &_m)
146  {
147  this->addedMass = _m;
148  return this->addedMass.value() == this->addedMass.value().Transposed();
149  }
150 
154  public: const std::optional< Matrix6<T> > &FluidAddedMass() const
155  {
156  return this->addedMass;
157  }
158 
164  public: Matrix3<T> Moi() const
165  {
166  auto R = Matrix3<T>(this->pose.Rot());
167  return R * this->massMatrix.Moi() * R.Transposed();
168  }
169 
185  public: Matrix6<T> BodyMatrix() const
186  {
187  Matrix6<T> result;
188 
190  Matrix3<T>::Identity * this->massMatrix.Mass());
191 
192  result.SetSubmatrix(Matrix6<T>::BOTTOM_RIGHT, this->Moi());
193 
194  auto x = this->pose.Pos().X();
195  auto y = this->pose.Pos().Y();
196  auto z = this->pose.Pos().Z();
197  auto skew = Matrix3<T>(
198  0, -z, y,
199  z, 0, -x,
200  -y, x, 0) * this->massMatrix.Mass();
203 
204  return result;
205  }
206 
214  public: Matrix6<T> SpatialMatrix() const
215  {
216  return this->addedMass.has_value() ?
217  this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
218  }
219 
224  public: bool SetInertialRotation(const Quaternion<T> &_q)
225  {
226  auto moi = this->Moi();
227  this->pose.Rot() = _q;
228  auto R = Matrix3<T>(_q);
229  return this->massMatrix.SetMoi(R.Transposed() * moi * R);
230  }
231 
244  public: bool SetMassMatrixRotation(const Quaternion<T> &_q,
245  const T _tol = 1e-6)
246  {
247  this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
248  _q.Inverse();
249  const auto moments = this->MassMatrix().PrincipalMoments(_tol);
250  const auto diag = Matrix3<T>(
251  moments[0], 0, 0,
252  0, moments[1], 0,
253  0, 0, moments[2]);
254  const auto R = Matrix3<T>(_q);
255  return this->massMatrix.SetMoi(R * diag * R.Transposed());
256  }
257 
261  public: Inertial &operator=(const Inertial<T> &_inertial) = default;
262 
267  public: bool operator==(const Inertial<T> &_inertial) const
268  {
269  return (this->pose == _inertial.Pose()) &&
270  (this->massMatrix == _inertial.MassMatrix()) &&
271  (this->addedMass == _inertial.FluidAddedMass());
272  }
273 
277  public: bool operator!=(const Inertial<T> &_inertial) const
278  {
279  return !(*this == _inertial);
280  }
281 
287  public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
288  {
289  T m1 = this->massMatrix.Mass();
290  T m2 = _inertial.MassMatrix().Mass();
291 
292  // Total mass
293  T mass = m1 + m2;
294 
295  // Only continue if total mass is positive
296  if (mass <= 0)
297  {
298  return *this;
299  }
300 
301  auto com1 = this->Pose().Pos();
302  auto com2 = _inertial.Pose().Pos();
303  // New center of mass location in base frame
304  auto com = (m1*com1 + m2*com2) / mass;
305 
306  // Components of new moment of inertia matrix
307  Vector3<T> ixxyyzz;
308  Vector3<T> ixyxzyz;
309  // First add matrices in base frame
310  {
311  auto moi = this->Moi() + _inertial.Moi();
312  ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
313  ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
314  }
315  // Then account for parallel axis theorem
316  {
317  auto dc = com1 - com;
318  ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
319  ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
320  ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
321  ixyxzyz.X() -= m1 * dc[0] * dc[1];
322  ixyxzyz.Y() -= m1 * dc[0] * dc[2];
323  ixyxzyz.Z() -= m1 * dc[1] * dc[2];
324  }
325  {
326  auto dc = com2 - com;
327  ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
328  ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
329  ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
330  ixyxzyz.X() -= m2 * dc[0] * dc[1];
331  ixyxzyz.Y() -= m2 * dc[0] * dc[2];
332  ixyxzyz.Z() -= m2 * dc[1] * dc[2];
333  }
334  this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
335  this->pose = Pose3<T>(com, Quaternion<T>::Identity);
336 
337  return *this;
338  }
339 
345  public: Inertial<T> &operator-=(const Inertial<T> &_inertial)
346  {
347  T m = this->massMatrix.Mass();
348  T m2 = _inertial.MassMatrix().Mass();
349 
350  // Remaining mass
351  T m1 = m - m2;
352 
353  // Only continue if remaining mass is positive
354  if (m1 <= 0)
355  {
356  return *this;
357  }
358 
359  auto com = this->Pose().Pos();
360  auto com2 = _inertial.Pose().Pos();
361  // Remaining center of mass location in base frame
362  auto com1 = (m*com - m2*com2)/m1;
363 
364  // Components of new moment of inertia matrix
365  Vector3<T> ixxyyzz;
366  Vector3<T> ixyxzyz;
367 
368  // First subtract matrices in base frame
369  {
370  auto moi = this->Moi() - _inertial.Moi();
371  ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
372  ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
373  }
374  // Then account for parallel axis theorem
375  {
376  auto dc = com1 - com;
377  ixxyyzz.X() -= m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
378  ixxyyzz.Y() -= m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
379  ixxyyzz.Z() -= m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
380  ixyxzyz.X() += m1 * dc[0] * dc[1];
381  ixyxzyz.Y() += m1 * dc[0] * dc[2];
382  ixyxzyz.Z() += m1 * dc[1] * dc[2];
383  }
384  {
385  auto dc = com2 - com;
386  ixxyyzz.X() -= m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
387  ixxyyzz.Y() -= m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
388  ixxyyzz.Z() -= m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
389  ixyxzyz.X() += m2 * dc[0] * dc[1];
390  ixyxzyz.Y() += m2 * dc[0] * dc[2];
391  ixyxzyz.Z() += m2 * dc[1] * dc[2];
392  }
393  this->massMatrix = MassMatrix3<T>(m1, ixxyyzz, ixyxzyz);
394  this->pose = Pose3<T>(com1, Quaternion<T>::Identity);
395 
396  return *this;
397  }
398 
404  public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
405  {
406  return Inertial<T>(*this) += _inertial;
407  }
408 
414  public: const Inertial<T> operator-(const Inertial<T> &_inertial) const
415  {
416  return Inertial<T>(*this) -= _inertial;
417  }
418 
421  private: MassMatrix3<T> massMatrix;
422 
425  private: Pose3<T> pose;
426 
428  private: std::optional<Matrix6<T>> addedMass;
429  };
430 
433  } // namespace GZ_MATH_VERSION_NAMESPACE
434 } // namespace gz::math
435 #endif // GZ_MATH_INERTIAL_HH_