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 
21 #include "ignition/math/Pose3.hh"
22 
23 namespace ignition
24 {
25  namespace math
26  {
31  template<typename T>
32  class Inertial
33  {
35  public: Inertial()
36  {}
37 
41  public: Inertial(const MassMatrix3<T> &_massMatrix,
42  const Pose3<T> &_pose)
43  : massMatrix(_massMatrix), pose(_pose)
44  {}
45 
48  public: Inertial(const Inertial<T> &_inertial)
49  : massMatrix(_inertial.MassMatrix()), pose(_inertial.Pose())
50  {}
51 
53  public: virtual ~Inertial() {}
54 
58  public: bool SetMassMatrix(const MassMatrix3<T> &_m)
59  {
60  this->massMatrix = _m;
61  return this->massMatrix.IsValid();
62  }
63 
66  public: const MassMatrix3<T> &MassMatrix() const
67  {
68  return this->massMatrix;
69  }
70 
74  public: bool SetPose(const Pose3<T> &_pose)
75  {
76  this->pose = _pose;
77  return this->massMatrix.IsValid();
78  }
79 
82  public: const Pose3<T> &Pose() const
83  {
84  return this->pose;
85  }
86 
90  public: Matrix3<T> MOI() const
91  {
92  auto R = Matrix3<T>(this->pose.Rot());
93  return R * this->massMatrix.MOI() * R.Transposed();
94  }
95 
100  public: bool SetInertialRotation(const Quaternion<T> &_q)
101  {
102  auto moi = this->MOI();
103  this->pose.Rot() = _q;
104  auto R = Matrix3<T>(_q);
105  return this->massMatrix.MOI(R.Transposed() * moi * R);
106  }
107 
120  public: bool SetMassMatrixRotation(const Quaternion<T> &_q,
121  const T _tol = 1e-6)
122  {
123  this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
124  _q.Inverse();
125  const auto moments = this->MassMatrix().PrincipalMoments(_tol);
126  const auto diag = Matrix3<T>(
127  moments[0], 0, 0,
128  0, moments[1], 0,
129  0, 0, moments[2]);
130  const auto R = Matrix3<T>(_q);
131  return this->massMatrix.MOI(R * diag * R.Transposed());
132  }
133 
137  public: Inertial &operator=(const Inertial<T> &_inertial)
138  {
139  this->massMatrix = _inertial.MassMatrix();
140  this->pose = _inertial.Pose();
141 
142  return *this;
143  }
144 
149  public: bool operator==(const Inertial<T> &_inertial) const
150  {
151  return (this->pose == _inertial.Pose()) &&
152  (this->massMatrix == _inertial.MassMatrix());
153  }
154 
158  public: bool operator!=(const Inertial<T> &_inertial) const
159  {
160  return !(*this == _inertial);
161  }
162 
168  public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
169  {
170  T m1 = this->massMatrix.Mass();
171  T m2 = _inertial.MassMatrix().Mass();
172 
173  // Total mass
174  T mass = m1 + m2;
175 
176  // Only continue if total mass is positive
177  if (mass <= 0)
178  {
179  return *this;
180  }
181 
182  auto com1 = this->Pose().Pos();
183  auto com2 = _inertial.Pose().Pos();
184  // New center of mass location in base frame
185  auto com = (m1*com1 + m2*com2) / mass;
186 
187  // Components of new moment of inertia matrix
188  Vector3<T> ixxyyzz;
189  Vector3<T> ixyxzyz;
190  // First add matrices in base frame
191  {
192  auto moi = this->MOI() + _inertial.MOI();
193  ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
194  ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
195  }
196  // Then account for parallel axis theorem
197  {
198  auto dc = com1 - com;
199  ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
200  ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
201  ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
202  ixxyyzz.X() -= m1 * dc[0] * dc[1];
203  ixxyyzz.Y() -= m1 * dc[0] * dc[2];
204  ixxyyzz.Z() -= m1 * dc[1] * dc[2];
205  }
206  {
207  auto dc = com2 - com;
208  ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
209  ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
210  ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
211  ixxyyzz.X() -= m2 * dc[0] * dc[1];
212  ixxyyzz.Y() -= m2 * dc[0] * dc[2];
213  ixxyyzz.Z() -= m2 * dc[1] * dc[2];
214  }
215  this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
216  this->pose = Pose3<T>(com, Quaternion<T>::Identity);
217 
218  return *this;
219  }
220 
226  public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
227  {
228  return Inertial<T>(*this) += _inertial;
229  }
230 
233  private: MassMatrix3<T> massMatrix;
234 
237  private: Pose3<T> pose;
238  };
239 
242  }
243 }
244 #endif
T X() const
Get the x value.
Definition: Vector3.hh:635
const MassMatrix3< T > & MassMatrix() const
Get the mass and inertia matrix.
Definition: Inertial.hh:66
bool SetInertialRotation(const Quaternion< T > &_q)
Set the inertial pose rotation without affecting the MOI in the base coordinate frame.
Definition: Inertial.hh:100
bool SetMassMatrix(const MassMatrix3< T > &_m)
Set the mass and inertia matrix.
Definition: Inertial.hh:58
bool operator==(const Inertial< T > &_inertial) const
Equality comparison operator.
Definition: Inertial.hh:149
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: MassMatrix3.hh:39
Inertial< T > & operator+=(const Inertial< T > &_inertial)
Adds inertial properties to current object.
Definition: Inertial.hh:168
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:30
A class for inertial information about a rigid body consisting of the scalar mass, a 3x3 symmetric moment of inertia matrix, and center of mass reference frame pose.
Definition: Inertial.hh:32
Matrix3< T > MOI() const
Get the moment of inertia matrix expressed in the base coordinate frame.
Definition: Inertial.hh:90
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:120
bool SetPose(const Pose3< T > &_pose)
Set the pose of center of mass reference frame.
Definition: Inertial.hh:74
Inertial< double > Inertiald
Definition: Inertial.hh:240
Inertial()
Default Constructor.
Definition: Inertial.hh:35
A 3x3 matrix class.
Definition: Matrix3.hh:35
virtual ~Inertial()
Destructor.
Definition: Inertial.hh:53
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix3.hh:475
T Y() const
Get the y value.
Definition: Vector3.hh:642
Inertial(const Inertial< T > &_inertial)
Copy constructor.
Definition: Inertial.hh:48
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:128
Inertial & operator=(const Inertial< T > &_inertial)
Equal operator.
Definition: Inertial.hh:137
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
const Pose3< T > & Pose() const
Get the pose of center of mass reference frame.
Definition: Inertial.hh:82
const Inertial< T > operator+(const Inertial< T > &_inertial) const
Adds inertial properties to current object.
Definition: Inertial.hh:226
T Z() const
Get the z value.
Definition: Vector3.hh:649
Definition: Angle.hh:38
A quaternion class.
Definition: Matrix3.hh:30
Inertial(const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
Constructor.
Definition: Inertial.hh:41
bool operator!=(const Inertial< T > &_inertial) const
Inequality test operator.
Definition: Inertial.hh:158
Inertial< float > Inertialf
Definition: Inertial.hh:241