Rigid Body Dynamics Library
SpatialAlgebraOperators.h
Go to the documentation of this file.
1/*
2 * RBDL - Rigid Body Dynamics Library
3 * Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
4 *
5 * Licensed under the zlib license. See LICENSE for more details.
6 */
7
8#ifndef RBDL_SPATIALALGEBRAOPERATORS_H
9#define RBDL_SPATIALALGEBRAOPERATORS_H
10
11#include <iostream>
12//#include <cmath>
13
14namespace RigidBodyDynamics {
15
16namespace Math {
17
18inline Matrix3d VectorCrossMatrix (const Vector3d &vector) {
19 return Matrix3d (
20 0., -vector[2], vector[1],
21 vector[2], 0., -vector[0],
22 -vector[1], vector[0], 0.
23 );
24}
25
27struct RBDL_DLLAPI SpatialRigidBodyInertia {
29 m (0.),
30 h (Vector3d::Zero()),
31 Ixx (0.), Iyx(0.), Iyy(0.), Izx(0.), Izy(0.), Izz(0.)
32 {}
34 Scalar mass, const Vector3d &com_mass, const Matrix3d &inertia) :
35 m (mass), h (com_mass),
36 Ixx (inertia(0,0)),
37 Iyx (inertia(1,0)), Iyy(inertia(1,1)),
38 Izx (inertia(2,0)), Izy(inertia(2,1)), Izz(inertia(2,2))
39 { }
41 const Scalar &Ixx,
42 const Scalar &Iyx, const Scalar &Iyy,
43 const Scalar &Izx, const Scalar &Izy, const Scalar &Izz
44 ) :
45 m (m), h (h),
46 Ixx (Ixx),
47 Iyx (Iyx), Iyy(Iyy),
48 Izx (Izx), Izy(Izy), Izz(Izz)
49 { }
50
52 Vector3d mv_lower (mv[3], mv[4], mv[5]);
53
54 Vector3d res_upper = Vector3d (
55 Ixx * mv[0] + Iyx * mv[1] + Izx * mv[2],
56 Iyx * mv[0] + Iyy * mv[1] + Izy * mv[2],
57 Izx * mv[0] + Izy * mv[1] + Izz * mv[2]
58 ) + h.cross(mv_lower);
59 Vector3d res_lower = m * mv_lower - h.cross (Vector3d (mv[0], mv[1], mv[2]));
60
61 return SpatialVector (
62 res_upper[0], res_upper[1], res_upper[2],
63 res_lower[0], res_lower[1], res_lower[2]
64 );
65 }
66
69 m + rbi.m,
70 h + rbi.h,
71 Ixx + rbi.Ixx,
72 Iyx + rbi.Iyx, Iyy + rbi.Iyy,
73 Izx + rbi.Izx, Izy + rbi.Izy, Izz + rbi.Izz
74 );
75 }
76
78 m = Ic(3,3);
79 h.set (-Ic(1,5), Ic(0,5), -Ic(0,4));
80 Ixx = Ic(0,0);
81 Iyx = Ic(1,0); Iyy = Ic(1,1);
82 Izx = Ic(2,0); Izy = Ic(2,1); Izz = Ic(2,2);
83 }
84
86 SpatialMatrix result;
87 result(0,0) = Ixx; result(0,1) = Iyx; result(0,2) = Izx;
88 result(1,0) = Iyx; result(1,1) = Iyy; result(1,2) = Izy;
89 result(2,0) = Izx; result(2,1) = Izy; result(2,2) = Izz;
90
91 result.block<3,3>(0,3) = VectorCrossMatrix(h);
92 result.block<3,3>(3,0) = - VectorCrossMatrix(h);
93 result.block<3,3>(3,3) = Matrix3d::Identity() * m;
94
95 return result;
96 }
97
98 void setSpatialMatrix (SpatialMatrix &mat) const {
99 mat(0,0) = Ixx; mat(0,1) = Iyx; mat(0,2) = Izx;
100 mat(1,0) = Iyx; mat(1,1) = Iyy; mat(1,2) = Izy;
101 mat(2,0) = Izx; mat(2,1) = Izy; mat(2,2) = Izz;
102
103 mat(3,0) = 0.; mat(3,1) = h[2]; mat(3,2) = -h[1];
104 mat(4,0) = -h[2]; mat(4,1) = 0.; mat(4,2) = h[0];
105 mat(5,0) = h[1]; mat(5,1) = -h[0]; mat(5,2) = 0.;
106
107 mat(0,3) = 0.; mat(0,4) = -h[2]; mat(0,5) = h[1];
108 mat(1,3) = h[2]; mat(1,4) = 0.; mat(1,5) = -h[0];
109 mat(2,3) = -h[1]; mat(2,4) = h[0]; mat(2,5) = 0.;
110
111 mat(3,3) = m; mat(3,4) = 0.; mat(3,5) = 0.;
112 mat(4,3) = 0.; mat(4,4) = m; mat(4,5) = 0.;
113 mat(5,3) = 0.; mat(5,4) = 0.; mat(5,5) = m;
114 }
115
116 static SpatialRigidBodyInertia createFromMassComInertiaC (Scalar mass, const Vector3d &com, const Matrix3d &inertia_C) {
118 result.m = mass;
119 result.h = com * mass;
120 Matrix3d I = inertia_C + VectorCrossMatrix (com) * VectorCrossMatrix(com).transpose() * mass;
121 result.Ixx = I(0,0);
122 result.Iyx = I(1,0);
123 result.Iyy = I(1,1);
124 result.Izx = I(2,0);
125 result.Izy = I(2,1);
126 result.Izz = I(2,2);
127 return result;
128 }
129
135 Scalar Ixx, Iyx, Iyy, Izx, Izy, Izz;
136};
137
145struct RBDL_DLLAPI SpatialTransform {
147 E (Matrix3d::Identity()),
148 r (Vector3d::Zero())
149 {}
150 SpatialTransform (const Matrix3d &rotation, const Vector3d &translation) :
151 E (rotation),
152 r (translation)
153 {}
154
160 Vector3d v_rxw (
161 v_sp[3] - r[1]*v_sp[2] + r[2]*v_sp[1],
162 v_sp[4] - r[2]*v_sp[0] + r[0]*v_sp[2],
163 v_sp[5] - r[0]*v_sp[1] + r[1]*v_sp[0]
164 );
165 return SpatialVector (
166 E(0,0) * v_sp[0] + E(0,1) * v_sp[1] + E(0,2) * v_sp[2],
167 E(1,0) * v_sp[0] + E(1,1) * v_sp[1] + E(1,2) * v_sp[2],
168 E(2,0) * v_sp[0] + E(2,1) * v_sp[1] + E(2,2) * v_sp[2],
169 E(0,0) * v_rxw[0] + E(0,1) * v_rxw[1] + E(0,2) * v_rxw[2],
170 E(1,0) * v_rxw[0] + E(1,1) * v_rxw[1] + E(1,2) * v_rxw[2],
171 E(2,0) * v_rxw[0] + E(2,1) * v_rxw[1] + E(2,2) * v_rxw[2]
172 );
173 }
174
180 Vector3d E_T_f (
181 E(0,0) * f_sp[3] + E(1,0) * f_sp[4] + E(2,0) * f_sp[5],
182 E(0,1) * f_sp[3] + E(1,1) * f_sp[4] + E(2,1) * f_sp[5],
183 E(0,2) * f_sp[3] + E(1,2) * f_sp[4] + E(2,2) * f_sp[5]
184 );
185
186 return SpatialVector (
187 E(0,0) * f_sp[0] + E(1,0) * f_sp[1] + E(2,0) * f_sp[2] - r[2] * E_T_f[1] + r[1] * E_T_f[2],
188 E(0,1) * f_sp[0] + E(1,1) * f_sp[1] + E(2,1) * f_sp[2] + r[2] * E_T_f[0] - r[0] * E_T_f[2],
189 E(0,2) * f_sp[0] + E(1,2) * f_sp[1] + E(2,2) * f_sp[2] - r[1] * E_T_f[0] + r[0] * E_T_f[1],
190 E_T_f [0],
191 E_T_f [1],
192 E_T_f [2]
193 );
194 }
195
200 rbi.m,
201 E * (rbi.h - rbi.m * r),
202 E *
203 (
204 Matrix3d (
205 rbi.Ixx, rbi.Iyx, rbi.Izx,
206 rbi.Iyx, rbi.Iyy, rbi.Izy,
207 rbi.Izx, rbi.Izy, rbi.Izz
208 )
210 + (VectorCrossMatrix(rbi.h - rbi.m * r) * VectorCrossMatrix (r))
211 )
212 * E.transpose()
213 );
214 }
215
219 Vector3d E_T_mr = E.transpose() * rbi.h + rbi.m * r;
221 rbi.m,
222 E_T_mr,
223 E.transpose() *
224 Matrix3d (
225 rbi.Ixx, rbi.Iyx, rbi.Izx,
226 rbi.Iyx, rbi.Iyy, rbi.Izy,
227 rbi.Izx, rbi.Izy, rbi.Izz
228 ) * E
229 - VectorCrossMatrix(r) * VectorCrossMatrix (E.transpose() * rbi.h)
230 - VectorCrossMatrix (E_T_mr) * VectorCrossMatrix (r));
231 }
232
234 Vector3d En_rxf = E * (Vector3d (f_sp[0], f_sp[1], f_sp[2]) - r.cross(Vector3d (f_sp[3], f_sp[4], f_sp[5])));
235 // Vector3d En_rxf = E * (Vector3d (f_sp[0], f_sp[1], f_sp[2]) - r.cross(Eigen::Map<Vector3d> (&(f_sp[3]))));
236
237 return SpatialVector (
238 En_rxf[0],
239 En_rxf[1],
240 En_rxf[2],
241 E(0,0) * f_sp[3] + E(0,1) * f_sp[4] + E(0,2) * f_sp[5],
242 E(1,0) * f_sp[3] + E(1,1) * f_sp[4] + E(1,2) * f_sp[5],
243 E(2,0) * f_sp[3] + E(2,1) * f_sp[4] + E(2,2) * f_sp[5]
244 );
245 }
246
248 Matrix3d _Erx =
249 E * Matrix3d (
250 0., -r[2], r[1],
251 r[2], 0., -r[0],
252 -r[1], r[0], 0.
253 );
254 SpatialMatrix result;
255 result.block<3,3>(0,0) = E;
256 result.block<3,3>(0,3) = Matrix3d::Zero();
257 result.block<3,3>(3,0) = -_Erx;
258 result.block<3,3>(3,3) = E;
259
260 return result;
261 }
262
264 Matrix3d _Erx =
265 E * Matrix3d (
266 0., -r[2], r[1],
267 r[2], 0., -r[0],
268 -r[1], r[0], 0.
269 );
270 SpatialMatrix result;
271 result.block<3,3>(0,0) = E;
272 result.block<3,3>(0,3) = -_Erx;
273 result.block<3,3>(3,0) = Matrix3d::Zero();
274 result.block<3,3>(3,3) = E;
275
276 return result;
277 }
278
280 Matrix3d _Erx =
281 E * Matrix3d (
282 0., -r[2], r[1],
283 r[2], 0., -r[0],
284 -r[1], r[0], 0.
285 );
286 SpatialMatrix result;
287 result.block<3,3>(0,0) = E.transpose();
288 result.block<3,3>(0,3) = -_Erx.transpose();
289 result.block<3,3>(3,0) = Matrix3d::Zero();
290 result.block<3,3>(3,3) = E.transpose();
291
292 return result;
293 }
294
296 return SpatialTransform (
297 E.transpose(),
298 - E * r
299 );
300 }
301
303 return SpatialTransform (E * XT.E, XT.r + XT.E.transpose() * r);
304 }
305
306 void operator*= (const SpatialTransform &XT) {
307 r = XT.r + XT.E.transpose() * r;
308 E *= XT.E;
309 }
310
313};
314
315inline std::ostream& operator<<(std::ostream& output, const SpatialRigidBodyInertia &rbi) {
316 output << "rbi.m = " << rbi.m << std::endl;
317 output << "rbi.h = " << rbi.h.transpose();
318 output << "rbi.Ixx = " << rbi.Ixx << std::endl;
319 output << "rbi.Iyx = " << rbi.Iyx << " rbi.Iyy = " << rbi.Iyy << std::endl;
320 output << "rbi.Izx = " << rbi.Izx << " rbi.Izy = " << rbi.Izy << " rbi.Izz = " << rbi.Izz << std::endl;
321 return output;
322}
323
324inline std::ostream& operator<<(std::ostream& output, const SpatialTransform &X) {
325 output << "X.E = " << std::endl << X.E << std::endl;
326 output << "X.r = " << X.r.transpose();
327 return output;
328}
329
330inline SpatialTransform Xrot (Scalar angle_rad, const Vector3d &axis) {
331 Scalar s, c;
332 s = sin(angle_rad);
333 c = cos(angle_rad);
334
335 return SpatialTransform (
336 Matrix3d (
337 axis[0] * axis[0] * (1.0f - c) + c,
338 axis[1] * axis[0] * (1.0f - c) + axis[2] * s,
339 axis[0] * axis[2] * (1.0f - c) - axis[1] * s,
340
341 axis[0] * axis[1] * (1.0f - c) - axis[2] * s,
342 axis[1] * axis[1] * (1.0f - c) + c,
343 axis[1] * axis[2] * (1.0f - c) + axis[0] * s,
344
345 axis[0] * axis[2] * (1.0f - c) + axis[1] * s,
346 axis[1] * axis[2] * (1.0f - c) - axis[0] * s,
347 axis[2] * axis[2] * (1.0f - c) + c
348
349 ),
350 Vector3d (0., 0., 0.)
351 );
352}
353
354inline SpatialTransform Xrotx (const Scalar &xrot) {
355 Scalar s, c;
356 s = sin (xrot);
357 c = cos (xrot);
358 return SpatialTransform (
359 Matrix3d (
360 1., 0., 0.,
361 0., c, s,
362 0., -s, c
363 ),
364 Vector3d (0., 0., 0.)
365 );
366}
367
368inline SpatialTransform Xroty (const Scalar &yrot) {
369 Scalar s, c;
370 s = sin (yrot);
371 c = cos (yrot);
372 return SpatialTransform (
373 Matrix3d (
374 c, 0., -s,
375 0., 1., 0.,
376 s, 0., c
377 ),
378 Vector3d (0., 0., 0.)
379 );
380}
381
382inline SpatialTransform Xrotz (const Scalar &zrot) {
383 Scalar s, c;
384 s = sin (zrot);
385 c = cos (zrot);
386 return SpatialTransform (
387 Matrix3d (
388 c, s, 0.,
389 -s, c, 0.,
390 0., 0., 1.
391 ),
392 Vector3d (0., 0., 0.)
393 );
394}
395
397 return SpatialTransform (
398 Matrix3d::Identity(),
399 r
400 );
401}
402
404 return SpatialMatrix (
405 0, -v[2], v[1], 0, 0, 0,
406 v[2], 0, -v[0], 0, 0, 0,
407 -v[1], v[0], 0, 0, 0, 0,
408 0, -v[5], v[4], 0, -v[2], v[1],
409 v[5], 0, -v[3], v[2], 0, -v[0],
410 -v[4], v[3], 0, -v[1], v[0], 0
411 );
412}
413
414inline SpatialVector crossm (const SpatialVector &v1, const SpatialVector &v2) {
415 return SpatialVector (
416 -v1[2] * v2[1] + v1[1] * v2[2],
417 v1[2] * v2[0] - v1[0] * v2[2],
418 -v1[1] * v2[0] + v1[0] * v2[1],
419 -v1[5] * v2[1] + v1[4] * v2[2] - v1[2] * v2[4] + v1[1] * v2[5],
420 v1[5] * v2[0] - v1[3] * v2[2] + v1[2] * v2[3] - v1[0] * v2[5],
421 -v1[4] * v2[0] + v1[3] * v2[1] - v1[1] * v2[3] + v1[0] * v2[4]
422 );
423}
424
426 return SpatialMatrix (
427 0, -v[2], v[1], 0, -v[5], v[4],
428 v[2], 0, -v[0], v[5], 0, -v[3],
429 -v[1], v[0], 0, -v[4], v[3], 0,
430 0, 0, 0, 0, -v[2], v[1],
431 0, 0, 0, v[2], 0, -v[0],
432 0, 0, 0, -v[1], v[0], 0
433 );
434}
435
436inline SpatialVector crossf (const SpatialVector &v1, const SpatialVector &v2) {
437 return SpatialVector (
438 -v1[2] * v2[1] + v1[1] * v2[2] - v1[5] * v2[4] + v1[4] * v2[5],
439 v1[2] * v2[0] - v1[0] * v2[2] + v1[5] * v2[3] - v1[3] * v2[5],
440 -v1[1] * v2[0] + v1[0] * v2[1] - v1[4] * v2[3] + v1[3] * v2[4],
441 - v1[2] * v2[4] + v1[1] * v2[5],
442 v1[2] * v2[3] - v1[0] * v2[5],
443 - v1[1] * v2[3] + v1[0] * v2[4]
444 );
445}
446
447} /* Math */
448
449} /* RigidBodyDynamics */
450
451/* RBDL_SPATIALALGEBRAOPERATORS_H*/
452#endif
MX_Xd_static< nrows, ncols > operator*(const MX_Xd_static< nrows, ncols > &me, const MX_Xd_scalar &other)
Definition: MX_Xd_utils.h:13
MX_Xd_static< nrows, ncols > operator+(const MX_Xd_static< nrows, ncols > &me, const MX_Xd_dynamic &other)
Definition: MX_Xd_utils.h:40
SpatialMatrix_t SpatialMatrix
Definition: rbdl_math.h:62
SpatialTransform Xrotz(const Scalar &zrot)
SpatialMatrix crossf(const SpatialVector &v)
SpatialTransform Xroty(const Scalar &yrot)
SpatialVector_t SpatialVector
Definition: rbdl_math.h:61
SpatialMatrix crossm(const SpatialVector &v)
SpatialTransform Xtrans(const Vector3d &r)
Matrix3d VectorCrossMatrix(const Vector3d &vector)
SpatialTransform Xrot(Scalar angle_rad, const Vector3d &axis)
std::ostream & operator<<(std::ostream &output, const SpatialRigidBodyInertia &rbi)
SpatialTransform Xrotx(const Scalar &xrot)
MX_Xd_scalar cos(const MX_Xd_scalar &x)
Definition: MX_Xd_utils.h:351
MX_Xd_scalar sin(const MX_Xd_scalar &x)
Definition: MX_Xd_utils.h:345
Compact representation for Spatial Inertia.
static SpatialRigidBodyInertia createFromMassComInertiaC(Scalar mass, const Vector3d &com, const Matrix3d &inertia_C)
Vector3d h
Coordinates of the center of mass.
SpatialRigidBodyInertia(Scalar m, const Vector3d &h, const Scalar &Ixx, const Scalar &Iyx, const Scalar &Iyy, const Scalar &Izx, const Scalar &Izy, const Scalar &Izz)
SpatialRigidBodyInertia(Scalar mass, const Vector3d &com_mass, const Matrix3d &inertia)
Compact representation of spatial transformations.
SpatialVector applyTranspose(const SpatialVector &f_sp)
SpatialVector apply(const SpatialVector &v_sp)
SpatialRigidBodyInertia apply(const SpatialRigidBodyInertia &rbi)
SpatialVector applyAdjoint(const SpatialVector &f_sp)
SpatialTransform(const Matrix3d &rotation, const Vector3d &translation)
SpatialRigidBodyInertia applyTranspose(const SpatialRigidBodyInertia &rbi)