Gazebo Math

API Reference

6.15.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 <gz/math/config.hh>
21 #include "gz/math/MassMatrix3.hh"
22 #include "gz/math/Pose3.hh"
23 
24 namespace ignition
25 {
26  namespace math
27  {
28  // Inline bracket to help doxygen filtering.
29  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
30  //
44  template<typename T>
45  class Inertial
46  {
48  public: Inertial()
49  {}
50 
60  public: Inertial(const MassMatrix3<T> &_massMatrix,
61  const Pose3<T> &_pose)
62  : massMatrix(_massMatrix), pose(_pose)
63  {}
64 
67  public: Inertial(const Inertial<T> &_inertial)
68  : massMatrix(_inertial.MassMatrix()), pose(_inertial.Pose())
69  {}
70 
72  public: virtual ~Inertial() {}
73 
84  public: bool SetMassMatrix(const MassMatrix3<T> &_m,
85  const T _tolerance = IGN_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
86  {
87  this->massMatrix = _m;
88  return this->massMatrix.IsValid(_tolerance);
89  }
90 
95  public: const MassMatrix3<T> &MassMatrix() const
96  {
97  return this->massMatrix;
98  }
99 
103  public: bool SetPose(const Pose3<T> &_pose)
104  {
105  this->pose = _pose;
106  return this->massMatrix.IsValid();
107  }
108 
112  public: const Pose3<T> &Pose() const
113  {
114  return this->pose;
115  }
116 
119  public: Matrix3<T> IGN_DEPRECATED(5.0) MOI() const
120  {
121  return this->Moi();
122  }
123 
129  public: Matrix3<T> Moi() const
130  {
131  auto R = Matrix3<T>(this->pose.Rot());
132  return R * this->massMatrix.Moi() * R.Transposed();
133  }
134 
139  public: bool SetInertialRotation(const Quaternion<T> &_q)
140  {
141  auto moi = this->Moi();
142  this->pose.Rot() = _q;
143  auto R = Matrix3<T>(_q);
144  return this->massMatrix.SetMoi(R.Transposed() * moi * R);
145  }
146 
159  public: bool SetMassMatrixRotation(const Quaternion<T> &_q,
160  const T _tol = 1e-6)
161  {
162  this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
163  _q.Inverse();
164  const auto moments = this->MassMatrix().PrincipalMoments(_tol);
165  const auto diag = Matrix3<T>(
166  moments[0], 0, 0,
167  0, moments[1], 0,
168  0, 0, moments[2]);
169  const auto R = Matrix3<T>(_q);
170  return this->massMatrix.SetMoi(R * diag * R.Transposed());
171  }
172 
176  public: Inertial &operator=(const Inertial<T> &_inertial)
177  {
178  this->massMatrix = _inertial.MassMatrix();
179  this->pose = _inertial.Pose();
180 
181  return *this;
182  }
183 
188  public: bool operator==(const Inertial<T> &_inertial) const
189  {
190  return (this->pose == _inertial.Pose()) &&
191  (this->massMatrix == _inertial.MassMatrix());
192  }
193 
197  public: bool operator!=(const Inertial<T> &_inertial) const
198  {
199  return !(*this == _inertial);
200  }
201 
207  public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
208  {
209  T m1 = this->massMatrix.Mass();
210  T m2 = _inertial.MassMatrix().Mass();
211 
212  // Total mass
213  T mass = m1 + m2;
214 
215  // Only continue if total mass is positive
216  if (mass <= 0)
217  {
218  return *this;
219  }
220 
221  auto com1 = this->Pose().Pos();
222  auto com2 = _inertial.Pose().Pos();
223  // New center of mass location in base frame
224  auto com = (m1*com1 + m2*com2) / mass;
225 
226  // Components of new moment of inertia matrix
227  Vector3<T> ixxyyzz;
228  Vector3<T> ixyxzyz;
229  // First add matrices in base frame
230  {
231  auto moi = this->Moi() + _inertial.Moi();
232  ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
233  ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
234  }
235  // Then account for parallel axis theorem
236  {
237  auto dc = com1 - com;
238  ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
239  ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
240  ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
241  ixyxzyz.X() -= m1 * dc[0] * dc[1];
242  ixyxzyz.Y() -= m1 * dc[0] * dc[2];
243  ixyxzyz.Z() -= m1 * dc[1] * dc[2];
244  }
245  {
246  auto dc = com2 - com;
247  ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
248  ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
249  ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
250  ixyxzyz.X() -= m2 * dc[0] * dc[1];
251  ixyxzyz.Y() -= m2 * dc[0] * dc[2];
252  ixyxzyz.Z() -= m2 * dc[1] * dc[2];
253  }
254  this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
255  this->pose = Pose3<T>(com, Quaternion<T>::Identity);
256 
257  return *this;
258  }
259 
265  public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
266  {
267  return Inertial<T>(*this) += _inertial;
268  }
269 
272  private: MassMatrix3<T> massMatrix;
273 
276  private: Pose3<T> pose;
277  };
278 
281  }
282  }
283 }
284 #endif
Encapsulates a position and rotation in three space.
Definition: gz/math/Pose3.hh:34
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
Inertial()
Default Constructor.
Definition: gz/math/Inertial.hh:48
bool IsValid(const T _tolerance=IGN_MASSMATRIX3_DEFAULT_TOLERANCE< T >) const
Verify that inertia values are positive semi-definite and satisfy the triangle inequality.
Definition: gz/math/MassMatrix3.hh:595
T X() const
Get the x value.
Definition: gz/math/Vector3.hh:654
A 3x3 matrix class.
Definition: gz/math/Matrix3.hh:40
bool SetPose(const Pose3< T > &_pose)
Set the pose of the center of mass reference frame.
Definition: gz/math/Inertial.hh:103
Inertial< float > Inertialf
Definition: gz/math/Inertial.hh:280
The Inertial object provides a representation for the mass and inertia matrix of a body B....
Definition: gz/math/Inertial.hh:45
Inertial & operator=(const Inertial< T > &_inertial)
Equal operator.
Definition: gz/math/Inertial.hh:176
const MassMatrix3< T > & MassMatrix() const
Get the mass and inertia matrix.
Definition: gz/math/Inertial.hh:95
bool operator==(const Inertial< T > &_inertial) const
Equality comparison operator.
Definition: gz/math/Inertial.hh:188
T Z() const
Get the z value.
Definition: gz/math/Vector3.hh:668
Matrix3< T > Moi() const
Get the moment of inertia matrix computer about the body's center of mass and expressed in this Inert...
Definition: gz/math/Inertial.hh:129
const Inertial< T > operator+(const Inertial< T > &_inertial) const
Adds inertial properties to current object. The mass, center of mass location, and inertia matrix are...
Definition: gz/math/Inertial.hh:265
Inertial(const Inertial< T > &_inertial)
Copy constructor.
Definition: gz/math/Inertial.hh:67
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
const Pose3< T > & Pose() const
Get the pose of the center of mass reference frame.
Definition: gz/math/Inertial.hh:112
Inertial(const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
Constructs an inertial object from the mass matrix for a body B, about its center of mass Bcm,...
Definition: gz/math/Inertial.hh:60
bool SetMassMatrixRotation(const Quaternion< T > &_q, const T _tol=1e-6)
Set the MassMatrix rotation (eigenvectors of inertia matrix) without affecting the MOI in the base co...
Definition: gz/math/Inertial.hh:159
Inertial< double > Inertiald
Definition: gz/math/Inertial.hh:279
Matrix3< T > MOI() const
Get the moment of inertia matrix computer about the body's center of mass and expressed in this Inert...
Definition: gz/math/Inertial.hh:119
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: gz/math/Quaternion.hh:132
bool SetInertialRotation(const Quaternion< T > &_q)
Set the inertial pose rotation without affecting the MOI in the base coordinate frame.
Definition: gz/math/Inertial.hh:139
bool SetMassMatrix(const MassMatrix3< T > &_m, const T _tolerance=IGN_MASSMATRIX3_DEFAULT_TOLERANCE< T >)
Set the mass and inertia matrix.
Definition: gz/math/Inertial.hh:84
Inertial< T > & operator+=(const Inertial< T > &_inertial)
Adds inertial properties to current object. The mass, center of mass location, and inertia matrix are...
Definition: gz/math/Inertial.hh:207
T Y() const
Get the y value.
Definition: gz/math/Vector3.hh:661
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: gz/math/MassMatrix3.hh:45
virtual ~Inertial()
Destructor.
Definition: gz/math/Inertial.hh:72
bool operator!=(const Inertial< T > &_inertial) const
Inequality test operator.
Definition: gz/math/Inertial.hh:197
T pow(T... args)