Loading...
Searching...
No Matches
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>
23
24namespace ignition
25{
26 namespace math
27 {
28 inline namespace IGNITION_MATH_VERSION_NAMESPACE
29 {
34 template<typename T>
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
A class for inertial information about a rigid body consisting of the scalar mass,...
Definition Inertial.hh:36
bool operator==(const Inertial< T > &_inertial) const
Equality comparison operator.
Definition Inertial.hh:152
Matrix3< T > MOI() const
Get the moment of inertia matrix expressed in the base coordinate frame.
Definition Inertial.hh:93
Inertial()
Default Constructor.
Definition Inertial.hh:38
const Pose3< T > & Pose() const
Get the pose of center of mass reference frame.
Definition Inertial.hh:85
virtual ~Inertial()
Destructor.
Definition Inertial.hh:56
bool operator!=(const Inertial< T > &_inertial) const
Inequality test operator.
Definition Inertial.hh:161
const MassMatrix3< T > & MassMatrix() const
Get the mass and inertia matrix.
Definition Inertial.hh:69
Inertial< T > & operator+=(const Inertial< T > &_inertial)
Adds inertial properties to current object.
Definition Inertial.hh:171
Inertial & operator=(const Inertial< T > &_inertial)
Equal operator.
Definition Inertial.hh:140
Inertial(const Inertial< T > &_inertial)
Copy constructor.
Definition Inertial.hh:51
bool SetInertialRotation(const Quaternion< T > &_q)
Set the inertial pose rotation without affecting the MOI in the base coordinate frame.
Definition Inertial.hh:103
bool SetMassMatrix(const MassMatrix3< T > &_m)
Set the mass and inertia matrix.
Definition Inertial.hh:61
Inertial(const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
Constructor.
Definition Inertial.hh:44
bool SetPose(const Pose3< T > &_pose)
Set the pose of center of mass reference frame.
Definition Inertial.hh:77
const Inertial< T > operator+(const Inertial< T > &_inertial) const
Adds inertial properties to current object.
Definition Inertial.hh:229
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
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition MassMatrix3.hh:43
A 3x3 matrix class.
Definition Quaternion.hh:32
Encapsulates a position and rotation in three space.
Definition Pose3.hh:34
A quaternion class.
Definition Quaternion.hh:38
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition Quaternion.hh:131
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
T Z() const
Get the z value.
Definition Vector3.hh:661
T Y() const
Get the y value.
Definition Vector3.hh:654
T X() const
Get the x value.
Definition Vector3.hh:647
Inertial< double > Inertiald
Definition Inertial.hh:243
Inertial< float > Inertialf
Definition Inertial.hh:244
Definition Angle.hh:40