Gazebo Math
API Reference
8.1.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
src
gz-math
include
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 <optional>
21
22
#include <gz/math/config.hh>
23
#include "
gz/math/MassMatrix3.hh
"
24
#include "
gz/math/Matrix3.hh
"
25
#include "
gz/math/Matrix6.hh
"
26
#include "
gz/math/Pose3.hh
"
27
28
namespace
gz::math
29
{
30
// Inline bracket to help doxygen filtering.
31
inline
namespace
GZ_MATH_VERSION_NAMESPACE {
32
//
49
template
<
typename
T>
50
class
Inertial
51
{
53
public
:
Inertial
()
54
{}
55
65
public
:
Inertial
(
const
MassMatrix3<T>
&
_massMatrix
,
66
const
Pose3<T>
&
_pose
)
67
: massMatrix(
_massMatrix
), pose(
_pose
)
68
{}
69
81
public
:
Inertial
(
const
MassMatrix3<T>
&
_massMatrix
,
82
const
Pose3<T>
&
_pose
,
83
const
Matrix6<T>
&
_addedMass
)
84
: massMatrix(
_massMatrix
), pose(
_pose
), addedMass(
_addedMass
)
85
{}
86
97
public
:
bool
SetMassMatrix
(
const
MassMatrix3<T>
&
_m
,
98
const
T
_tolerance
=
GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>
)
99
{
100
this->massMatrix =
_m
;
101
return
this->massMatrix.IsValid(
_tolerance
);
102
}
103
108
public
:
const
MassMatrix3<T>
&
MassMatrix
()
const
109
{
110
return
this->massMatrix;
111
}
112
116
public
:
bool
SetPose
(
const
Pose3<T>
&
_pose
)
117
{
118
this->pose =
_pose
;
119
return
this->massMatrix.IsValid();
120
}
121
125
public
:
const
Pose3<T>
&
Pose
()
const
126
{
127
return
this->pose;
128
}
129
138
public
:
bool
SetFluidAddedMass
(
const
Matrix6<T>
&
_m
)
139
{
140
this->addedMass =
_m
;
141
return
this->addedMass.value() == this->addedMass.value().
Transposed
();
142
}
143
147
public
:
const
std::optional< Matrix6<T> > &
FluidAddedMass
()
const
148
{
149
return
this->addedMass;
150
}
151
157
public
:
Matrix3<T>
Moi
()
const
158
{
159
auto
R =
Matrix3<T>
(this->pose.Rot());
160
return
R * this->massMatrix.Moi() * R.Transposed();
161
}
162
178
public
:
Matrix6<T>
BodyMatrix
()
const
179
{
180
Matrix6<T>
result
;
181
182
result
.
SetSubmatrix
(
Matrix6<T>::TOP_LEFT
,
183
Matrix3<T>::Identity
* this->massMatrix.Mass());
184
185
result
.
SetSubmatrix
(
Matrix6<T>::BOTTOM_RIGHT
, this->Moi());
186
187
auto
x
= this->pose.Pos().X();
188
auto
y
= this->pose.Pos().Y();
189
auto
z
= this->pose.Pos().Z();
190
auto
skew
=
Matrix3<T>
(
191
0, -
z
,
y
,
192
z
, 0, -
x
,
193
-
y
,
x
, 0) * this->massMatrix.Mass();
194
result
.
SetSubmatrix
(
Matrix6<T>::BOTTOM_LEFT
,
skew
);
195
result
.
SetSubmatrix
(
Matrix6<T>::TOP_RIGHT
,
skew
.
Transposed
());
196
197
return
result
;
198
}
199
207
public
:
Matrix6<T>
SpatialMatrix
()
const
208
{
209
return
this->addedMass.has_value() ?
210
this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
211
}
212
217
public
:
bool
SetInertialRotation
(
const
Quaternion<T>
&
_q
)
218
{
219
auto
moi
= this->Moi();
220
this->pose.Rot() =
_q
;
221
auto
R =
Matrix3<T>
(
_q
);
222
return
this->massMatrix.SetMoi(R.Transposed() *
moi
* R);
223
}
224
237
public
:
bool
SetMassMatrixRotation
(
const
Quaternion<T>
&
_q
,
238
const
T
_tol
= 1
e
-6)
239
{
240
this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(
_tol
) *
241
_q
.Inverse();
242
const
auto
moments
= this->MassMatrix().PrincipalMoments(
_tol
);
243
const
auto
diag
=
Matrix3<T>
(
244
moments
[0], 0, 0,
245
0,
moments
[1], 0,
246
0, 0,
moments
[2]);
247
const
auto
R =
Matrix3<T>
(
_q
);
248
return
this->massMatrix.SetMoi(R *
diag
* R.
Transposed
());
249
}
250
255
public
:
bool
operator==
(
const
Inertial<T>
&
_inertial
)
const
256
{
257
return
(this->pose ==
_inertial
.Pose()) &&
258
(this->massMatrix ==
_inertial
.MassMatrix()) &&
259
(this->addedMass ==
_inertial
.FluidAddedMass());
260
}
261
265
public
:
bool
operator!=
(
const
Inertial<T>
&
_inertial
)
const
266
{
267
return
!(*
this
==
_inertial
);
268
}
269
275
public
:
Inertial<T>
&
operator+=
(
const
Inertial<T>
&
_inertial
)
276
{
277
T
m1
= this->massMatrix.Mass();
278
T
m2
=
_inertial
.MassMatrix().Mass();
279
280
// Total mass
281
T mass =
m1
+
m2
;
282
283
// Only continue if total mass is positive
284
if
(mass <= 0)
285
{
286
return
*
this
;
287
}
288
289
auto
com1
= this->Pose().Pos();
290
auto
com2
=
_inertial
.Pose().Pos();
291
// New center of mass location in base frame
292
auto
com
= (
m1
*
com1
+
m2
*
com2
) / mass;
293
294
// Components of new moment of inertia matrix
295
Vector3<T>
ixxyyzz
;
296
Vector3<T>
ixyxzyz
;
297
// First add matrices in base frame
298
{
299
auto
moi
= this->Moi() +
_inertial
.Moi();
300
ixxyyzz
=
Vector3<T>
(
moi
(0, 0),
moi
(1, 1),
moi
(2, 2));
301
ixyxzyz
=
Vector3<T>
(
moi
(0, 1),
moi
(0, 2),
moi
(1, 2));
302
}
303
// Then account for parallel axis theorem
304
{
305
auto
dc
=
com1
-
com
;
306
ixxyyzz
.X() +=
m1
* (
std::pow
(
dc
[1], 2) +
std::pow
(
dc
[2], 2));
307
ixxyyzz
.Y() +=
m1
* (
std::pow
(
dc
[2], 2) +
std::pow
(
dc
[0], 2));
308
ixxyyzz
.Z() +=
m1
* (
std::pow
(
dc
[0], 2) +
std::pow
(
dc
[1], 2));
309
ixyxzyz
.X() -=
m1
*
dc
[0] *
dc
[1];
310
ixyxzyz
.Y() -=
m1
*
dc
[0] *
dc
[2];
311
ixyxzyz
.Z() -=
m1
*
dc
[1] *
dc
[2];
312
}
313
{
314
auto
dc
=
com2
-
com
;
315
ixxyyzz
.X() +=
m2
* (
std::pow
(
dc
[1], 2) +
std::pow
(
dc
[2], 2));
316
ixxyyzz
.Y() +=
m2
* (
std::pow
(
dc
[2], 2) +
std::pow
(
dc
[0], 2));
317
ixxyyzz
.Z() +=
m2
* (
std::pow
(
dc
[0], 2) +
std::pow
(
dc
[1], 2));
318
ixyxzyz
.X() -=
m2
*
dc
[0] *
dc
[1];
319
ixyxzyz
.Y() -=
m2
*
dc
[0] *
dc
[2];
320
ixyxzyz
.Z() -=
m2
*
dc
[1] *
dc
[2];
321
}
322
this->massMatrix =
MassMatrix3<T>
(mass,
ixxyyzz
,
ixyxzyz
);
323
this->pose =
Pose3<T>
(
com
,
Quaternion<T>::Identity
);
324
325
return
*
this
;
326
}
327
333
public
:
Inertial<T>
&
operator-=
(
const
Inertial<T>
&
_inertial
)
334
{
335
T m = this->massMatrix.Mass();
336
T
m2
=
_inertial
.MassMatrix().Mass();
337
338
// Remaining mass
339
T
m1
= m -
m2
;
340
341
// Only continue if remaining mass is positive
342
if
(
m1
<= 0)
343
{
344
return
*
this
;
345
}
346
347
auto
com
= this->Pose().Pos();
348
auto
com2
=
_inertial
.Pose().Pos();
349
// Remaining center of mass location in base frame
350
auto
com1
= (m*
com
-
m2
*
com2
)/
m1
;
351
352
// Components of new moment of inertia matrix
353
Vector3<T>
ixxyyzz
;
354
Vector3<T>
ixyxzyz
;
355
356
// First subtract matrices in base frame
357
{
358
auto
moi
= this->Moi() -
_inertial
.Moi();
359
ixxyyzz
=
Vector3<T>
(
moi
(0, 0),
moi
(1, 1),
moi
(2, 2));
360
ixyxzyz
=
Vector3<T>
(
moi
(0, 1),
moi
(0, 2),
moi
(1, 2));
361
}
362
// Then account for parallel axis theorem
363
{
364
auto
dc
=
com1
-
com
;
365
ixxyyzz
.X() -=
m1
* (
std::pow
(
dc
[1], 2) +
std::pow
(
dc
[2], 2));
366
ixxyyzz
.Y() -=
m1
* (
std::pow
(
dc
[2], 2) +
std::pow
(
dc
[0], 2));
367
ixxyyzz
.Z() -=
m1
* (
std::pow
(
dc
[0], 2) +
std::pow
(
dc
[1], 2));
368
ixyxzyz
.X() +=
m1
*
dc
[0] *
dc
[1];
369
ixyxzyz
.Y() +=
m1
*
dc
[0] *
dc
[2];
370
ixyxzyz
.Z() +=
m1
*
dc
[1] *
dc
[2];
371
}
372
{
373
auto
dc
=
com2
-
com
;
374
ixxyyzz
.X() -=
m2
* (
std::pow
(
dc
[1], 2) +
std::pow
(
dc
[2], 2));
375
ixxyyzz
.Y() -=
m2
* (
std::pow
(
dc
[2], 2) +
std::pow
(
dc
[0], 2));
376
ixxyyzz
.Z() -=
m2
* (
std::pow
(
dc
[0], 2) +
std::pow
(
dc
[1], 2));
377
ixyxzyz
.X() +=
m2
*
dc
[0] *
dc
[1];
378
ixyxzyz
.Y() +=
m2
*
dc
[0] *
dc
[2];
379
ixyxzyz
.Z() +=
m2
*
dc
[1] *
dc
[2];
380
}
381
this->massMatrix =
MassMatrix3<T>
(
m1
,
ixxyyzz
,
ixyxzyz
);
382
this->pose =
Pose3<T>
(
com1
,
Quaternion<T>::Identity
);
383
384
return
*
this
;
385
}
386
392
public
:
const
Inertial<T>
operator+
(
const
Inertial<T>
&
_inertial
)
const
393
{
394
return
Inertial<T>
(*
this
) +=
_inertial
;
395
}
396
402
public
:
const
Inertial<T>
operator-
(
const
Inertial<T>
&
_inertial
)
const
403
{
404
return
Inertial<T>
(*
this
) -=
_inertial
;
405
}
406
409
private
:
MassMatrix3<T>
massMatrix;
410
413
private
:
Pose3<T>
pose;
414
416
private
: std::optional<Matrix6<T>> addedMass;
417
};
418
419
typedef
Inertial<double>
Inertiald
;
420
typedef
Inertial<float>
Inertialf
;
421
}
// namespace GZ_MATH_VERSION_NAMESPACE
422
}
// namespace gz::math
423
#endif
// GZ_MATH_INERTIAL_HH_