Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // RDL - Robot Dynamics Library
3 : : // Licensed under the zlib license. See LICENSE for more details.
4 : :
5 : : #ifndef __RDL_RIGID_BODY_INERTIA_HPP__
6 : : #define __RDL_RIGID_BODY_INERTIA_HPP__
7 : :
8 : : /**
9 : : * @file RigidBodyInertia.hpp
10 : : * @brief See V. Duindum p39-40 & Featherstone p32-33
11 : : */
12 : :
13 : : #include "rdl_dynamics/rdl_eigenmath.hpp"
14 : :
15 : : namespace RobotDynamics
16 : : {
17 : : namespace Math
18 : : {
19 : : /**
20 : : * @class RigidBodyInertia
21 : : * @brief This class stores a bodies mass, center of mass, and inertia information. The inertia elements are
22 : : * stored individually since the inertia matrix is a 3x3 symmetric matrix. The bodies inertia matrix, expressed
23 : : * about its center of mass, can be reconstructed as
24 : : * \f[
25 : : * I_c =
26 : : * \begin{bmatrix}
27 : : * I_xx & I_yx & I_zx \\
28 : : * I_yx & I_yy & I_zy \\
29 : : * I_zx & I_zy & I_zz
30 : : * \end{bmatrix}
31 : : * \f]
32 : : * The full RigidBodyInertia matrix has the following structure,
33 : : * \f[
34 : : * \begin{bmatrix}
35 : : * I_c + (h\times)h & h\times \\
36 : : * -h\times & \mathbf{1}_{3\times3}m
37 : : * \end{bmatrix}
38 : : * \f]
39 : : * where \f$ \mathbf{1}_{3\times 3} \f$ is a 3x3 identity matrix
40 : : */
41 : : class RigidBodyInertia : public TransformableGeometricObject
42 : : {
43 : : public:
44 : : /**
45 : : * @brief Constructor
46 : : */
47 : 14549 : RigidBodyInertia() : m(0.), h(Vector3d::Zero(3, 1)), Ixx(0.), Iyx(0.), Iyy(0.), Izx(0.), Izy(0.), Izz(0.)
48 : : {
49 : 14549 : }
50 : :
51 : : /**
52 : : * @brief Constructor
53 : : * @param mass Body mass
54 : : * @param com_mass Vector pointing to center of mass scaled by the body mass
55 : : * @param inertia 3x3 Inertia tensor about the bodies center of mass
56 : : */
57 : 11464 : RigidBodyInertia(double mass, const Vector3d& com_mass, const Matrix3d& inertia)
58 : 11464 : : m(mass), h(com_mass), Ixx(inertia(0, 0)), Iyx(inertia(1, 0)), Iyy(inertia(1, 1)), Izx(inertia(2, 0)), Izy(inertia(2, 1)), Izz(inertia(2, 2))
59 : : {
60 : 11464 : }
61 : :
62 : : /**
63 : : * @brief Constructor
64 : : * @param m Body mass
65 : : * @param h Vector pointing to center of mass scaled by the body mass
66 : : * @param Ixx
67 : : * @param Iyx
68 : : * @param Iyy
69 : : * @param Izx
70 : : * @param Izy
71 : : * @param Izz
72 : : */
73 : 8 : RigidBodyInertia(double m, const Vector3d& h, const double Ixx, const double Iyx, const double Iyy, const double Izx, const double Izy, const double Izz)
74 : 8 : : m(m), h(h), Ixx(Ixx), Iyx(Iyx), Iyy(Iyy), Izx(Izx), Izy(Izy), Izz(Izz)
75 : : {
76 : 8 : }
77 : :
78 : : /**
79 : : * @brief Copy constructor
80 : : * @param inertia
81 : : */
82 : 61076 : RigidBodyInertia(const RigidBodyInertia& inertia)
83 : 61076 : : m(inertia.m), h(inertia.h), Ixx(inertia.Ixx), Iyx(inertia.Iyx), Iyy(inertia.Iyy), Izx(inertia.Izx), Izy(inertia.Izy), Izz(inertia.Izz)
84 : : {
85 : 61076 : }
86 : :
87 : : /**
88 : : * @brief Setter
89 : : * @param I
90 : : */
91 : 969 : inline void set(const RigidBodyInertia& I)
92 : : {
93 : 969 : this->set(I.m, I.h, I.Ixx, I.Iyx, I.Iyy, I.Izx, I.Izy, I.Izz);
94 : 969 : }
95 : :
96 : : /**
97 : : * @brief Setter
98 : : * @param m Body mass
99 : : * @param h Vector pointing to center of mass scaled by the body mass
100 : : * @param Ixx
101 : : * @param Iyx
102 : : * @param Iyy
103 : : * @param Izx
104 : : * @param Izy
105 : : * @param Izz
106 : : */
107 : 969 : void set(double m, const Vector3d& h, const double Ixx, const double Iyx, const double Iyy, const double Izx, const double Izy, const double Izz)
108 : : {
109 : 969 : this->m = m;
110 : 969 : this->h = h;
111 : 969 : this->Ixx = Ixx;
112 : 969 : this->Iyx = Iyx;
113 : 969 : this->Iyy = Iyy;
114 : 969 : this->Izx = Izx;
115 : 969 : this->Izy = Izy;
116 : 969 : this->Izz = Izz;
117 : 969 : }
118 : :
119 : : /**
120 : : * @brief Overloaded plus-equals operator. Adds two inertia matrices
121 : : * @param rbi
122 : : */
123 : : void operator+=(const RigidBodyInertia& rbi);
124 : :
125 : : /**
126 : : * @brief Transform a Math::RigidBodyInertia matrix
127 : : * @param X
128 : : *
129 : : */
130 : : void transform(const SpatialTransform& X);
131 : :
132 : : /**
133 : : * @brief Copy, transform, and return a Math::RigidBodyInertia
134 : : * @param X
135 : : * @return Returns a copied and transformed Math::RigidBodyInertia
136 : : */
137 : 2553 : inline RigidBodyInertia transform_copy(const SpatialTransform& X) const
138 : : {
139 : 2553 : RigidBodyInertia I = *this;
140 : 2553 : I.transform(X);
141 : 2553 : return I;
142 : : }
143 : :
144 : : /**
145 : : * @brief Create a Math::RigidBodyInertia object from a 6x6 Math::SpatialMatrix
146 : : * @param Ic
147 : : */
148 : : void createFromMatrix(const SpatialMatrix& Ic);
149 : :
150 : : /**
151 : : * Get a Math::RigidBodyInertia as a 6x6 Math::SpatialMatrix
152 : : * @return A 6x6 matrix representing a Math::SpatialInertia matrix
153 : : */
154 : : SpatialMatrix toMatrix() const;
155 : :
156 : : /**
157 : : * @brief Store a Math::RigidBodyInertia in the Math::SpatialMatrix
158 : : * @param mat Modified to store a Math::RigidBodyInertia in the Math::SpatialMatrix
159 : : */
160 : : void setSpatialMatrix(SpatialMatrix& mat) const;
161 : :
162 : : /**
163 : : * @brief Given Math::RigidBodyInertia \f$ I_r \f$ ad Math::SpatialMatrix \f$ M_I \f$, returns Math::SpatialMatrix
164 : : **\f$ M_r \f$
165 : : * such that \f$ M_r = I_r - M_I \f$
166 : : * @param m
167 : : * @return New Math::SpatialMatrix that is the Math::SpatialMatrix subtracted from a Math::RigidBodyInertia
168 : : */
169 : 1 : inline SpatialMatrix subtractSpatialMatrix(const SpatialMatrix& m) const
170 : : {
171 : 1 : return SpatialMatrix(Ixx - m(0, 0), Iyx - m(0, 1), Izx - m(0, 2), -m(0, 3), -h[2] - m(0, 4), h[1] - m(0, 5), Iyx - m(1, 0), Iyy - m(1, 1), Izy - m(1, 2),
172 : 1 : h[2] - m(1, 3), -m(1, 4), -h[0] - m(1, 5), Izx - m(2, 0), Izy - m(2, 1), Izz - m(2, 2), -h[1] - m(2, 3), h[0] - m(2, 4), -m(2, 5), -m(3, 0),
173 : 1 : h[2] - m(3, 1), -h[1] - m(3, 2), this->m - m(3, 3), -m(3, 4), -m(3, 5), -h[2] - m(4, 0), -m(4, 1), h[0] - m(4, 2), -m(4, 3),
174 : 3 : this->m - m(4, 4), -m(4, 5), h[1] - m(5, 0), -h[0] - m(5, 1), -m(5, 2), -m(5, 3), -m(5, 4), this->m - m(5, 5));
175 : : }
176 : :
177 : : /**
178 : : * @brief Multiply a Math::RigidBodyInertia by a Math::SpatialVector and return the result as a new
179 : : **Math::SpatialVector
180 : : * @param v
181 : : * @return The result of a Math::RigidBodyInertia multiplied by a Math::SpatialVector
182 : : */
183 : 1475 : inline SpatialVector timesSpatialVector(const SpatialVector& v) const
184 : : {
185 : 1475 : return SpatialVector(Ixx * v[0] + Iyx * v[1] + Izx * v[2] + h[1] * v[5] - h[2] * v[4], Iyx * v[0] + Iyy * v[1] + Izy * v[2] - h[0] * v[5] + h[2] * v[3],
186 : 1475 : Izx * v[0] + Izy * v[1] + Izz * v[2] + h[0] * v[4] - h[1] * v[3], -h[1] * v[2] + h[2] * v[1] + m * v[3],
187 : 4425 : h[0] * v[2] - h[2] * v[0] + m * v[4], -h[0] * v[1] + h[1] * v[0] + m * v[5]);
188 : : }
189 : :
190 : : /**
191 : : * @brief A helper method that returns a 6x3 matrix that is a Math::RigidBodyInertia multiplied by a 6x3 matrix
192 : : * @param m
193 : : * @return The result of multiplying a Math::RigidBodyInertia by a 6x3 matrix
194 : : */
195 : 1 : Matrix63 multiplyMatrix63(Matrix63 m) const
196 : : {
197 : 1 : m.col(0) = this->timesSpatialVector(m.col(0));
198 : 1 : m.col(1) = this->timesSpatialVector(m.col(1));
199 : 1 : m.col(2) = this->timesSpatialVector(m.col(2));
200 : 1 : return m;
201 : : }
202 : :
203 : 5962 : RigidBodyInertia& operator=(const RigidBodyInertia& I)
204 : : {
205 : 5962 : m = I.m;
206 : 5962 : h = I.h;
207 : 5962 : Ixx = I.Ixx;
208 : 5962 : Iyx = I.Iyx;
209 : 5962 : Iyy = I.Iyy;
210 : 5962 : Izx = I.Izx;
211 : 5962 : Izy = I.Izy;
212 : 5962 : Izz = I.Izz;
213 : 5962 : return *this;
214 : : }
215 : :
216 : : double m; /**< Mass */
217 : :
218 : : Vector3d h; /**< Vector pointing to body center of mass in body frame, scaled by body mass */
219 : :
220 : : double Ixx; /**< Element in body's inertia matrix */
221 : : double Iyx; /**< Element in body's inertia matrix */
222 : : double Iyy; /**< Element in body's inertia matrix */
223 : : double Izx; /**< Element in body's inertia matrix */
224 : : double Izy; /**< Element in body's inertia matrix */
225 : : double Izz; /**< Element in body's inertia matrix */
226 : : };
227 : :
228 : : /**
229 : : * @brief Add two Math::RigidBodyInertia objects together
230 : : * @param rbi_in A copy that is modified and returned.
231 : : * @param rbi
232 : : * @return A Math::RigidBodyInertia that is the addition of the two arguments
233 : : */
234 : 2554 : inline RigidBodyInertia operator+(RigidBodyInertia rbi_in, const RigidBodyInertia& rbi)
235 : : {
236 : 2554 : rbi_in += rbi;
237 : 2554 : return rbi_in;
238 : : }
239 : :
240 : : /**
241 : : * @brief Operator for multiplying a Math::RigidBodyInertia by a Math::SpatialVector
242 : : * @param I
243 : : * @param v
244 : : * @return Math::RigidBodyInertia times Math::SpatialVector
245 : : */
246 : 2 : inline SpatialVector operator*(const RigidBodyInertia& I, const SpatialVector& v)
247 : : {
248 : 2 : return I.timesSpatialVector(v);
249 : : }
250 : :
251 : : /**
252 : : * @brief Operator for multiplying a Math::RigidBodyInertia by a Math::Matrix63
253 : : * @param I
254 : : * @param m
255 : : * @return The result of multiplying a Math::RigidBodyInertia by a Math::Matrix63
256 : : */
257 : 1 : inline Matrix63 operator*(const RigidBodyInertia& I, const Matrix63& m)
258 : : {
259 : 1 : return I.multiplyMatrix63(m);
260 : : }
261 : :
262 : : /**
263 : : * Creates a RigidBodyInertia from a body's mass, center of mass location, and 3x3 inertia matrix
264 : : * @param mass Body mass
265 : : * @param com Center of mass location in body frame
266 : : * @param inertia_C 3x3 inertia tensor about from located at center of mass and aligned with body frame
267 : : * @return RigidBodyInertia
268 : : */
269 : 12613 : static inline RigidBodyInertia createFromMassComInertiaC(double mass, const Vector3d& com, const Matrix3d& inertia_C)
270 : : {
271 : 12613 : RigidBodyInertia result;
272 : :
273 : 12613 : result.m = mass;
274 : 12613 : result.h = com * mass;
275 : 12613 : Matrix3d I = inertia_C + com.toTildeForm() * com.toTildeForm().transpose() * mass;
276 : 12613 : result.Ixx = I(0, 0);
277 : 12613 : result.Iyx = I(1, 0);
278 : 12613 : result.Iyy = I(1, 1);
279 : 12613 : result.Izx = I(2, 0);
280 : 12613 : result.Izy = I(2, 1);
281 : 12613 : result.Izz = I(2, 2);
282 : :
283 : 12613 : return RigidBodyInertia(mass, com * mass, inertia_C + com.toTildeForm() * com.toTildeForm().transpose() * mass);
284 : : }
285 : : typedef std::vector<RigidBodyInertia, Eigen::aligned_allocator<RigidBodyInertia>> RigidBodyInertiaV;
286 : : } // namespace Math
287 : : } // namespace RobotDynamics
288 : :
289 : : #endif //__SPATIAL_INERTIA_HPP__
|
