RigidBodyInertia.cpp
Go to the documentation of this file.
1 /*
2  * RDL - Robot Dynamics Library
3  * Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
4  *
5  * Licensed under the zlib license. See LICENSE for more details.
6  */
7 
8 #include "rdl_dynamics/RigidBodyInertia.hpp"
9 
10 namespace RobotDynamics
11 {
12 namespace Math
13 {
14 void RigidBodyInertia::operator+=(const RigidBodyInertia& rbi)
15 {
16  this->m += rbi.m;
17  this->h += rbi.h;
18  this->Ixx += rbi.Ixx;
19  this->Iyx += rbi.Iyx;
20  this->Iyy += rbi.Iyy;
21  this->Izx += rbi.Izx;
22  this->Izy += rbi.Izy;
23  this->Izz += rbi.Izz;
24 }
25 
26 void RigidBodyInertia::transform(const SpatialTransform& X)
27 {
28  double hx = h[0];
29  double hy = h[1];
30  double hz = h[2];
31 
32  double rx = X.r[0];
33  double ry = X.r[1];
34  double rz = X.r[2];
35 
36  double E00 = X.E(0, 0);
37  double E01 = X.E(0, 1);
38  double E02 = X.E(0, 2);
39  double E10 = X.E(1, 0);
40  double E11 = X.E(1, 1);
41  double E12 = X.E(1, 2);
42  double E20 = X.E(2, 0);
43  double E21 = X.E(2, 1);
44  double E22 = X.E(2, 2);
45 
46  double alpha = 2. * hx * rx;
47  double beta = 2. * hy * ry;
48  double gamma = 2. * hz * rz;
49  double epsilon = m * rx * rx;
50  double phi = m * ry * ry;
51  double psi = m * rz * rz;
52  double pi = m * rx * ry;
53  double a = m * rx * rz;
54  double b = m * ry * rz;
55  double c = hx * ry;
56  double d = hx * rz;
57  double e = hy * rz;
58  double f = hy * rx;
59  double g = hz * rx;
60  double j = hz * ry;
61 
62  double b00 = Ixx - gamma - beta + psi + phi;
63  double b01 = Iyx + f + c - pi;
64  double b02 = Izx + g + d - a;
65  double b11 = Iyy - gamma - alpha + psi + epsilon;
66  double b12 = Izy + j + e - b;
67  double b22 = Izz - beta - alpha + phi + epsilon;
68 
69  double w00 = E00 * b00 + E01 * b01 + E02 * b02;
70  double w01 = E00 * b01 + E01 * b11 + E02 * b12;
71  double w02 = E00 * b02 + E01 * b12 + E02 * b22;
72 
73  double w10 = E10 * b00 + E11 * b01 + E12 * b02;
74  double w11 = E10 * b01 + E11 * b11 + E12 * b12;
75  double w12 = E10 * b02 + E11 * b12 + E12 * b22;
76 
77  double w20 = E20 * b00 + E21 * b01 + E22 * b02;
78  double w21 = E20 * b01 + E21 * b11 + E22 * b12;
79  double w22 = E20 * b02 + E21 * b12 + E22 * b22;
80 
81  h = X.E * (h - m * X.r);
82  Ixx = w00 * E00 + w01 * E01 + w02 * E02;
83  Iyx = w00 * E10 + w01 * E11 + w02 * E12;
84  Iyy = w10 * E10 + w11 * E11 + w12 * E12;
85  Izx = w00 * E20 + w01 * E21 + w02 * E22;
86  Izy = w10 * E20 + w11 * E21 + w12 * E22;
87  Izz = w20 * E20 + w21 * E21 + w22 * E22;
88 }
89 
90 void RigidBodyInertia::createFromMatrix(const SpatialMatrix& Ic)
91 {
92  m = Ic(3, 3);
93  h.set(-Ic(1, 5), Ic(0, 5), -Ic(0, 4));
94  Ixx = Ic(0, 0);
95  Iyx = Ic(1, 0);
96  Iyy = Ic(1, 1);
97  Izx = Ic(2, 0);
98  Izy = Ic(2, 1);
99  Izz = Ic(2, 2);
100 }
101 
102 SpatialMatrix RigidBodyInertia::toMatrix() const
103 {
104  SpatialMatrix result;
105  result(0, 0) = Ixx;
106  result(0, 1) = Iyx;
107  result(0, 2) = Izx;
108  result(1, 0) = Iyx;
109  result(1, 1) = Iyy;
110  result(1, 2) = Izy;
111  result(2, 0) = Izx;
112  result(2, 1) = Izy;
113  result(2, 2) = Izz;
114 
115  result.block<3, 3>(0, 3) = toTildeForm(h);
116  result.block<3, 3>(3, 0) = -toTildeForm(h);
117  result.block<3, 3>(3, 3) = Matrix3d::Identity(3, 3) * m;
118 
119  return result;
120 }
121 
122 void RigidBodyInertia::setSpatialMatrix(SpatialMatrix& mat) const
123 {
124  mat(0, 0) = Ixx;
125  mat(0, 1) = Iyx;
126  mat(0, 2) = Izx;
127  mat(1, 0) = Iyx;
128  mat(1, 1) = Iyy;
129  mat(1, 2) = Izy;
130  mat(2, 0) = Izx;
131  mat(2, 1) = Izy;
132  mat(2, 2) = Izz;
133 
134  mat(3, 0) = 0.;
135  mat(3, 1) = h[2];
136  mat(3, 2) = -h[1];
137  mat(4, 0) = -h[2];
138  mat(4, 1) = 0.;
139  mat(4, 2) = h[0];
140  mat(5, 0) = h[1];
141  mat(5, 1) = -h[0];
142  mat(5, 2) = 0.;
143 
144  mat(0, 3) = 0.;
145  mat(0, 4) = -h[2];
146  mat(0, 5) = h[1];
147  mat(1, 3) = h[2];
148  mat(1, 4) = 0.;
149  mat(1, 5) = -h[0];
150  mat(2, 3) = -h[1];
151  mat(2, 4) = h[0];
152  mat(2, 5) = 0.;
153 
154  mat(3, 3) = m;
155  mat(3, 4) = 0.;
156  mat(3, 5) = 0.;
157  mat(4, 3) = 0.;
158  mat(4, 4) = m;
159  mat(4, 5) = 0.;
160  mat(5, 3) = 0.;
161  mat(5, 4) = 0.;
162  mat(5, 5) = m;
163 }
164 } // namespace Math
165 } // namespace RobotDynamics
RobotDynamics::Math::RigidBodyInertia::operator+=
void operator+=(const RigidBodyInertia &rbi)
Overloaded plus-equals operator. Adds two inertia matrices.
Definition: RigidBodyInertia.cpp:14
RigidBodyInertia.hpp
See V. Duindum p39-40 & Featherstone p32-33.
RobotDynamics::Math::toTildeForm
static Matrix3d toTildeForm(const Point3d &p)
Definition: Point3.hpp:370
RobotDynamics::Math::RigidBodyInertia::createFromMatrix
void createFromMatrix(const SpatialMatrix &Ic)
Create a Math::RigidBodyInertia object from a 6x6 Math::SpatialMatrix.
Definition: RigidBodyInertia.cpp:90
RobotDynamics::Math::SpatialTransform
Compact representation of spatial transformations.
Definition: rdl_eigenmath.h:337
RobotDynamics::Math::RigidBodyInertia
This class stores a bodies mass, center of mass, and inertia information. The inertia elements are st...
Definition: RigidBodyInertia.hpp:45
RobotDynamics::Math::RigidBodyInertia::transform
void transform(const SpatialTransform &X)
Transform a Math::RigidBodyInertia matrix.
Definition: RigidBodyInertia.cpp:26
RobotDynamics::Math::Vector3d::set
void set(const Eigen::Vector3d &v)
Definition: rdl_eigenmath.h:83
RobotDynamics::Math::RigidBodyInertia::setSpatialMatrix
void setSpatialMatrix(SpatialMatrix &mat) const
Store a Math::RigidBodyInertia in the Math::SpatialMatrix.
Definition: RigidBodyInertia.cpp:122
RobotDynamics
Namespace for all structures of the RobotDynamics library.
Definition: Body.h:21

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:28