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 IGNITION_MATH_INERTIAL_HH_
18 #define IGNITION_MATH_INERTIAL_HH_
19 
20 #include <ignition/math/config.hh>
22 #include "ignition/math/Pose3.hh"
23 
24 namespace ignition
25 {
26  namespace math
27  {
28  inline namespace IGNITION_MATH_VERSION_NAMESPACE
29  {
34  template<typename T>
35  class Inertial
36  {
38  public: Inertial()
39  {}
40 
44  public: Inertial(const MassMatrix3<T> &_massMatrix,
45  const Pose3<T> &_pose)
46  : massMatrix(_massMatrix), pose(_pose)
47  {}
48 
51  public: Inertial(const Inertial<T> &_inertial)
52  : massMatrix(_inertial.MassMatrix()), pose(_inertial.Pose())
53  {}
54 
56  public: virtual ~Inertial() {}
57 
61  public: bool SetMassMatrix(const MassMatrix3<T> &_m)
62  {
63  this->massMatrix = _m;
64  return this->massMatrix.IsValid();
65  }
66 
69  public: const MassMatrix3<T> &MassMatrix() const
70  {
71  return this->massMatrix;
72  }
73 
77  public: bool SetPose(const Pose3<T> &_pose)
78  {
79  this->pose = _pose;
80  return this->massMatrix.IsValid();
81  }
82 
85  public: const Pose3<T> &Pose() const
86  {
87  return this->pose;
88  }
89 
93  public: Matrix3<T> MOI() const
94  {
95  auto R = Matrix3<T>(this->pose.Rot());
96  return R * this->massMatrix.MOI() * R.Transposed();
97  }
98 
103  public: bool SetInertialRotation(const Quaternion<T> &_q)
104  {
105  auto moi = this->MOI();
106  this->pose.Rot() = _q;
107  auto R = Matrix3<T>(_q);
108  return this->massMatrix.MOI(R.Transposed() * moi * R);
109  }
110 
123  public: bool SetMassMatrixRotation(const Quaternion<T> &_q,
124  const T _tol = 1e-6)
125  {
126  this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
127  _q.Inverse();
128  const auto moments = this->MassMatrix().PrincipalMoments(_tol);
129  const auto diag = Matrix3<T>(
130  moments[0], 0, 0,
131  0, moments[1], 0,
132  0, 0, moments[2]);
133  const auto R = Matrix3<T>(_q);
134  return this->massMatrix.MOI(R * diag * R.Transposed());
135  }
136 
140  public: Inertial &operator=(const Inertial<T> &_inertial)
141  {
142  this->massMatrix = _inertial.MassMatrix();
143  this->pose = _inertial.Pose();
144 
145  return *this;
146  }
147 
152  public: bool operator==(const Inertial<T> &_inertial) const
153  {
154  return (this->pose == _inertial.Pose()) &&
155  (this->massMatrix == _inertial.MassMatrix());
156  }
157 
161  public: bool operator!=(const Inertial<T> &_inertial) const
162  {
163  return !(*this == _inertial);
164  }
165 
171  public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
172  {
173  T m1 = this->massMatrix.Mass();
174  T m2 = _inertial.MassMatrix().Mass();
175 
176  // Total mass
177  T mass = m1 + m2;
178 
179  // Only continue if total mass is positive
180  if (mass <= 0)
181  {
182  return *this;
183  }
184 
185  auto com1 = this->Pose().Pos();
186  auto com2 = _inertial.Pose().Pos();
187  // New center of mass location in base frame
188  auto com = (m1*com1 + m2*com2) / mass;
189 
190  // Components of new moment of inertia matrix
191  Vector3<T> ixxyyzz;
192  Vector3<T> ixyxzyz;
193  // First add matrices in base frame
194  {
195  auto moi = this->MOI() + _inertial.MOI();
196  ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
197  ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
198  }
199  // Then account for parallel axis theorem
200  {
201  auto dc = com1 - com;
202  ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
203  ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
204  ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
205  ixxyyzz.X() -= m1 * dc[0] * dc[1];
206  ixxyyzz.Y() -= m1 * dc[0] * dc[2];
207  ixxyyzz.Z() -= m1 * dc[1] * dc[2];
208  }
209  {
210  auto dc = com2 - com;
211  ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
212  ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
213  ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
214  ixxyyzz.X() -= m2 * dc[0] * dc[1];
215  ixxyyzz.Y() -= m2 * dc[0] * dc[2];
216  ixxyyzz.Z() -= m2 * dc[1] * dc[2];
217  }
218  this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
219  this->pose = Pose3<T>(com, Quaternion<T>::Identity);
220 
221  return *this;
222  }
223 
229  public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
230  {
231  return Inertial<T>(*this) += _inertial;
232  }
233 
236  private: MassMatrix3<T> massMatrix;
237 
240  private: Pose3<T> pose;
241  };
242 
245  }
246  }
247 }
248 #endif
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Matrix3
A 3x3 matrix class.
Definition: Matrix3.hh:39
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::Inertial
Inertial()
Default Constructor.
Definition: Inertial.hh:38
ignition
Definition: Angle.hh:40
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::Pose
const Pose3< T > & Pose() const
Get the pose of center of mass reference frame.
Definition: Inertial.hh:85
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::MOI
Matrix3< T > MOI() const
Get the moment of inertia matrix expressed in the base coordinate frame.
Definition: Inertial.hh:93
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::SetMassMatrix
bool SetMassMatrix(const MassMatrix3< T > &_m)
Set the mass and inertia matrix.
Definition: Inertial.hh:61
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::Inertial
Inertial(const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
Constructor.
Definition: Inertial.hh:44
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::operator!=
bool operator!=(const Inertial< T > &_inertial) const
Inequality test operator.
Definition: Inertial.hh:161
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion< T >
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::operator=
Inertial & operator=(const Inertial< T > &_inertial)
Equal operator.
Definition: Inertial.hh:140
MassMatrix3.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::operator+
const Inertial< T > operator+(const Inertial< T > &_inertial) const
Adds inertial properties to current object.
Definition: Inertial.hh:229
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::operator==
bool operator==(const Inertial< T > &_inertial) const
Equality comparison operator.
Definition: Inertial.hh:152
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: Vector3.hh:40
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Inverse
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:131
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::SetPose
bool SetPose(const Pose3< T > &_pose)
Set the pose of center of mass reference frame.
Definition: Inertial.hh:77
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::operator+=
Inertial< T > & operator+=(const Inertial< T > &_inertial)
Adds inertial properties to current object.
Definition: Inertial.hh:171
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::SetMassMatrixRotation
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: Inertial.hh:123
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: MassMatrix3.hh:43
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertiald
Inertial< double > Inertiald
Definition: Inertial.hh:243
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::SetInertialRotation
bool SetInertialRotation(const Quaternion< T > &_q)
Set the inertial pose rotation without affecting the MOI in the base coordinate frame.
Definition: Inertial.hh:103
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Pose3
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:34
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::Inertial
Inertial(const Inertial< T > &_inertial)
Copy constructor.
Definition: Inertial.hh:51
Pose3.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertialf
Inertial< float > Inertialf
Definition: Inertial.hh:244
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::MassMatrix
const MassMatrix3< T > & MassMatrix() const
Get the mass and inertia matrix.
Definition: Inertial.hh:69
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial
A class for inertial information about a rigid body consisting of the scalar mass,...
Definition: Inertial.hh:36
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Inertial::~Inertial
virtual ~Inertial()
Destructor.
Definition: Inertial.hh:56
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::X
T X() const
Get the x value.
Definition: Vector3.hh:647
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Z
T Z() const
Get the z value.
Definition: Vector3.hh:661
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Y
T Y() const
Get the y value.
Definition: Vector3.hh:654