pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
inertia.hpp
1//
2// Copyright (c) 2015-2018 CNRS
3// Copyright (c) 2018-2025 INRIA
4// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
5//
6
7#ifndef __pinocchio_spatial_inertia_hpp__
8#define __pinocchio_spatial_inertia_hpp__
9
10#include "pinocchio/math/fwd.hpp"
11#include "pinocchio/spatial/symmetric3.hpp"
12#include "pinocchio/spatial/force.hpp"
13#include "pinocchio/spatial/motion.hpp"
14#include "pinocchio/spatial/skew.hpp"
15
16namespace pinocchio
17{
18 template<class Derived>
19 struct InertiaBase : NumericalBase<Derived>
20 {
21 SPATIAL_TYPEDEF_TEMPLATE(Derived);
22
23 Derived & derived()
24 {
25 return *static_cast<Derived *>(this);
26 }
27 const Derived & derived() const
28 {
29 return *static_cast<const Derived *>(this);
30 }
31
32 Derived & const_cast_derived() const
33 {
34 return *const_cast<Derived *>(&derived());
35 }
36
37 Scalar mass() const
38 {
39 return static_cast<const Derived *>(this)->mass();
40 }
41 Scalar & mass()
42 {
43 return static_cast<const Derived *>(this)->mass();
44 }
45 const Vector3 & lever() const
46 {
47 return static_cast<const Derived *>(this)->lever();
48 }
49 Vector3 & lever()
50 {
51 return static_cast<const Derived *>(this)->lever();
52 }
53 const Symmetric3 & inertia() const
54 {
55 return static_cast<const Derived *>(this)->inertia();
56 }
57 Symmetric3 & inertia()
58 {
59 return static_cast<const Derived *>(this)->inertia();
60 }
61
62 template<typename Matrix6Like>
63 void matrix(const Eigen::MatrixBase<Matrix6Like> & mat) const
64 {
65 derived().matrix_impl(mat);
66 }
67 Matrix6 matrix() const
68 {
69 return derived().matrix_impl();
70 }
71 operator Matrix6() const
72 {
73 return matrix();
74 }
75
76 template<typename Matrix6Like>
77 void inverse(const Eigen::MatrixBase<Matrix6Like> & mat) const
78 {
79 derived().inverse_impl(mat);
80 }
81 Matrix6 inverse() const
82 {
83 return derived().inverse_impl();
84 }
85
86 Derived & operator=(const Derived & clone)
87 {
88 return derived().__equl__(clone);
89 }
90 bool operator==(const Derived & other) const
91 {
92 return derived().isEqual(other);
93 }
94 bool operator!=(const Derived & other) const
95 {
96 return !(*this == other);
97 }
98
99 Derived & operator+=(const Derived & Yb)
100 {
101 return derived().__pequ__(Yb);
102 }
103 Derived operator+(const Derived & Yb) const
104 {
105 return derived().__plus__(Yb);
106 }
107 Derived & operator-=(const Derived & Yb)
108 {
109 return derived().__mequ__(Yb);
110 }
111 Derived operator-(const Derived & Yb) const
112 {
113 return derived().__minus__(Yb);
114 }
115
116 template<typename MotionDerived>
118 operator*(const MotionDense<MotionDerived> & v) const
119 {
120 return derived().__mult__(v);
121 }
122
123 template<typename MotionDerived>
124 Scalar vtiv(const MotionDense<MotionDerived> & v) const
125 {
126 return derived().vtiv_impl(v);
127 }
128
129 template<typename MotionDerived>
130 Matrix6 variation(const MotionDense<MotionDerived> & v) const
131 {
132 return derived().variation_impl(v);
133 }
134
143 template<typename MotionDerived, typename M6>
144 static void
145 vxi(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout)
146 {
148 Derived::vxi_impl(v, I, Iout);
149 }
150
151 template<typename MotionDerived>
152 Matrix6 vxi(const MotionDense<MotionDerived> & v) const
153 {
154 Matrix6 Iout;
155 vxi(v, derived(), Iout);
156 return Iout;
157 }
158
167 template<typename MotionDerived, typename M6>
168 static void
169 ivx(const MotionDense<MotionDerived> & v, const Derived & I, const Eigen::MatrixBase<M6> & Iout)
170 {
172 Derived::ivx_impl(v, I, Iout);
173 }
174
175 template<typename MotionDerived>
176 Matrix6 ivx(const MotionDense<MotionDerived> & v) const
177 {
178 Matrix6 Iout;
179 ivx(v, derived(), Iout);
180 return Iout;
181 }
182
183 void setZero()
184 {
185 derived().setZero();
186 }
187 void setIdentity()
188 {
189 derived().setIdentity();
190 }
191 void setRandom()
192 {
193 derived().setRandom();
194 }
195
196 bool isApprox(
197 const Derived & other,
198 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
199 {
200 return derived().isApprox_impl(other, prec);
201 }
202
203 bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
204 {
205 return derived().isZero_impl(prec);
206 }
207
209 template<typename S2, int O2>
210 Derived se3Action(const SE3Tpl<S2, O2> & M) const
211 {
212 return derived().se3Action_impl(M);
213 }
214
216 template<typename S2, int O2>
217 Derived se3ActionInverse(const SE3Tpl<S2, O2> & M) const
218 {
219 return derived().se3ActionInverse_impl(M);
220 }
221
222 void disp(std::ostream & os) const
223 {
224 static_cast<const Derived *>(this)->disp_impl(os);
225 }
226 friend std::ostream & operator<<(std::ostream & os, const InertiaBase<Derived> & X)
227 {
228 X.disp(os);
229 return os;
230 }
231 };
232
233 // class InertiaBase
234 template<typename T, int U>
235 struct traits<InertiaTpl<T, U>>
236 {
237 typedef T Scalar;
238 typedef Eigen::Matrix<T, 3, 1, U> Vector3;
239 typedef Eigen::Matrix<T, 4, 1, U> Vector4;
240 typedef Eigen::Matrix<T, 6, 1, U> Vector6;
241 typedef Eigen::Matrix<T, 3, 3, U> Matrix3;
242 typedef Eigen::Matrix<T, 4, 4, U> Matrix4;
243 typedef Eigen::Matrix<T, 6, 6, U> Matrix6;
244 typedef Eigen::Matrix<T, 10, 10, U> Matrix10;
245 typedef Matrix6 ActionMatrix_t;
246 typedef Vector3 Angular_t;
247 typedef Vector3 Linear_t;
248 typedef const Vector3 ConstAngular_t;
249 typedef const Vector3 ConstLinear_t;
250 typedef Eigen::Quaternion<T, U> Quaternion_t;
251 typedef SE3Tpl<T, U> SE3;
252 typedef ForceTpl<T, U> Force;
253 typedef MotionTpl<T, U> Motion;
257 enum
258 {
259 LINEAR = 0,
260 ANGULAR = 3
261 };
262 }; // traits InertiaTpl
263
264 template<typename _Scalar, int _Options>
265 struct InertiaTpl : public InertiaBase<InertiaTpl<_Scalar, _Options>>
266 {
268
269 SPATIAL_TYPEDEF_TEMPLATE(InertiaTpl);
270 enum
271 {
272 Options = _Options
273 };
274
275 typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
276 typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
277 typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10;
280
281 // Constructors
282 InertiaTpl()
283 {
284 }
285
286 InertiaTpl(const Scalar & mass, const Vector3 & com, const Matrix3 & rotational_inertia)
287 : m_mass(mass)
288 , m_com(com)
289 , m_inertia(rotational_inertia)
290 {
291 }
292
293 explicit InertiaTpl(const Matrix6 & I6)
294 {
295 assert(check_expression_if_real<Scalar>(pinocchio::isZero(I6 - I6.transpose())));
296 mass() = I6(LINEAR, LINEAR);
297 const typename Matrix6::template ConstFixedBlockXpr<3, 3>::Type mc_cross =
298 I6.template block<3, 3>(ANGULAR, LINEAR);
299 lever() = unSkew(mc_cross);
300 lever() /= mass();
301
302 Matrix3 I3(mc_cross * mc_cross);
303 I3 /= mass();
304 I3 += I6.template block<3, 3>(ANGULAR, ANGULAR);
305 const Symmetric3 S3(I3);
306 inertia() = S3;
307 }
308
309 InertiaTpl(const Scalar & mass, const Vector3 & com, const Symmetric3 & rotational_inertia)
310 : m_mass(mass)
311 , m_com(com)
312 , m_inertia(rotational_inertia)
313 {
314 }
315
316 InertiaTpl(const InertiaTpl & clone) // Copy constructor
317 : m_mass(clone.mass())
318 , m_com(clone.lever())
319 , m_inertia(clone.inertia())
320 {
321 }
322
323 InertiaTpl & operator=(const InertiaTpl & clone) // Copy assignment operator
324 {
325 m_mass = clone.mass();
326 m_com = clone.lever();
327 m_inertia = clone.inertia();
328 return *this;
329 }
330
331 template<typename S2, int O2>
332 explicit InertiaTpl(const InertiaTpl<S2, O2> & clone)
333 {
334 *this = clone.template cast<Scalar>();
335 }
336
337 // Initializers
338 static InertiaTpl Zero()
339 {
340 return InertiaTpl(Scalar(0), Vector3::Zero(), Symmetric3::Zero());
341 }
342
343 void setZero()
344 {
345 mass() = Scalar(0);
346 lever().setZero();
347 inertia().setZero();
348 }
349
350 static InertiaTpl Identity()
351 {
352 return InertiaTpl(Scalar(1), Vector3::Zero(), Symmetric3::Identity());
353 }
354
355 void setIdentity()
356 {
357 mass() = Scalar(1);
358 lever().setZero();
359 inertia().setIdentity();
360 }
361
362 static InertiaTpl Random()
363 {
364 // We have to shoot "I" definite positive and not only symmetric.
365 return InertiaTpl(
366 Eigen::internal::random<Scalar>() + 1, Vector3::Random(), Symmetric3::RandomPositive());
367 }
368
375 static InertiaTpl FromSphere(const Scalar mass, const Scalar radius)
376 {
377 return FromEllipsoid(mass, radius, radius, radius);
378 }
379
389 static InertiaTpl
390 FromEllipsoid(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
391 {
392 const Scalar a = mass * (y * y + z * z) / Scalar(5);
393 const Scalar b = mass * (x * x + z * z) / Scalar(5);
394 const Scalar c = mass * (y * y + x * x) / Scalar(5);
395 return InertiaTpl(
396 mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c));
397 }
398
407 static InertiaTpl FromCylinder(const Scalar mass, const Scalar radius, const Scalar length)
408 {
409 const Scalar radius_square = radius * radius;
410 const Scalar a = mass * (radius_square / Scalar(4) + length * length / Scalar(12));
411 const Scalar c = mass * (radius_square / Scalar(2));
412 return InertiaTpl(
413 mass, Vector3::Zero(), Symmetric3(a, Scalar(0), a, Scalar(0), Scalar(0), c));
414 }
415
424 static InertiaTpl FromBox(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
425 {
426 const Scalar a = mass * (y * y + z * z) / Scalar(12);
427 const Scalar b = mass * (x * x + z * z) / Scalar(12);
428 const Scalar c = mass * (y * y + x * x) / Scalar(12);
429 return InertiaTpl(
430 mass, Vector3::Zero(), Symmetric3(a, Scalar(0), b, Scalar(0), Scalar(0), c));
431 }
432
440 static InertiaTpl FromCapsule(const Scalar mass, const Scalar radius, const Scalar height)
441 {
442 const Scalar pi = boost::math::constants::pi<Scalar>();
443
444 // first need to compute mass repartition between cylinder and halfsphere
445 const Scalar v_cyl = pi * math::pow(radius, 2) * height;
446 const Scalar v_hs = Scalar(2) / Scalar(3) * math::pow(radius, 3) * pi;
447 const Scalar total_v = v_cyl + Scalar(2) * v_hs;
448
449 const Scalar m_cyl = mass * (v_cyl / total_v);
450 const Scalar m_hs = mass * (v_hs / total_v);
451
452 // Then Distance between halfSphere center and cylindere center.
453 const Scalar dist_hc_cyl = height / Scalar(2) + Scalar(0.375) * radius;
454
455 // Computes inertia terms
456 const Scalar ix_c =
457 m_cyl * (math::pow(height, 2) / Scalar(12) + math::pow(radius, 2) / Scalar(4));
458 const Scalar iz_c = m_cyl * math::pow(radius, 2) / Scalar(2);
459
460 // For halfsphere inertia see, "Dynamics: Theory and Applications" McGraw-Hill, New York,
461 // 1985, by T.R. Kane and D.A. Levinson, Appendix 23.
462 const Scalar ix_hs = m_hs * math::pow(radius, 2) * Scalar(0.259375);
463 const Scalar iz_hs = m_hs * math::pow(radius, 2) * Scalar(0.4);
464
465 // Put everything together using the parallel axis theorem
466 const Scalar ix = ix_c + Scalar(2) * (ix_hs + m_hs * math::pow(dist_hc_cyl, 2));
467 const Scalar iz = iz_c + Scalar(2) * iz_hs;
468
469 return InertiaTpl(
470 mass, Vector3::Zero(), Symmetric3(ix, Scalar(0), ix, Scalar(0), Scalar(0), iz));
471 }
472
473 void setRandom()
474 {
475 mass() = static_cast<Scalar>(std::rand()) / static_cast<Scalar>(RAND_MAX);
476 lever().setRandom();
477 inertia().setRandom();
478 }
479
480 template<typename Matrix6Like>
481 void matrix_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const
482 {
483 Matrix6Like & M = M_.const_cast_derived();
484
485 M.template block<3, 3>(LINEAR, LINEAR).setZero();
486 M.template block<3, 3>(LINEAR, LINEAR).diagonal().fill(mass());
487 M.template block<3, 3>(ANGULAR, LINEAR) = alphaSkew(mass(), lever());
488 M.template block<3, 3>(LINEAR, ANGULAR) = -M.template block<3, 3>(ANGULAR, LINEAR);
489 M.template block<3, 3>(ANGULAR, ANGULAR) =
490 (inertia() - AlphaSkewSquare(mass(), lever())).matrix();
491 }
492
493 Matrix6 matrix_impl() const
494 {
495 Matrix6 M;
496 matrix_impl(M);
497 return M;
498 }
499
500 template<typename Matrix6Like>
501 void inverse_impl(const Eigen::MatrixBase<Matrix6Like> & M_) const
502 {
503 Matrix6Like & M = M_.const_cast_derived();
504 inertia().inverse(M.template block<3, 3>(ANGULAR, ANGULAR));
505
506 M.template block<3, 3>(LINEAR, ANGULAR).noalias() =
507 -M.template block<3, 3>(ANGULAR, ANGULAR).colwise().cross(lever());
508 M.template block<3, 3>(ANGULAR, LINEAR) = M.template block<3, 3>(LINEAR, ANGULAR).transpose();
509
510 const Scalar &cx = lever()[0], cy = lever()[1], cz = lever()[2];
511
512 M.template block<3, 3>(LINEAR, LINEAR).col(0).noalias() =
513 cy * M.template block<3, 3>(LINEAR, ANGULAR).col(2)
514 - cz * M.template block<3, 3>(LINEAR, ANGULAR).col(1);
515
516 M.template block<3, 3>(LINEAR, LINEAR).col(1).noalias() =
517 cz * M.template block<3, 3>(LINEAR, ANGULAR).col(0)
518 - cx * M.template block<3, 3>(LINEAR, ANGULAR).col(2);
519
520 M.template block<3, 3>(LINEAR, LINEAR).col(2).noalias() =
521 cx * M.template block<3, 3>(LINEAR, ANGULAR).col(1)
522 - cy * M.template block<3, 3>(LINEAR, ANGULAR).col(0);
523
524 const Scalar m_inv = Scalar(1) / mass();
525 M.template block<3, 3>(LINEAR, LINEAR).diagonal().array() += m_inv;
526 }
527
528 Matrix6 inverse_impl() const
529 {
530 Matrix6 res;
531 inverse_impl(res);
532 return res;
533 }
534
542 Vector10 toDynamicParameters() const
543 {
544 Vector10 v;
545
546 v[0] = mass();
547 v.template segment<3>(1).noalias() = mass() * lever();
548 v.template segment<6>(4) = (inertia() - AlphaSkewSquare(mass(), lever())).data();
549
550 return v;
551 }
552
561 template<typename Vector10Like>
562 static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & params)
563 {
564 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, params, 10, 1);
565
566 const Scalar & mass = params[0];
567 Vector3 lever = params.template segment<3>(1);
568 lever /= mass;
569
570 return InertiaTpl(
571 mass, lever, Symmetric3(params.template segment<6>(4)) + AlphaSkewSquare(mass, lever));
572 }
573
583 {
584 return pseudo_inertia.toInertia();
585 }
586
593 {
594 PseudoInertia pseudo_inertia = PseudoInertia::FromInertia(*this);
595 return pseudo_inertia;
596 }
597
610
611 // Arithmetic operators
612 InertiaTpl & __equl__(const InertiaTpl & clone)
613 {
614 mass() = clone.mass();
615 lever() = clone.lever();
616 inertia() = clone.inertia();
617 return *this;
618 }
619
620 // Required by std::vector boost::python bindings.
621 bool isEqual(const InertiaTpl & Y2) const
622 {
623 return (mass() == Y2.mass()) && (lever() == Y2.lever()) && (inertia() == Y2.inertia());
624 }
625
626 bool isApprox_impl(
627 const InertiaTpl & other,
628 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
629 {
630 using math::fabs;
631 return fabs(static_cast<Scalar>(mass() - other.mass())) <= prec
632 && lever().isApprox(other.lever(), prec) && inertia().isApprox(other.inertia(), prec);
633 }
634
635 bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
636 {
637 using math::fabs;
638 return fabs(mass()) <= prec && lever().isZero(prec) && inertia().isZero(prec);
639 }
640
641 InertiaTpl __plus__(const InertiaTpl & Yb) const
642 {
643 /* Y_{a+b} = ( m_a+m_b,
644 * (m_a*c_a + m_b*c_b ) / (m_a + m_b),
645 * I_a + I_b - (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
646 */
647
648 const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
649
650 const Scalar & mab = mass() + Yb.mass();
651 const Scalar mab_inv = Scalar(1) / math::max(mab, eps);
652 const Vector3 & AB = (lever() - Yb.lever()).eval();
653 return InertiaTpl(
654 mab, (mass() * lever() + Yb.mass() * Yb.lever()) * mab_inv,
655 inertia() + Yb.inertia()
656 - (mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB));
657 }
658
659 InertiaTpl & __pequ__(const InertiaTpl & Yb)
660 {
661 static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
662
663 const InertiaTpl & Ya = *this;
664 const Scalar & mab = mass() + Yb.mass();
665 const Scalar mab_inv = Scalar(1) / math::max(mab, eps);
666 const Vector3 & AB = (Ya.lever() - Yb.lever()).eval();
667 lever() *= (mass() * mab_inv);
668 lever() += (Yb.mass() * mab_inv) * Yb.lever(); // c *= mab_inv;
669 inertia() += Yb.inertia();
670 inertia() -= (Ya.mass() * Yb.mass() * mab_inv) * typename Symmetric3::SkewSquare(AB);
671 mass() = mab;
672 return *this;
673 }
674
675 InertiaTpl __minus__(const InertiaTpl & Yb) const
676 {
677 /* Y_{a} = ( m_{a+b}+m_b,
678 * (m_{a+b}*c_{a+b} - m_b*c_b ) / (m_a),
679 * I_{a+b} - I_b + (m_a*m_b)/(m_a+m_b) * AB_x * AB_x )
680 */
681
682 const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
683
684 const Scalar ma = mass() - Yb.mass();
685 assert(check_expression_if_real<Scalar>(ma >= Scalar(0)));
686
687 const Scalar ma_inv = Scalar(1) / math::max(ma, eps);
688 const Vector3 c_a((mass() * lever() - Yb.mass() * Yb.lever()) * ma_inv);
689
690 const Vector3 AB = c_a - Yb.lever();
691
692 return InertiaTpl(
693 ma, c_a,
694 inertia() - Yb.inertia() + (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB));
695 }
696
697 InertiaTpl & __mequ__(const InertiaTpl & Yb)
698 {
699 static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
700
701 const Scalar ma = mass() - Yb.mass();
702 assert(check_expression_if_real<Scalar>(ma >= Scalar(0)));
703
704 const Scalar ma_inv = Scalar(1) / math::max(ma, eps);
705
706 lever() *= (mass() * ma_inv);
707 lever().noalias() -= (Yb.mass() * ma_inv) * Yb.lever();
708
709 const Vector3 AB = lever() - Yb.lever();
710 inertia() -= Yb.inertia();
711 inertia() += (ma * Yb.mass() / mass()) * typename Symmetric3::SkewSquare(AB);
712 mass() = ma;
713
714 return *this;
715 }
716
717 template<typename MotionDerived>
718 ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options>
719 __mult__(const MotionDense<MotionDerived> & v) const
720 {
721 typedef ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options>
722 ReturnType;
723 ReturnType f;
724 __mult__(v, f);
725 return f;
726 }
727
728 template<typename MotionDerived, typename ForceDerived>
729 void __mult__(const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f) const
730 {
731 f.linear().noalias() = mass() * (v.linear() - lever().cross(v.angular()));
732 Symmetric3::rhsMult(inertia(), v.angular(), f.angular());
733 f.angular() += lever().cross(f.linear());
734 // f.angular().noalias() = c.cross(f.linear()) + I*v.angular();
735 }
736
737 template<typename MotionDerived>
738 Scalar vtiv_impl(const MotionDense<MotionDerived> & v) const
739 {
740 const Vector3 cxw(lever().cross(v.angular()));
741 Scalar res = mass() * (v.linear().squaredNorm() - Scalar(2) * v.linear().dot(cxw));
742 const Vector3 mcxcxw(-mass() * lever().cross(cxw));
743 res += v.angular().dot(mcxcxw);
744 res += inertia().vtiv(v.angular());
745
746 return res;
747 }
748
749 template<typename MotionDerived>
750 Matrix6 variation(const MotionDense<MotionDerived> & v) const
751 {
752 Matrix6 res;
753 const Motion mv(v * mass());
754
755 res.template block<3, 3>(LINEAR, ANGULAR) =
756 -skew(mv.linear()) - skewSquare(mv.angular(), lever()) + skewSquare(lever(), mv.angular());
757 res.template block<3, 3>(ANGULAR, LINEAR) =
758 res.template block<3, 3>(LINEAR, ANGULAR).transpose();
759
760 // res.template block<3,3>(LINEAR,LINEAR) = mv.linear()*c.transpose(); // use as
761 // temporary variable res.template block<3,3>(ANGULAR,ANGULAR) = res.template
762 // block<3,3>(LINEAR,LINEAR) - res.template block<3,3>(LINEAR,LINEAR).transpose();
763 res.template block<3, 3>(ANGULAR, ANGULAR) =
764 -skewSquare(mv.linear(), lever()) - skewSquare(lever(), mv.linear());
765
766 res.template block<3, 3>(LINEAR, LINEAR) =
767 (inertia() - AlphaSkewSquare(mass(), lever())).matrix();
768
769 res.template block<3, 3>(ANGULAR, ANGULAR) -=
770 res.template block<3, 3>(LINEAR, LINEAR) * skew(v.angular());
771 res.template block<3, 3>(ANGULAR, ANGULAR) +=
772 cross(v.angular(), res.template block<3, 3>(LINEAR, LINEAR));
773
774 res.template block<3, 3>(LINEAR, LINEAR).setZero();
775 return res;
776 }
777
778 template<typename MotionDerived, typename M6>
779 static void vxi_impl(
780 const MotionDense<MotionDerived> & v,
781 const InertiaTpl & I,
782 const Eigen::MatrixBase<M6> & Iout)
783 {
784 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6);
785 M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout);
786
787 // Block 1,1
788 alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR));
789 // Iout_.template block<3,3>(LINEAR,LINEAR) = alphaSkew(I.mass(),v.angular());
790 const Vector3 mc(I.mass() * I.lever());
791
792 // Block 1,2
793 skewSquare(-v.angular(), mc, Iout_.template block<3, 3>(LINEAR, ANGULAR));
794
796 alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(ANGULAR, LINEAR));
797 Iout_.template block<3, 3>(ANGULAR, LINEAR) -= Iout_.template block<3, 3>(LINEAR, ANGULAR);
798
800 skewSquare(-v.linear(), mc, Iout_.template block<3, 3>(ANGULAR, ANGULAR));
801
802 // TODO: I do not why, but depending on the CPU, these three lines can give
803 // wrong output.
804 // typename Symmetric3::AlphaSkewSquare mcxcx(I.mass(),I.lever());
805 // const Symmetric3 I_mcxcx(I.inertia() - mcxcx);
806 // Iout_.template block<3,3>(ANGULAR,ANGULAR) += I_mcxcx.vxs(v.angular());
807 Symmetric3 mcxcx(typename Symmetric3::AlphaSkewSquare(I.mass(), I.lever()));
808 Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().vxs(v.angular());
809 Iout_.template block<3, 3>(ANGULAR, ANGULAR) -= mcxcx.vxs(v.angular());
810 }
811
812 template<typename MotionDerived, typename M6>
813 static void ivx_impl(
814 const MotionDense<MotionDerived> & v,
815 const InertiaTpl & I,
816 const Eigen::MatrixBase<M6> & Iout)
817 {
818 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6);
819 M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout);
820
821 // Block 1,1
822 alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR));
823
824 // Block 2,1
825 const Vector3 mc(I.mass() * I.lever());
826 skewSquare(mc, v.angular(), Iout_.template block<3, 3>(ANGULAR, LINEAR));
827
828 // Block 1,2
829 alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(LINEAR, ANGULAR));
830
831 // Block 2,2
832 cross(
833 -I.lever(), Iout_.template block<3, 3>(ANGULAR, LINEAR),
834 Iout_.template block<3, 3>(ANGULAR, ANGULAR));
835 Iout_.template block<3, 3>(ANGULAR, ANGULAR) += I.inertia().svx(v.angular());
836 for (int k = 0; k < 3; ++k)
837 Iout_.template block<3, 3>(ANGULAR, ANGULAR).col(k) +=
838 I.lever().cross(Iout_.template block<3, 3>(LINEAR, ANGULAR).col(k));
839
840 // Block 1,2
841 Iout_.template block<3, 3>(LINEAR, ANGULAR) -= Iout_.template block<3, 3>(ANGULAR, LINEAR);
842 }
843
844 // Getters
845 Scalar mass() const
846 {
847 return m_mass;
848 }
849 const Vector3 & lever() const
850 {
851 return m_com;
852 }
853 const Symmetric3 & inertia() const
854 {
855 return m_inertia;
856 }
857
858 Scalar & mass()
859 {
860 return m_mass;
861 }
862 Vector3 & lever()
863 {
864 return m_com;
865 }
866 Symmetric3 & inertia()
867 {
868 return m_inertia;
869 }
870
872 template<typename S2, int O2>
874 {
875 /* The multiplication RIR' has a particular form that could be used, however it
876 * does not seems to be more efficient, see http://stackoverflow.m_comom/questions/
877 * 13215467/eigen-best-way-to-evaluate-asa-transpose-and-store-the-result-in-a-symmetric .*/
878 return InertiaTpl(
879 mass(), M.translation() + M.rotation() * lever(), inertia().rotate(M.rotation()));
880 }
881
883 template<typename S2, int O2>
885 {
886 return InertiaTpl(
887 mass(), M.rotation().transpose() * (lever() - M.translation()),
888 inertia().rotate(M.rotation().transpose()));
889 }
890
891 template<typename MotionDerived>
892 Force vxiv(const MotionDense<MotionDerived> & v) const
893 {
894 const Vector3 & mcxw = mass() * lever().cross(v.angular());
895 const Vector3 & mv_mcxw = mass() * v.linear() - mcxw;
896 return Force(
897 v.angular().cross(mv_mcxw),
898 v.angular().cross(lever().cross(mv_mcxw) + inertia() * v.angular())
899 - v.linear().cross(mcxw));
900 }
901
902 void disp_impl(std::ostream & os) const
903 {
904 os << " m = " << mass() << "\n"
905 << " c = " << lever().transpose() << "\n"
906 << " I = \n"
907 << inertia().matrix() << "";
908 }
909
911 template<typename NewScalar>
913 {
915 pinocchio::cast<NewScalar>(mass()), lever().template cast<NewScalar>(),
916 inertia().template cast<NewScalar>());
917 }
918
919 // TODO: adjust code
920 // /// \brief Check whether *this is a valid inertia, resulting from a positive mass
921 // distribution bool isValid() const
922 // {
923 // return
924 // (m_mass > Scalar(0) && m_inertia.isValid())
925 // || (m_mass == Scalar(0) && (m_inertia.data().array() == Scalar(0)).all());
926 // }
927
928 protected:
929 Scalar m_mass;
930 Vector3 m_com;
931 Symmetric3 m_inertia;
932
933 }; // class InertiaTpl
934
943 template<typename _Scalar, int _Options>
945 {
946 typedef _Scalar Scalar;
947 enum
948 {
949 Options = _Options,
950 };
951 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
952 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
953 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
954 typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
956
958 Vector3 h;
959 Matrix3 sigma;
960
961 PseudoInertiaTpl(Scalar mass, const Vector3 & h, const Matrix3 & sigma)
962 : mass(mass)
963 , h(h)
964 , sigma(sigma)
965 {
966 }
967
972 Matrix4 toMatrix() const
973 {
974 Matrix4 pseudo_inertia = Matrix4::Zero();
975 pseudo_inertia.template block<3, 3>(0, 0) = sigma;
976 pseudo_inertia.template block<3, 1>(0, 3) = h;
977 pseudo_inertia.template block<1, 3>(3, 0) = h.transpose();
978 pseudo_inertia(3, 3) = mass;
979 return pseudo_inertia;
980 }
981
988 {
989 Scalar mass = pseudo_inertia(3, 3);
990 Vector3 h = pseudo_inertia.template block<3, 1>(0, 3);
991 Matrix3 sigma = pseudo_inertia.template block<3, 3>(0, 0);
992 return PseudoInertiaTpl(mass, h, sigma);
993 }
994
1000 template<typename Vector10Like>
1001 static PseudoInertiaTpl
1002 FromDynamicParameters(const Eigen::MatrixBase<Vector10Like> & dynamic_params)
1003 {
1004 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector10Like, dynamic_params, 10, 1);
1006 Vector3 h = dynamic_params.template segment<3>(1);
1007 Matrix3 I_bar;
1008 // clang-format off
1012 // clang-format on
1013
1014 Matrix3 sigma = 0.5 * I_bar.trace() * Matrix3::Identity() - I_bar;
1015 return PseudoInertiaTpl(mass, h, sigma);
1016 }
1017
1022 Vector10 toDynamicParameters() const
1023 {
1024 Matrix3 I_bar = sigma.trace() * Matrix3::Identity() - sigma;
1025
1026 Vector10 dynamic_params;
1027 dynamic_params[0] = mass;
1028 dynamic_params.template segment<3>(1) = h;
1029 dynamic_params[4] = I_bar(0, 0);
1030 dynamic_params[5] = I_bar(0, 1);
1031 dynamic_params[6] = I_bar(1, 1);
1032 dynamic_params[7] = I_bar(0, 2);
1033 dynamic_params[8] = I_bar(1, 2);
1034 dynamic_params[9] = I_bar(2, 2);
1035
1036 return dynamic_params;
1037 }
1038
1045 {
1046 Vector10 dynamic_params = inertia.toDynamicParameters();
1048 }
1049
1059
1070
1072 template<typename NewScalar>
1079
1080 void disp_impl(std::ostream & os) const
1081 {
1082 os << " m = " << mass << "\n"
1083 << " h = " << h.transpose() << "\n"
1084 << " sigma = \n"
1085 << sigma << "";
1086 }
1087
1088 friend std::ostream & operator<<(std::ostream & os, const PseudoInertiaTpl & pi)
1089 {
1090 pi.disp_impl(os);
1091 return os;
1092 }
1093 };
1094
1102 template<typename _Scalar, int _Options>
1104 {
1105 typedef _Scalar Scalar;
1106 enum
1107 {
1108 Options = _Options,
1109 };
1110 typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
1111 typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10;
1113
1114 Vector10 parameters;
1119 explicit LogCholeskyParametersTpl(const Vector10 & log_cholesky)
1121 {
1122 }
1127 Vector10 toDynamicParameters() const
1128 {
1129 using math::exp;
1130 using math::pow;
1131
1132 // clang-format off
1133 Vector10 dynamic_params;
1134
1135 const Scalar alpha = parameters[0];
1136 const Scalar d1 = parameters[1];
1137 const Scalar d2 = parameters[2];
1138 const Scalar d3 = parameters[3];
1139 const Scalar s12 = parameters[4];
1140 const Scalar s23 = parameters[5];
1141 const Scalar s13 = parameters[6];
1142 const Scalar t1 = parameters[7];
1143 const Scalar t2 = parameters[8];
1144 const Scalar t3 = parameters[9];
1145
1146 const Scalar exp_d1 = exp(d1);
1147 const Scalar exp_d2 = exp(d2);
1148 const Scalar exp_d3 = exp(d3);
1149
1150 dynamic_params[0] = 1;
1151 dynamic_params[1] = t1;
1152 dynamic_params[2] = t2;
1153 dynamic_params[3] = t3;
1154 dynamic_params[4] = pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + pow(exp_d2, 2) + pow(exp_d3, 2);
1155 dynamic_params[5] = -s12 * exp_d2 - s13 * s23 - t1 * t2;
1156 dynamic_params[6] = pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + pow(exp_d1, 2) + pow(exp_d3, 2);
1157 dynamic_params[7] = -s13 * exp_d3 - t1 * t3;
1158 dynamic_params[8] = -s23 * exp_d3 - t2 * t3;
1159 dynamic_params[9] = pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + pow(exp_d1, 2) + pow(exp_d2, 2);
1160
1161 const Scalar exp_2_alpha = exp(2 * alpha);
1163 // clang-format on
1164
1165 return dynamic_params;
1166 }
1167
1173 {
1175 return PseudoInertia::FromDynamicParameters(dynamic_params);
1176 }
1177
1187
1192 Matrix10 calculateJacobian() const
1193 {
1194 using math::exp;
1195 using math::pow;
1196
1197 Matrix10 jacobian = Matrix10::Zero();
1198
1199 const Scalar alpha = parameters[0];
1200 const Scalar d1 = parameters[1];
1201 const Scalar d2 = parameters[2];
1202 const Scalar d3 = parameters[3];
1203 const Scalar s12 = parameters[4];
1204 const Scalar s23 = parameters[5];
1205 const Scalar s13 = parameters[6];
1206 const Scalar t1 = parameters[7];
1207 const Scalar t2 = parameters[8];
1208 const Scalar t3 = parameters[9];
1209
1210 const Scalar exp_2alpha = exp(2 * alpha);
1211 const Scalar exp_2d1 = exp(2 * d1);
1212 const Scalar exp_2d2 = exp(2 * d2);
1213 const Scalar exp_2d3 = exp(2 * d3);
1214 // const Scalar exp_d1 = exp(d1);
1215 const Scalar exp_d2 = exp(d2);
1216 const Scalar exp_d3 = exp(d3);
1217
1218 // clang-format off
1219 jacobian(0, 0) = 2 * exp_2alpha;
1220
1221 jacobian(1, 0) = 2 * t1 * exp_2alpha;
1222 jacobian(1, 7) = exp_2alpha;
1223
1224 jacobian(2, 0) = 2 * t2 * exp_2alpha;
1225 jacobian(2, 8) = exp_2alpha;
1226
1227 jacobian(3, 0) = 2 * t3 * exp_2alpha;
1228 jacobian(3, 9) = exp_2alpha;
1229
1230 jacobian(4, 0) = 2 * (pow(s23, 2) + pow(t2, 2) + pow(t3, 2) + exp_2d2 + exp_2d3) * exp_2alpha;
1231 jacobian(4, 2) = 2 * exp_2alpha * exp_2d2;
1232 jacobian(4, 3) = 2 * exp_2alpha * exp_2d3;
1233 jacobian(4, 5) = 2 * s23 * exp_2alpha;
1234 jacobian(4, 8) = 2 * t2 * exp_2alpha;
1235 jacobian(4, 9) = 2 * t3 * exp_2alpha;
1236
1237 jacobian(5, 0) = -2 * (s12 * exp_d2 + s13 * s23 + t1 * t2) * exp_2alpha;
1238 jacobian(5, 2) = -s12 * exp_2alpha * exp_d2;
1239 jacobian(5, 4) = -exp_2alpha * exp_d2;
1240 jacobian(5, 5) = -s13 * exp_2alpha;
1241 jacobian(5, 6) = -s23 * exp_2alpha;
1242 jacobian(5, 7) = -t2 * exp_2alpha;
1243 jacobian(5, 8) = -t1 * exp_2alpha;
1244
1245 jacobian(6, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(t1, 2) + pow(t3, 2) + exp_2d1 + exp_2d3) * exp_2alpha;
1246 jacobian(6, 1) = 2 * exp_2alpha * exp_2d1;
1247 jacobian(6, 3) = 2 * exp_2alpha * exp_2d3;
1248 jacobian(6, 4) = 2 * s12 * exp_2alpha;
1249 jacobian(6, 6) = 2 * s13 * exp_2alpha;
1250 jacobian(6, 7) = 2 * t1 * exp_2alpha;
1251 jacobian(6, 9) = 2 * t3 * exp_2alpha;
1252
1253 jacobian(7, 0) = -2 * (s13 * exp_d3 + t1 * t3) * exp_2alpha;
1254 jacobian(7, 3) = -s13 * exp_2alpha * exp_d3;
1255 jacobian(7, 6) = -exp_2alpha * exp_d3;
1256 jacobian(7, 7) = -t3 * exp_2alpha;
1257 jacobian(7, 9) = -t1 * exp_2alpha;
1258
1259 jacobian(8, 0) = -2 * (s23 * exp_d3 + t2 * t3) * exp_2alpha;
1260 jacobian(8, 3) = -s23 * exp_2alpha * exp_d3;
1261 jacobian(8, 5) = -exp_2alpha * exp_d3;
1262 jacobian(8, 8) = -t3 * exp_2alpha;
1263 jacobian(8, 9) = -t2 * exp_2alpha;
1264
1265 jacobian(9, 0) = 2 * (pow(s12, 2) + pow(s13, 2) + pow(s23, 2) + pow(t1, 2) + pow(t2, 2) + exp_2d1 + exp_2d2) * exp_2alpha;
1266 jacobian(9, 1) = 2 * exp_2alpha * exp_2d1;
1267 jacobian(9, 2) = 2 * exp_2alpha * exp_2d2;
1268 jacobian(9, 4) = 2 * s12 * exp_2alpha;
1269 jacobian(9, 5) = 2 * s23 * exp_2alpha;
1270 jacobian(9, 6) = 2 * s13 * exp_2alpha;
1271 jacobian(9, 7) = 2 * t1 * exp_2alpha;
1272 jacobian(9, 8) = 2 * t2 * exp_2alpha;
1273 // clang-format on
1274
1275 return jacobian;
1276 }
1277
1279 template<typename NewScalar>
1284
1285 void disp_impl(std::ostream & os) const
1286 {
1287 os << " alpha: " << parameters[0] << "\n"
1288 << " d1: " << parameters[1] << "\n"
1289 << " d2: " << parameters[2] << "\n"
1290 << " d3: " << parameters[3] << "\n"
1291 << " s12: " << parameters[4] << "\n"
1292 << " s23: " << parameters[5] << "\n"
1293 << " s13: " << parameters[6] << "\n"
1294 << " t1: " << parameters[7] << "\n"
1295 << " t2: " << parameters[8] << "\n"
1296 << " t3: " << parameters[9] << "";
1297 }
1298
1299 friend std::ostream & operator<<(std::ostream & os, const LogCholeskyParametersTpl & lcp)
1300 {
1301 lcp.disp_impl(os);
1302 return os;
1303 }
1304 };
1305
1306} // namespace pinocchio
1307
1308#endif // ifndef __pinocchio_spatial_inertia_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
void skewSquare(const Eigen::MatrixBase< V1 > &u, const Eigen::MatrixBase< V2 > &v, const Eigen::MatrixBase< Matrix3 > &C)
Computes the square cross product linear operator C(u,v) such that for any vector w,...
Definition skew.hpp:182
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition skew.hpp:228
void unSkew(const Eigen::MatrixBase< Matrix3 > &M, const Eigen::MatrixBase< Vector3 > &v)
Inverse of skew operator. From a given skew-symmetric matrix M of dimension 3x3, it extracts the supp...
Definition skew.hpp:93
void alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar....
Definition skew.hpp:134
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition skew.hpp:22
static void vxi(const MotionDense< MotionDerived > &v, const Derived &I, const Eigen::MatrixBase< M6 > &Iout)
Time variation operator. It computes the time derivative of an inertia I corresponding to the formula...
Definition inertia.hpp:145
Derived se3Action(const SE3Tpl< S2, O2 > &M) const
aI = aXb.act(bI)
Definition inertia.hpp:210
Derived se3ActionInverse(const SE3Tpl< S2, O2 > &M) const
bI = aXb.actInv(aI)
Definition inertia.hpp:217
static void ivx(const MotionDense< MotionDerived > &v, const Derived &I, const Eigen::MatrixBase< M6 > &Iout)
Time variation operator. It computes the time derivative of an inertia I corresponding to the formula...
Definition inertia.hpp:169
static InertiaTpl FromSphere(const Scalar mass, const Scalar radius)
Computes the Inertia of a sphere defined by its mass and its radius.
Definition inertia.hpp:375
static InertiaTpl FromBox(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
Computes the Inertia of a box defined by its mass and main dimensions (x,y,z).
Definition inertia.hpp:424
PseudoInertia toPseudoInertia() const
Convert the InertiaTpl object to a 4x4 pseudo inertia matrix.
Definition inertia.hpp:592
static InertiaTpl FromEllipsoid(const Scalar mass, const Scalar x, const Scalar y, const Scalar z)
Computes the Inertia of an ellipsoid defined by its mass and main semi-axis dimensions (x,...
Definition inertia.hpp:390
static InertiaTpl FromDynamicParameters(const Eigen::MatrixBase< Vector10Like > &params)
Definition inertia.hpp:562
Vector10 toDynamicParameters() const
Definition inertia.hpp:542
InertiaTpl< NewScalar, Options > cast() const
Definition inertia.hpp:912
InertiaTpl se3Action_impl(const SE3Tpl< S2, O2 > &M) const
aI = aXb.act(bI)
Definition inertia.hpp:873
static InertiaTpl FromPseudoInertia(const PseudoInertia &pseudo_inertia)
Create an InertiaTpl object from a PseudoInertia object.
Definition inertia.hpp:582
static InertiaTpl FromCapsule(const Scalar mass, const Scalar radius, const Scalar height)
Computes the Inertia of a capsule defined by its mass, radius and length along the Z axis....
Definition inertia.hpp:440
static InertiaTpl FromCylinder(const Scalar mass, const Scalar radius, const Scalar length)
Computes the Inertia of a cylinder defined by its mass, radius and length along the Z axis.
Definition inertia.hpp:407
static InertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters &log_cholesky)
Create an InertiaTpl object from log Cholesky parameters.
Definition inertia.hpp:605
InertiaTpl se3ActionInverse_impl(const SE3Tpl< S2, O2 > &M) const
bI = aXb.actInv(aI)
Definition inertia.hpp:884
A structure representing log Cholesky parameters.
Definition inertia.hpp:1104
Matrix10 calculateJacobian() const
Calculates the Jacobian of the log Cholesky parameters.
Definition inertia.hpp:1192
Vector10 parameters
10-dimensional vector of log Cholesky parameters
Definition inertia.hpp:1114
PseudoInertia toPseudoInertia() const
Converts the LogCholeskyParametersTpl object to a PseudoInertiaTpl object.
Definition inertia.hpp:1172
LogCholeskyParametersTpl< NewScalar, Options > cast() const
Definition inertia.hpp:1280
Vector10 toDynamicParameters() const
Converts the LogCholeskyParametersTpl object to dynamic parameters.
Definition inertia.hpp:1127
InertiaTpl< Scalar, Options > toInertia() const
Converts the LogCholeskyParametersTpl object to an InertiaTpl object.
Definition inertia.hpp:1182
LogCholeskyParametersTpl(const Vector10 &log_cholesky)
Constructor for LogCholeskyParametersTpl.
Definition inertia.hpp:1119
A structure representing a pseudo inertia matrix.
Definition inertia.hpp:945
static PseudoInertiaTpl FromMatrix(const Matrix4 &pseudo_inertia)
Constructs a PseudoInertiaTpl object from a 4x4 pseudo inertia matrix.
Definition inertia.hpp:987
static PseudoInertiaTpl FromInertia(const InertiaTpl< Scalar, Options > &inertia)
Constructs a PseudoInertiaTpl object from an InertiaTpl object.
Definition inertia.hpp:1044
static PseudoInertiaTpl FromLogCholeskyParameters(const LogCholeskyParameters &log_cholesky)
Constructs a PseudoInertiaTpl object from log Cholesky parameters.
Definition inertia.hpp:1065
Matrix3 sigma
3x3 matrix part of the pseudo inertia
Definition inertia.hpp:959
Matrix4 toMatrix() const
Converts the PseudoInertiaTpl object to a 4x4 matrix.
Definition inertia.hpp:972
Vector10 toDynamicParameters() const
Converts the PseudoInertiaTpl object to dynamic parameters.
Definition inertia.hpp:1022
Vector3 h
Vector part of the pseudo inertia.
Definition inertia.hpp:958
InertiaTpl< Scalar, Options > toInertia() const
Converts the PseudoInertiaTpl object to an InertiaTpl object.
Definition inertia.hpp:1054
static PseudoInertiaTpl FromDynamicParameters(const Eigen::MatrixBase< Vector10Like > &dynamic_params)
Constructs a PseudoInertiaTpl object from dynamic parameters.
Definition inertia.hpp:1002
PseudoInertiaTpl< NewScalar, Options > cast() const
Definition inertia.hpp:1073
Scalar mass
Mass of the pseudo inertia.
Definition inertia.hpp:957
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72