Rigid Body Dynamics Library
SpatialAlgebraOperators.h
Go to the documentation of this file.
1 /*
2  * RBDL - Rigid Body Dynamics Library
3  * Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
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 
14 namespace RigidBodyDynamics {
15 
16 namespace Math {
17 
18 inline 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 
27 struct RBDL_DLLAPI SpatialRigidBodyInertia {
29  m (0.),
30  h (Vector3d::Zero(3,1)),
31  Ixx (0.), Iyx(0.), Iyy(0.), Izx(0.), Izy(0.), Izz(0.)
32  {}
34  double 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  { }
40  SpatialRigidBodyInertia (double m, const Vector3d &h,
41  const double &Ixx,
42  const double &Iyx, const double &Iyy,
43  const double &Izx, const double &Izy, const double &Izz
44  ) :
45  m (m), h (h),
46  Ixx (Ixx),
47  Iyx (Iyx), Iyy(Iyy),
48  Izx (Izx), Izy(Izy), Izz(Izz)
49  { }
50 
51  SpatialVector operator* (const SpatialVector &mv) {
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 
77  void createFromMatrix (const SpatialMatrix &Ic) {
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(3,3) * 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 (double 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 
131  double m;
135  double Ixx, Iyx, Iyy, Izx, Izy, Izz;
136 };
137 
145 struct RBDL_DLLAPI SpatialTransform {
147  E (Matrix3d::Identity(3,3)),
148  r (Vector3d::Zero(3,1))
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 
199  return SpatialRigidBodyInertia (
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;
220  return SpatialRigidBodyInertia (
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(3,3);
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(3,3);
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(3,3);
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 
302  SpatialTransform operator* (const SpatialTransform &XT) const {
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 
315 inline 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 
324 inline 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 
330 inline SpatialTransform Xrot (double angle_rad, const Vector3d &axis) {
331  double 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 
354 inline SpatialTransform Xrotx (const double &xrot) {
355  double 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 
368 inline SpatialTransform Xroty (const double &yrot) {
369  double 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 
382 inline SpatialTransform Xrotz (const double &zrot) {
383  double 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 
396 inline SpatialTransform Xtrans (const Vector3d &r) {
397  return SpatialTransform (
398  Matrix3d::Identity(3,3),
399  r
400  );
401 }
402 
403 inline SpatialMatrix crossm (const SpatialVector &v) {
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 
414 inline 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 
425 inline SpatialMatrix crossf (const SpatialVector &v) {
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 
436 inline 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
SpatialMatrix crossf(const SpatialVector &v)
Compact representation of spatial transformations.
Compact representation for Spatial Inertia.
SpatialVector applyAdjoint(const SpatialVector &f_sp)
static SpatialRigidBodyInertia createFromMassComInertiaC(double mass, const Vector3d &com, const Matrix3d &inertia_C)
SpatialRigidBodyInertia(double m, const Vector3d &h, const double &Ixx, const double &Iyx, const double &Iyy, const double &Izx, const double &Izy, const double &Izz)
SpatialVector applyTranspose(const SpatialVector &f_sp)
SpatialVector_t SpatialVector
Definition: rbdl_math.h:54
SpatialMatrix crossm(const SpatialVector &v)
Matrix3d VectorCrossMatrix(const Vector3d &vector)
Namespace for all structures of the RigidBodyDynamics library.
Definition: Contacts.cc:22
SpatialTransform Xtrans(const Vector3d &r)
SpatialRigidBodyInertia applyTranspose(const SpatialRigidBodyInertia &rbi)
SpatialTransform Xrotx(const double &xrot)
SpatialTransform(const Matrix3d &rotation, const Vector3d &translation)
double Ixx
Inertia expressed at the origin.
SpatialTransform Xroty(const double &yrot)
SpatialVector apply(const SpatialVector &v_sp)
std::ostream & operator<<(std::ostream &output, const SpatialRigidBodyInertia &rbi)
SpatialMatrix_t SpatialMatrix
Definition: rbdl_math.h:55
SpatialRigidBodyInertia apply(const SpatialRigidBodyInertia &rbi)
SpatialTransform Xrotz(const double &zrot)
SpatialTransform Xrot(double angle_rad, const Vector3d &axis)
SpatialRigidBodyInertia(double mass, const Vector3d &com_mass, const Matrix3d &inertia)
Vector3d h
Coordinates of the center of mass.