pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
symmetric3.hpp
1//
2// Copyright (c) 2014-2021 CNRS INRIA
3//
4
5#ifndef __pinocchio_spatial_symmetric3__
6#define __pinocchio_spatial_symmetric3__
7
8#include "pinocchio/spatial/fwd.hpp"
9
10#include "pinocchio/math/matrix.hpp"
11
12namespace pinocchio
13{
14 template<typename _Scalar, int _Options>
16 {
17 typedef _Scalar Scalar;
18 };
19
20 template<typename _Scalar, int _Options>
21 class Symmetric3Tpl : public NumericalBase<Symmetric3Tpl<_Scalar, _Options>>
22 {
23 public:
24 typedef _Scalar Scalar;
25 enum
26 {
27 Options = _Options
28 };
29 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
30 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
31 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
32 typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2;
33 typedef Eigen::Matrix<Scalar, 3, 2, Options> Matrix32;
34
36
37 public:
39 {
40 }
41
42 template<typename Sc, int Opt>
43 explicit Symmetric3Tpl(const Eigen::Matrix<Sc, 3, 3, Opt> & I)
44 {
45 assert(check_expression_if_real<Scalar>(pinocchio::isZero((I - I.transpose()))));
46 m_data(0) = I(0, 0);
47 m_data(1) = I(1, 0);
48 m_data(2) = I(1, 1);
49 m_data(3) = I(2, 0);
50 m_data(4) = I(2, 1);
51 m_data(5) = I(2, 2);
52 }
53
54 explicit Symmetric3Tpl(const Vector6 & I)
55 : m_data(I)
56 {
57 }
58
60 {
61 *this = other;
62 }
63
64 template<typename S2, int O2>
66 {
67 *this = other.template cast<Scalar>();
68 }
69
75 Symmetric3Tpl & operator=(const Symmetric3Tpl & clone) // Copy assignment operator
76 {
77 m_data = clone.m_data;
78 return *this;
79 }
80
82 const Scalar & a0,
83 const Scalar & a1,
84 const Scalar & a2,
85 const Scalar & a3,
86 const Scalar & a4,
87 const Scalar & a5)
88 {
89 m_data << a0, a1, a2, a3, a4, a5;
90 }
91
92 static Symmetric3Tpl Zero()
93 {
94 return Symmetric3Tpl(Vector6::Zero());
95 }
96 void setZero()
97 {
98 m_data.setZero();
99 }
100
101 static Symmetric3Tpl Random()
102 {
103 return RandomPositive();
104 }
105 void setRandom()
106 {
107 Scalar a = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
108 b = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
109 c = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
110 d = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
111 e = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
112 f = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0;
113
114 m_data << a, b, c, d, e, f;
115 }
116
117 static Symmetric3Tpl Identity()
118 {
119 return Symmetric3Tpl(Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1));
120 }
121 void setIdentity()
122 {
123 m_data << Scalar(1), Scalar(0), Scalar(1), Scalar(0), Scalar(0), Scalar(1);
124 }
125
126 template<typename Vector3Like>
127 void setDiagonal(const Eigen::MatrixBase<Vector3Like> & diag)
128 {
129 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
130 m_data[0] = diag[0];
131 m_data[2] = diag[1];
132 m_data[5] = diag[2];
133 }
134
135 /* Required by Inertia::operator== */
136 bool operator==(const Symmetric3Tpl & other) const
137 {
138 return m_data == other.m_data;
139 }
140
141 bool operator!=(const Symmetric3Tpl & other) const
142 {
143 return !(*this == other);
144 }
145
146 bool isApprox(
147 const Symmetric3Tpl & other,
148 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
149 {
150 return m_data.isApprox(other.m_data, prec);
151 }
152
153 bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
154 {
155 return m_data.isZero(prec);
156 }
157
158 void fill(const Scalar value)
159 {
160 m_data.fill(value);
161 }
162
163 template<typename Matrix3Like>
164 void inverse(const Eigen::MatrixBase<Matrix3Like> & res_) const
165 {
166 Matrix3Like & res = res_.const_cast_derived();
167 const Scalar &a11 = m_data[0], a21 = m_data[1], a22 = m_data[2], a31 = m_data[3],
168 a32 = m_data[4], a33 = m_data[5];
169
170 res(0, 0) = a33 * a22 - a32 * a32;
171 res(1, 0) = res(0, 1) = -(a33 * a21 - a32 * a31);
172 res(2, 0) = res(0, 2) = a32 * a21 - a22 * a31;
173 res(1, 1) = a33 * a11 - a31 * a31;
174 res(2, 1) = res(1, 2) = -(a32 * a11 - a21 * a31);
175 res(2, 2) = a22 * a11 - a21 * a21;
176
177 const Scalar det = a11 * res(0, 0) + a21 * res(0, 1) + a31 * res(0, 2);
178 res /= det;
179 }
180
181 Matrix3 inverse() const
182 {
183 Matrix3 res;
184 inverse(res);
185 return res;
186 }
187
189 {
190 const Vector3 & v;
191 SkewSquare(const Vector3 & v)
192 : v(v)
193 {
194 }
195 operator Symmetric3Tpl() const
196 {
197 const Scalar &x = v[0], &y = v[1], &z = v[2];
198 return Symmetric3Tpl(-y * y - z * z, x * y, -x * x - z * z, x * z, y * z, -x * x - y * y);
199 }
200 }; // struct SkewSquare
201
202 Symmetric3Tpl operator-(const SkewSquare & v) const
203 {
204 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
205 return Symmetric3Tpl(
206 m_data[0] + y * y + z * z, m_data[1] - x * y, m_data[2] + x * x + z * z, m_data[3] - x * z,
207 m_data[4] - y * z, m_data[5] + x * x + y * y);
208 }
209
210 Symmetric3Tpl & operator-=(const SkewSquare & v)
211 {
212 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
213 m_data[0] += y * y + z * z;
214 m_data[1] -= x * y;
215 m_data[2] += x * x + z * z;
216 m_data[3] -= x * z;
217 m_data[4] -= y * z;
218 m_data[5] += x * x + y * y;
219 return *this;
220 }
221
223 {
224 const Scalar & m;
225 const Vector3 & v;
226
227 AlphaSkewSquare(const Scalar & m, const SkewSquare & v)
228 : m(m)
229 , v(v.v)
230 {
231 }
232 AlphaSkewSquare(const Scalar & m, const Vector3 & v)
233 : m(m)
234 , v(v)
235 {
236 }
237
238 operator Symmetric3Tpl() const
239 {
240 const Scalar &x = v[0], &y = v[1], &z = v[2];
241 return Symmetric3Tpl(
242 -m * (y * y + z * z), m * x * y, -m * (x * x + z * z), m * x * z, m * y * z,
243 -m * (x * x + y * y));
244 }
245 };
246
247 friend AlphaSkewSquare operator*(const Scalar & m, const SkewSquare & sk)
248 {
249 return AlphaSkewSquare(m, sk);
250 }
251
252 Symmetric3Tpl operator-(const AlphaSkewSquare & v) const
253 {
254 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
255 return Symmetric3Tpl(
256 m_data[0] + v.m * (y * y + z * z), m_data[1] - v.m * x * y,
257 m_data[2] + v.m * (x * x + z * z), m_data[3] - v.m * x * z, m_data[4] - v.m * y * z,
258 m_data[5] + v.m * (x * x + y * y));
259 }
260
261 Symmetric3Tpl & operator-=(const AlphaSkewSquare & v)
262 {
263 const Scalar &x = v.v[0], &y = v.v[1], &z = v.v[2];
264 m_data[0] += v.m * (y * y + z * z);
265 m_data[1] -= v.m * x * y;
266 m_data[2] += v.m * (x * x + z * z);
267 m_data[3] -= v.m * x * z;
268 m_data[4] -= v.m * y * z;
269 m_data[5] += v.m * (x * x + y * y);
270 return *this;
271 }
272
273 const Vector6 & data() const
274 {
275 return m_data;
276 }
277 Vector6 & data()
278 {
279 return m_data;
280 }
281
282 // static Symmetric3Tpl SkewSq( const Vector3 & v )
283 // {
284 // const Scalar & x = v[0], & y = v[1], & z = v[2];
285 // return Symmetric3Tpl(-y*y-z*z,
286 // x*y, -x*x-z*z,
287 // x*z, y*z, -x*x-y*y );
288 // }
289
290 /* Shoot a positive definite matrix. */
291 static Symmetric3Tpl RandomPositive()
292 {
293 Scalar a = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
294 b = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
295 c = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
296 d = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
297 e = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0,
298 f = Scalar(std::rand()) / RAND_MAX * 2.0 - 1.0;
299 return Symmetric3Tpl(
300 a * a + b * b + d * d, a * b + b * c + d * e, b * b + c * c + e * e, a * d + b * e + d * f,
301 b * d + c * e + e * f, d * d + e * e + f * f);
302 }
303
304 Matrix3 matrix() const
305 {
306 Matrix3 res;
307 res(0, 0) = m_data(0);
308 res(0, 1) = m_data(1);
309 res(0, 2) = m_data(3);
310 res(1, 0) = m_data(1);
311 res(1, 1) = m_data(2);
312 res(1, 2) = m_data(4);
313 res(2, 0) = m_data(3);
314 res(2, 1) = m_data(4);
315 res(2, 2) = m_data(5);
316 return res;
317 }
318 operator Matrix3() const
319 {
320 return matrix();
321 }
322
323 Scalar vtiv(const Vector3 & v) const
324 {
325 const Scalar & x = v[0];
326 const Scalar & y = v[1];
327 const Scalar & z = v[2];
328
329 const Scalar xx = x * x;
330 const Scalar xy = x * y;
331 const Scalar xz = x * z;
332 const Scalar yy = y * y;
333 const Scalar yz = y * z;
334 const Scalar zz = z * z;
335
336 return m_data(0) * xx + m_data(2) * yy + m_data(5) * zz
337 + 2. * (m_data(1) * xy + m_data(3) * xz + m_data(4) * yz);
338 }
339
350 template<typename Vector3, typename Matrix3>
351 static void vxs(
352 const Eigen::MatrixBase<Vector3> & v,
353 const Symmetric3Tpl & S3,
354 const Eigen::MatrixBase<Matrix3> & M)
355 {
358
359 const Scalar & a = S3.data()[0];
360 const Scalar & b = S3.data()[1];
361 const Scalar & c = S3.data()[2];
362 const Scalar & d = S3.data()[3];
363 const Scalar & e = S3.data()[4];
364 const Scalar & f = S3.data()[5];
365
366 const typename Vector3::RealScalar & v0 = v[0];
367 const typename Vector3::RealScalar & v1 = v[1];
368 const typename Vector3::RealScalar & v2 = v[2];
369
370 Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
371 M_(0, 0) = d * v1 - b * v2;
372 M_(1, 0) = a * v2 - d * v0;
373 M_(2, 0) = b * v0 - a * v1;
374
375 M_(0, 1) = e * v1 - c * v2;
376 M_(1, 1) = b * v2 - e * v0;
377 M_(2, 1) = c * v0 - b * v1;
378
379 M_(0, 2) = f * v1 - e * v2;
380 M_(1, 2) = d * v2 - f * v0;
381 M_(2, 2) = e * v0 - d * v1;
382 }
383
394 template<typename Vector3>
395 Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
396 {
397 Matrix3 M;
398 vxs(v, *this, M);
399 return M;
400 }
401
411 template<typename Vector3, typename Matrix3>
412 static void svx(
413 const Eigen::MatrixBase<Vector3> & v,
414 const Symmetric3Tpl & S3,
415 const Eigen::MatrixBase<Matrix3> & M)
416 {
419
420 const Scalar & a = S3.data()[0];
421 const Scalar & b = S3.data()[1];
422 const Scalar & c = S3.data()[2];
423 const Scalar & d = S3.data()[3];
424 const Scalar & e = S3.data()[4];
425 const Scalar & f = S3.data()[5];
426
427 const typename Vector3::RealScalar & v0 = v[0];
428 const typename Vector3::RealScalar & v1 = v[1];
429 const typename Vector3::RealScalar & v2 = v[2];
430
431 Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, M);
432 M_(0, 0) = b * v2 - d * v1;
433 M_(1, 0) = c * v2 - e * v1;
434 M_(2, 0) = e * v2 - f * v1;
435
436 M_(0, 1) = d * v0 - a * v2;
437 M_(1, 1) = e * v0 - b * v2;
438 M_(2, 1) = f * v0 - d * v2;
439
440 M_(0, 2) = a * v1 - b * v0;
441 M_(1, 2) = b * v1 - c * v0;
442 M_(2, 2) = d * v1 - e * v0;
443 }
444
453 template<typename Vector3>
454 Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
455 {
456 Matrix3 M;
457 svx(v, *this, M);
458 return M;
459 }
460
461 Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
462 {
463 return Symmetric3Tpl(m_data + s2.m_data);
464 }
465
466 Symmetric3Tpl operator-(const Symmetric3Tpl & s2) const
467 {
468 return Symmetric3Tpl(m_data - s2.m_data);
469 }
470
471 Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
472 {
473 m_data += s2.m_data;
474 return *this;
475 }
476
477 Symmetric3Tpl & operator-=(const Symmetric3Tpl & s2)
478 {
479 m_data -= s2.m_data;
480 return *this;
481 }
482
483 Symmetric3Tpl & operator*=(const Scalar s)
484 {
485 m_data *= s;
486 return *this;
487 }
488
489 template<typename V3in, typename V3out>
490 static void rhsMult(
491 const Symmetric3Tpl & S3,
492 const Eigen::MatrixBase<V3in> & vin,
493 const Eigen::MatrixBase<V3out> & vout)
494 {
495 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in, Vector3);
496 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out, Vector3);
497
498 V3out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3out, vout);
499
500 vout_[0] = S3.m_data(0) * vin[0] + S3.m_data(1) * vin[1] + S3.m_data(3) * vin[2];
501 vout_[1] = S3.m_data(1) * vin[0] + S3.m_data(2) * vin[1] + S3.m_data(4) * vin[2];
502 vout_[2] = S3.m_data(3) * vin[0] + S3.m_data(4) * vin[1] + S3.m_data(5) * vin[2];
503 }
504
505 template<typename V3>
506 Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
507 {
508 Vector3 res;
509 rhsMult(*this, v, res);
510 return res;
511 }
512
513 // Matrix3 operator*(const Matrix3 &a) const
514 // {
515 // Matrix3 r;
516 // for(unsigned int i=0; i<3; ++i)
517 // {
518 // r(0,i) = m_data(0) * a(0,i) + m_data(1) * a(1,i) + m_data(3) * a(2,i);
519 // r(1,i) = m_data(1) * a(0,i) + m_data(2) * a(1,i) + m_data(4) * a(2,i);
520 // r(2,i) = m_data(3) * a(0,i) + m_data(4) * a(1,i) + m_data(5) * a(2,i);
521 // }
522 // return r;
523 // }
524
525 const Scalar & operator()(const int i, const int j) const
526 {
527 return ((i != 2) && (j != 2)) ? m_data[i + j] : m_data[i + j + 1];
528 }
529
530 template<typename Matrix3Like>
531 Symmetric3Tpl operator-(const Eigen::MatrixBase<Matrix3Like> & S) const
532 {
533 assert(check_expression_if_real<Scalar>(pinocchio::isZero(S - S.transpose())));
534 return Symmetric3Tpl(
535 m_data(0) - S(0, 0), m_data(1) - S(1, 0), m_data(2) - S(1, 1), m_data(3) - S(2, 0),
536 m_data(4) - S(2, 1), m_data(5) - S(2, 2));
537 }
538
539 template<typename Matrix3Like>
540 Symmetric3Tpl operator+(const Eigen::MatrixBase<Matrix3Like> & S) const
541 {
542 assert(check_expression_if_real<Scalar>(pinocchio::isZero(S - S.transpose())));
543 return Symmetric3Tpl(
544 m_data(0) + S(0, 0), m_data(1) + S(1, 0), m_data(2) + S(1, 1), m_data(3) + S(2, 0),
545 m_data(4) + S(2, 1), m_data(5) + S(2, 2));
546 }
547
548 /* --- Symmetric R*S*R' and R'*S*R products --- */
549 public: // private:
552 Matrix32 decomposeltI() const
553 {
554 Matrix32 L;
555 L << m_data(0) - m_data(5), m_data(1), m_data(1), m_data(2) - m_data(5), 2 * m_data(3),
556 m_data(4) + m_data(4);
557 return L;
558 }
559
560 /* R*S*R' */
561 template<typename D>
562 Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
563 {
565 assert(
567 && "R is not a Unitary matrix");
568
570
571 // 4 a
572 const Matrix32 L(decomposeltI());
573
574 // Y = R' L ===> (12 m + 8 a)
575 const Matrix2 Y(R.template block<2, 3>(1, 0) * L);
576
577 // Sres= Y R ===> (16 m + 8a)
578 Sres.m_data(1) = Y(0, 0) * R(0, 0) + Y(0, 1) * R(0, 1);
579 Sres.m_data(2) = Y(0, 0) * R(1, 0) + Y(0, 1) * R(1, 1);
580 Sres.m_data(3) = Y(1, 0) * R(0, 0) + Y(1, 1) * R(0, 1);
581 Sres.m_data(4) = Y(1, 0) * R(1, 0) + Y(1, 1) * R(1, 1);
582 Sres.m_data(5) = Y(1, 0) * R(2, 0) + Y(1, 1) * R(2, 1);
583
584 // r=R' v ( 6m + 3a)
585 const Vector3 r(
586 -R(0, 0) * m_data(4) + R(0, 1) * m_data(3), -R(1, 0) * m_data(4) + R(1, 1) * m_data(3),
587 -R(2, 0) * m_data(4) + R(2, 1) * m_data(3));
588
589 // Sres_11 (3a)
590 Sres.m_data(0) = L(0, 0) + L(1, 1) - Sres.m_data(2) - Sres.m_data(5);
591
592 // Sres + D + (Ev)x ( 9a)
593 Sres.m_data(0) += m_data(5);
594 Sres.m_data(1) += r(2);
595 Sres.m_data(2) += m_data(5);
596 Sres.m_data(3) -= r(1);
597 Sres.m_data(4) += r(0);
598 Sres.m_data(5) += m_data(5);
599
600 return Sres;
601 }
602
604 template<typename NewScalar>
609
610 friend std::ostream & operator<<(std::ostream & os, const Symmetric3Tpl<Scalar, Options> & S3)
611 {
612 os << "m_data: " << S3.m_data.transpose() << "\n";
613 return os;
614 }
615
616 // TODO: adjust code
617 // bool isValid() const
618 // {
619 // return
620 // m_data(0) >= Scalar(0)
621 // && m_data(2) >= Scalar(0)
622 // && m_data(5) >= Scalar(0);
623 // }
624
625 protected:
626 Vector6 m_data;
627
628 }; // class Symmetric3Tpl
629
630} // namespace pinocchio
631
632#endif // ifndef __pinocchio_spatial_symmetric3__
Symmetric3Tpl & operator=(const Symmetric3Tpl &clone)
Copy assignment operator.
Matrix3 svx(const Eigen::MatrixBase< Vector3 > &v) const
Performs the operation .
static void vxs(const Eigen::MatrixBase< Vector3 > &v, const Symmetric3Tpl &S3, const Eigen::MatrixBase< Matrix3 > &M)
Performs the operation . This operation is equivalent to applying the cross product of v on each colu...
Symmetric3Tpl< NewScalar, Options > cast() const
Matrix3 vxs(const Eigen::MatrixBase< Vector3 > &v) const
Performs the operation . This operation is equivalent to applying the cross product of v on each colu...
Matrix32 decomposeltI() const
Computes L for a symmetric matrix A.
static void svx(const Eigen::MatrixBase< Vector3 > &v, const Symmetric3Tpl &S3, const Eigen::MatrixBase< Matrix3 > &M)
Performs the operation .
Main pinocchio namespace.
Definition treeview.dox:11
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition matrix.hpp:155
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72