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 
99  public: Inertial &operator=(const Inertial<T> &_inertial)
100  {
101  this->massMatrix = _inertial.MassMatrix();
102  this->pose = _inertial.Pose();
103 
104  return *this;
105  }
106 
111  public: bool operator==(const Inertial<T> &_inertial) const
112  {
113  return (this->pose == _inertial.Pose()) &&
114  (this->massMatrix == _inertial.MassMatrix());
115  }
116 
120  public: bool operator!=(const Inertial<T> &_inertial) const
121  {
122  return !(*this == _inertial);
123  }
124 
130  public: Inertial<T> &operator+=(const Inertial<T> &_inertial)
131  {
132  T m1 = this->massMatrix.Mass();
133  T m2 = _inertial.MassMatrix().Mass();
134 
135  // Total mass
136  T mass = m1 + m2;
137 
138  // Only continue if total mass is positive
139  if (mass <= 0)
140  {
141  return *this;
142  }
143 
144  auto com1 = this->Pose().Pos();
145  auto com2 = _inertial.Pose().Pos();
146  // New center of mass location in base frame
147  auto com = (m1*com1 + m2*com2) / mass;
148 
149  // Components of new moment of inertia matrix
150  Vector3<T> ixxyyzz;
151  Vector3<T> ixyxzyz;
152  // First add matrices in base frame
153  {
154  auto moi = this->MOI() + _inertial.MOI();
155  ixxyyzz = Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
156  ixyxzyz = Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
157  }
158  // Then account for parallel axis theorem
159  {
160  auto dc = com1 - com;
161  ixxyyzz.X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
162  ixxyyzz.Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
163  ixxyyzz.Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
164  ixxyyzz.X() -= m1 * dc[0] * dc[1];
165  ixxyyzz.Y() -= m1 * dc[0] * dc[2];
166  ixxyyzz.Z() -= m1 * dc[1] * dc[2];
167  }
168  {
169  auto dc = com2 - com;
170  ixxyyzz.X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
171  ixxyyzz.Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
172  ixxyyzz.Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
173  ixxyyzz.X() -= m2 * dc[0] * dc[1];
174  ixxyyzz.Y() -= m2 * dc[0] * dc[2];
175  ixxyyzz.Z() -= m2 * dc[1] * dc[2];
176  }
177  this->massMatrix = MassMatrix3<T>(mass, ixxyyzz, ixyxzyz);
178  this->pose = Pose3<T>(com, Quaternion<T>::Identity);
179 
180  return *this;
181  }
182 
188  public: const Inertial<T> operator+(const Inertial<T> &_inertial) const
189  {
190  return Inertial<T>(*this) += _inertial;
191  }
192 
195  private: MassMatrix3<T> massMatrix;
196 
199  private: Pose3<T> pose;
200  };
201 
204  }
205 }
206 #endif
const Inertial< T > operator+(const Inertial< T > &_inertial) const
Adds inertial properties to current object.
Definition: Inertial.hh:188
bool SetMassMatrix(const MassMatrix3< T > &_m)
Set the mass and inertia matrix.
Definition: Inertial.hh:58
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:130
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:30
T X() const
Get the x value.
Definition: Vector3.hh:639
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
bool operator!=(const Inertial< T > &_inertial) const
Inequality test operator.
Definition: Inertial.hh:120
Matrix3< T > MOI() const
Get the moment of inertia matrix expressed in the base coordinate frame.
Definition: Inertial.hh:90
T Y() const
Get the y value.
Definition: Vector3.hh:646
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:202
Inertial()
Default Constructor.
Definition: Inertial.hh:35
A 3x3 matrix class.
Definition: Matrix3.hh:35
virtual ~Inertial()
Destructor.
Definition: Inertial.hh:53
Inertial(const Inertial< T > &_inertial)
Copy constructor.
Definition: Inertial.hh:48
T Z() const
Get the z value.
Definition: Vector3.hh:653
Inertial & operator=(const Inertial< T > &_inertial)
Equal operator.
Definition: Inertial.hh:99
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
bool operator==(const Inertial< T > &_inertial) const
Equality comparison operator.
Definition: Inertial.hh:111
const MassMatrix3< T > & MassMatrix() const
Get the mass and inertia matrix.
Definition: Inertial.hh:66
const Pose3< T > & Pose() const
Get the pose of center of mass reference frame.
Definition: Inertial.hh:82
Definition: AffineException.hh:30
A quaternion class.
Definition: Matrix3.hh:30
Inertial(const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
Constructor.
Definition: Inertial.hh:41
Inertial< float > Inertialf
Definition: Inertial.hh:203