275 typedef typename Symmetric3::AlphaSkewSquare AlphaSkewSquare;
276 typedef Eigen::Matrix<Scalar, 10, 1, Options> Vector10;
277 typedef Eigen::Matrix<Scalar, 10, 10, Options> Matrix10;
296 mass() =
I6(LINEAR, LINEAR);
317 : m_mass(clone.mass())
318 , m_com(clone.lever())
319 , m_inertia(clone.inertia())
325 m_mass = clone.mass();
326 m_com = clone.lever();
327 m_inertia = clone.inertia();
331 template<
typename S2,
int O2>
340 return InertiaTpl(Scalar(0), Vector3::Zero(), Symmetric3::Zero());
352 return InertiaTpl(Scalar(1), Vector3::Zero(), Symmetric3::Identity());
359 inertia().setIdentity();
366 Eigen::internal::random<Scalar>() + 1, Vector3::Random(), Symmetric3::RandomPositive());
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);
396 mass, Vector3::Zero(),
Symmetric3(
a, Scalar(0), b, Scalar(0), Scalar(0), c));
410 const Scalar
a = mass * (
radius_square / Scalar(4) + length * length / Scalar(12));
413 mass, Vector3::Zero(),
Symmetric3(
a, Scalar(0),
a, Scalar(0), Scalar(0), c));
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);
430 mass, Vector3::Zero(),
Symmetric3(
a, Scalar(0), b, Scalar(0), Scalar(0), c));
442 const Scalar pi = boost::math::constants::pi<Scalar>();
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;
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);
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);
470 mass, Vector3::Zero(),
Symmetric3(
ix, Scalar(0),
ix, Scalar(0), Scalar(0),
iz));
475 mass() =
static_cast<Scalar
>(std::rand()) /
static_cast<Scalar
>(
RAND_MAX);
477 inertia().setRandom();
480 template<
typename Matrix6Like>
481 void matrix_impl(
const Eigen::MatrixBase<Matrix6Like> & M_)
const
483 Matrix6Like & M = M_.const_cast_derived();
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();
493 Matrix6 matrix_impl()
const
500 template<
typename Matrix6Like>
501 void inverse_impl(
const Eigen::MatrixBase<Matrix6Like> & M_)
const
503 Matrix6Like & M = M_.const_cast_derived();
504 inertia().inverse(M.template block<3, 3>(ANGULAR, ANGULAR));
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();
510 const Scalar &cx = lever()[0], cy = lever()[1], cz = lever()[2];
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);
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);
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);
524 const Scalar m_inv = Scalar(1) / mass();
525 M.template block<3, 3>(LINEAR, LINEAR).diagonal().array() += m_inv;
528 Matrix6 inverse_impl()
const
547 v.template
segment<3>(1).noalias() = mass() * lever();
548 v.template
segment<6>(4) = (inertia() - AlphaSkewSquare(mass(), lever())).data();
561 template<
typename Vector10Like>
566 const Scalar & mass =
params[0];
614 mass() = clone.mass();
615 lever() = clone.lever();
616 inertia() = clone.inertia();
621 bool isEqual(
const InertiaTpl & Y2)
const
623 return (mass() == Y2.mass()) && (lever() == Y2.lever()) && (inertia() == Y2.inertia());
627 const InertiaTpl & other,
628 const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
631 return fabs(
static_cast<Scalar
>(mass() - other.mass())) <= prec
632 && lever().isApprox(other.lever(), prec) && inertia().isApprox(other.inertia(), prec);
635 bool isZero_impl(
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
const
638 return fabs(mass()) <= prec && lever().isZero(prec) && inertia().isZero(prec);
641 InertiaTpl __plus__(
const InertiaTpl & Yb)
const
648 const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
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();
654 mab, (mass() * lever() + Yb.mass() * Yb.lever()) * mab_inv,
655 inertia() + Yb.inertia()
656 - (mass() * Yb.mass() * mab_inv) *
typename Symmetric3::SkewSquare(AB));
659 InertiaTpl & __pequ__(
const InertiaTpl & Yb)
661 static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
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();
669 inertia() += Yb.inertia();
670 inertia() -= (Ya.mass() * Yb.mass() * mab_inv) *
typename Symmetric3::SkewSquare(AB);
675 InertiaTpl __minus__(
const InertiaTpl & Yb)
const
682 const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
684 const Scalar ma = mass() - Yb.mass();
685 assert(check_expression_if_real<Scalar>(ma >= Scalar(0)));
687 const Scalar ma_inv = Scalar(1) / math::max(ma, eps);
688 const Vector3 c_a((mass() * lever() - Yb.mass() * Yb.lever()) * ma_inv);
690 const Vector3 AB = c_a - Yb.lever();
694 inertia() - Yb.inertia() + (ma * Yb.mass() / mass()) *
typename Symmetric3::SkewSquare(AB));
697 InertiaTpl & __mequ__(
const InertiaTpl & Yb)
699 static const Scalar eps = ::Eigen::NumTraits<Scalar>::epsilon();
701 const Scalar ma = mass() - Yb.mass();
702 assert(check_expression_if_real<Scalar>(ma >= Scalar(0)));
704 const Scalar ma_inv = Scalar(1) / math::max(ma, eps);
706 lever() *= (mass() * ma_inv);
707 lever().noalias() -= (Yb.mass() * ma_inv) * Yb.lever();
709 const Vector3 AB = lever() - Yb.lever();
710 inertia() -= Yb.inertia();
711 inertia() += (ma * Yb.mass() / mass()) *
typename Symmetric3::SkewSquare(AB);
717 template<
typename MotionDerived>
718 ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options>
719 __mult__(
const MotionDense<MotionDerived> & v)
const
721 typedef ForceTpl<typename traits<MotionDerived>::Scalar, traits<MotionDerived>::Options>
728 template<
typename MotionDerived,
typename ForceDerived>
729 void __mult__(
const MotionDense<MotionDerived> & v, ForceDense<ForceDerived> & f)
const
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());
737 template<
typename MotionDerived>
738 Scalar vtiv_impl(
const MotionDense<MotionDerived> & v)
const
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());
749 template<
typename MotionDerived>
750 Matrix6 variation(
const MotionDense<MotionDerived> & v)
const
753 const Motion mv(v * mass());
755 res.template block<3, 3>(LINEAR, ANGULAR) =
757 res.template block<3, 3>(ANGULAR, LINEAR) =
758 res.template block<3, 3>(LINEAR, ANGULAR).transpose();
763 res.template block<3, 3>(ANGULAR, ANGULAR) =
766 res.template block<3, 3>(LINEAR, LINEAR) =
767 (inertia() - AlphaSkewSquare(mass(), lever())).matrix();
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));
774 res.template block<3, 3>(LINEAR, LINEAR).setZero();
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)
784 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6);
785 M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout);
788 alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR));
790 const Vector3 mc(I.mass() * I.lever());
793 skewSquare(-v.angular(), mc, Iout_.template block<3, 3>(LINEAR, ANGULAR));
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);
800 skewSquare(-v.linear(), mc, Iout_.template block<3, 3>(ANGULAR, 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());
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)
818 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6, 6, 6);
819 M6 & Iout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, Iout);
822 alphaSkew(I.mass(), v.angular(), Iout_.template block<3, 3>(LINEAR, LINEAR));
825 const Vector3 mc(I.mass() * I.lever());
826 skewSquare(mc, v.angular(), Iout_.template block<3, 3>(ANGULAR, LINEAR));
829 alphaSkew(I.mass(), v.linear(), Iout_.template block<3, 3>(LINEAR, ANGULAR));
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));
841 Iout_.template block<3, 3>(LINEAR, ANGULAR) -= Iout_.template block<3, 3>(ANGULAR, LINEAR);
849 const Vector3 & lever()
const
853 const Symmetric3 & inertia()
const
866 Symmetric3 & inertia()
872 template<
typename S2,
int O2>
879 mass(), M.translation() + M.rotation() * lever(), inertia().rotate(M.rotation()));
883 template<
typename S2,
int O2>
887 mass(), M.rotation().transpose() * (lever() - M.translation()),
888 inertia().rotate(M.rotation().transpose()));
891 template<
typename MotionDerived>
894 const Vector3 &
mcxw = mass() * lever().cross(v.angular());
895 const Vector3 &
mv_mcxw = mass() * v.linear() -
mcxw;
898 v.angular().cross(lever().
cross(
mv_mcxw) + inertia() * v.angular())
899 - v.linear().cross(
mcxw));
902 void disp_impl(std::ostream & os)
const
904 os <<
" m = " << mass() <<
"\n"
905 <<
" c = " << lever().transpose() <<
"\n"
907 << inertia().matrix() <<
"";
911 template<
typename NewScalar>