spatial.cpp

Go to the documentation of this file.

//
// Copyright (c) 2015-2021 CNRS INRIA
//
 
#include <iostream>
 
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/spatial/skew.hpp"
#include "pinocchio/spatial/cartesian-axis.hpp"
#include "pinocchio/spatial/spatial-axis.hpp"
 
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
 
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
 
BOOST_AUTO_TEST_CASE(test_SE3)
{
  using namespace pinocchio;
  typedef SE3::HomogeneousMatrixType HomogeneousMatrixType;
  typedef SE3::ActionMatrixType ActionMatrixType;
  typedef SE3::Vector3 Vector3;
  typedef Eigen::Matrix<double, 4, 1> Vector4;
 
  const SE3 identity = SE3::Identity();
 
  typedef SE3::Quaternion Quaternion;
  typedef SE3::Vector4 Vector4;
  Quaternion quat(Vector4::Random().normalized());
 
  SE3 m_from_quat(quat, Vector3::Random());
 
  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb * bmc;
 
  HomogeneousMatrixType aMb = amb;
  HomogeneousMatrixType bMc = bmc;
 
  // Test internal product
  HomogeneousMatrixType aMc = amc;
  BOOST_CHECK(aMc.isApprox(aMb * bMc));
 
  HomogeneousMatrixType bMa = amb.inverse();
  BOOST_CHECK(bMa.isApprox(aMb.inverse()));
 
  // Test point action
  Vector3 p = Vector3::Random();
  Vector4 p4;
  p4.head(3) = p;
  p4[3] = 1;
 
  Vector3 Mp = (aMb * p4).head(3);
  BOOST_CHECK(amb.act(p).isApprox(Mp));
 
  Vector3 Mip = (aMb.inverse() * p4).head(3);
  BOOST_CHECK(amb.actInv(p).isApprox(Mip));
 
  // Test action matrix
  ActionMatrixType aXb = amb;
  ActionMatrixType bXc = bmc;
  ActionMatrixType aXc = amc;
  BOOST_CHECK(aXc.isApprox(aXb * bXc));
 
  ActionMatrixType bXa = amb.inverse();
  BOOST_CHECK(bXa.isApprox(aXb.inverse()));
 
  ActionMatrixType X_identity = identity.toActionMatrix();
  BOOST_CHECK(X_identity.isIdentity());
 
  ActionMatrixType X_identity_inverse = identity.toActionMatrixInverse();
  BOOST_CHECK(X_identity_inverse.isIdentity());
 
  // Test dual action matrix
  BOOST_CHECK(aXb.inverse().transpose().isApprox(amb.toDualActionMatrix()));
 
  // Test isIdentity
  BOOST_CHECK(identity.isIdentity());
 
  // Test isApprox
  BOOST_CHECK(identity.isApprox(identity));
 
  // Test cast
  typedef SE3Tpl<float> SE3f;
  SE3f::Matrix3 rot_float(amb.rotation().cast<float>());
  SE3f amb_float = amb.cast<float>();
  BOOST_CHECK(amb_float.isApprox(amb.cast<float>()));
 
  SE3f amb_float2(amb);
  BOOST_CHECK(amb_float.isApprox(amb_float2));
 
  // Test actInv
  const SE3 M = SE3::Random();
  const SE3 Minv = M.inverse();
 
  BOOST_CHECK(Minv.actInv(Minv).isIdentity());
  BOOST_CHECK(M.actInv(identity).isApprox(Minv));
 
  // Test normalization
  {
    const double prec = Eigen::NumTraits<double>::dummy_precision();
    SE3 M(SE3::Random());
    M.rotation() += prec * SE3::Matrix3::Random();
    BOOST_CHECK(!M.isNormalized());
 
    SE3 M_normalized = M.normalized();
    BOOST_CHECK(M_normalized.isNormalized());
 
    M.normalize();
    BOOST_CHECK(M.isNormalized());
  }
}
 
BOOST_AUTO_TEST_CASE(test_Motion)
{
  using namespace pinocchio;
  typedef SE3::ActionMatrixType ActionMatrixType;
  typedef Motion::Vector6 Vector6;
 
  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb * bmc;
 
  Motion bv = Motion::Random();
  Motion bv2 = Motion::Random();
 
  typedef MotionBase<Motion> Base;
 
  Vector6 bv_vec = bv;
  Vector6 bv2_vec = bv2;
 
  // std::stringstream
  std::stringstream ss;
  ss << bv << std::endl;
  BOOST_CHECK(!ss.str().empty());
 
  // Test .+.
  Vector6 bvPbv2_vec = bv + bv2;
  BOOST_CHECK(bvPbv2_vec.isApprox(bv_vec + bv2_vec));
 
  Motion bplus = static_cast<Base &>(bv) + static_cast<Base &>(bv2);
  BOOST_CHECK((bv + bv2).isApprox(bplus));
 
  Motion v_not_zero(Vector6::Ones());
  BOOST_CHECK(!v_not_zero.isZero());
 
  Motion v_zero(Vector6::Zero());
  BOOST_CHECK(v_zero.isZero());
 
  // Test == and !=
  BOOST_CHECK(bv == bv);
  BOOST_CHECK(!(bv != bv));
 
  // Test -.
  Vector6 Mbv_vec = -bv;
  BOOST_CHECK(Mbv_vec.isApprox(-bv_vec));
 
  // Test .+=.
  Motion bv3 = bv;
  bv3 += bv2;
  BOOST_CHECK(bv3.toVector().isApprox(bv_vec + bv2_vec));
 
  // Test .=V6
  bv3 = bv2_vec;
  BOOST_CHECK(bv3.toVector().isApprox(bv2_vec));
 
  // Test scalar*M6
  Motion twicebv(2. * bv);
  BOOST_CHECK(twicebv.isApprox(Motion(2. * bv.toVector())));
 
  // Test M6*scalar
  Motion bvtwice(bv * 2.);
  BOOST_CHECK(bvtwice.isApprox(twicebv));
 
  // Test M6/scalar
  Motion bvdividedbytwo(bvtwice / 2.);
  BOOST_CHECK(bvdividedbytwo.isApprox(bv));
 
  // Test constructor from V6
  Motion bv4(bv2_vec);
  BOOST_CHECK(bv4.toVector().isApprox(bv2_vec));
 
  // Test action
  ActionMatrixType aXb = amb;
  BOOST_CHECK(amb.act(bv).toVector().isApprox(aXb * bv_vec));
 
  // Test action inverse
  ActionMatrixType bXc = bmc;
  BOOST_CHECK(bmc.actInv(bv).toVector().isApprox(bXc.inverse() * bv_vec));
 
  // Test double action
  Motion cv = Motion::Random();
  bv = bmc.act(cv);
  BOOST_CHECK(amb.act(bv).toVector().isApprox(amc.act(cv).toVector()));
 
  // Simple test for cross product vxv
  Motion vxv = bv.cross(bv);
  BOOST_CHECK_SMALL(
    vxv.toVector().tail(3).norm(),
    1e-3); // previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
 
  // Test Action Matrix
  Motion v2xv = bv2.cross(bv);
  Motion::ActionMatrixType actv2 = bv2.toActionMatrix();
 
  BOOST_CHECK(v2xv.toVector().isApprox(actv2 * bv.toVector()));
 
  // Test Dual Action Matrix
  Force f(bv.toVector());
  Force v2xf = bv2.cross(f);
  Motion::ActionMatrixType dualactv2 = bv2.toDualActionMatrix();
 
  BOOST_CHECK(v2xf.toVector().isApprox(dualactv2 * f.toVector()));
  BOOST_CHECK(dualactv2.isApprox(-actv2.transpose()));
 
  // Simple test for cross product vxf
  Force vxf = bv.cross(f);
  BOOST_CHECK(vxf.linear().isApprox(bv.angular().cross(f.linear())));
  BOOST_CHECK_SMALL(
    vxf.angular().norm(),
    1e-3); // previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3));
 
  // Test frame change for vxf
  Motion av = Motion::Random();
  Force af = Force::Random();
  bv = amb.actInv(av);
  Force bf = amb.actInv(af);
  Force avxf = av.cross(af);
  Force bvxf = bv.cross(bf);
  BOOST_CHECK(avxf.toVector().isApprox(amb.act(bvxf).toVector()));
 
  // Test frame change for vxv
  av = Motion::Random();
  Motion aw = Motion::Random();
  bv = amb.actInv(av);
  Motion bw = amb.actInv(aw);
  Motion avxw = av.cross(aw);
  Motion bvxw = bv.cross(bw);
  BOOST_CHECK(avxw.toVector().isApprox(amb.act(bvxw).toVector()));
 
  // Test isApprox
  bv.toVector().setOnes();
  const double eps = 1e-6;
  BOOST_CHECK(bv == bv);
  BOOST_CHECK(bv.isApprox(bv));
  Motion bv_approx(bv);
  bv_approx.linear()[0] += eps;
  BOOST_CHECK(bv_approx.isApprox(bv, eps));
 
  // Test ref() method
  {
    Motion a(Motion::Random());
    BOOST_CHECK(a.ref().isApprox(a));
 
    const Motion b(a);
    BOOST_CHECK(b.isApprox(a.ref()));
  }
 
  // Test cast
  {
    typedef MotionTpl<float> Motionf;
    Motion a(Motion::Random());
    Motionf a_float = a.cast<float>();
    BOOST_CHECK(a_float.isApprox(a.cast<float>()));
 
    Motionf a_float2(a);
    BOOST_CHECK(a_float.isApprox(a_float2));
  }
}
 
BOOST_AUTO_TEST_CASE(test_motion_ref)
{
  using namespace pinocchio;
  typedef Motion::Vector6 Vector6;
 
  typedef MotionRef<Vector6> MotionV6;
 
  Motion v_ref(Motion::Random());
  MotionV6 v(v_ref.toVector());
 
  BOOST_CHECK(v_ref.isApprox(v));
 
  MotionV6::MotionPlain v2(v * 2.);
  Motion v2_ref(v_ref * 2.);
 
  BOOST_CHECK(v2_ref.isApprox(v2));
 
  v2 = v_ref + v;
  BOOST_CHECK(v2_ref.isApprox(v2));
 
  v = v2;
  BOOST_CHECK(v2.isApprox(v));
 
  v2 = v - v;
  BOOST_CHECK(v2.isApprox(Motion::Zero()));
 
  SE3 M(SE3::Identity());
  v2 = M.act(v);
  BOOST_CHECK(v2.isApprox(v));
 
  v2 = M.actInv(v);
  BOOST_CHECK(v2.isApprox(v));
 
  Motion v3(Motion::Random());
  v_ref.setRandom();
  v = v_ref;
  v2 = v.cross(v3);
  v2_ref = v_ref.cross(v3);
 
  BOOST_CHECK(v2.isApprox(v2_ref));
 
  v.setRandom();
  v.setZero();
  BOOST_CHECK(v.isApprox(Motion::Zero()));
 
  // Test ref() method
  {
    Vector6 v6(Vector6::Random());
    MotionV6 a(v6);
    BOOST_CHECK(a.ref().isApprox(a));
 
    const Motion b(a);
    BOOST_CHECK(b.isApprox(a.ref()));
  }
}
 
BOOST_AUTO_TEST_CASE(test_motion_zero)
{
  using namespace pinocchio;
  Motion v((MotionZero()));
 
  BOOST_CHECK(v.toVector().isZero());
  BOOST_CHECK(MotionZero() == Motion::Zero());
 
  // SE3.act
  SE3 m(SE3::Random());
  BOOST_CHECK(m.act(MotionZero()) == Motion::Zero());
  BOOST_CHECK(m.actInv(MotionZero()) == Motion::Zero());
 
  // Motion.cross
  Motion v2(Motion::Random());
  BOOST_CHECK(v2.cross(MotionZero()) == Motion::Zero());
}
 
BOOST_AUTO_TEST_CASE(test_Force)
{
  using namespace pinocchio;
  typedef SE3::ActionMatrixType ActionMatrixType;
  typedef Force::Vector6 Vector6;
 
  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb * bmc;
 
  Force bf = Force::Random();
  Force bf2 = Force::Random();
 
  Vector6 bf_vec = bf;
  Vector6 bf2_vec = bf2;
 
  // std::stringstream
  std::stringstream ss;
  ss << bf << std::endl;
  BOOST_CHECK(!ss.str().empty());
 
  // Test .+.
  Vector6 bfPbf2_vec = bf + bf2;
  BOOST_CHECK(bfPbf2_vec.isApprox(bf_vec + bf2_vec));
 
  // Test -.
  Vector6 Mbf_vec = -bf;
  BOOST_CHECK(Mbf_vec.isApprox(-bf_vec));
 
  // Test .+=.
  Force bf3 = bf;
  bf3 += bf2;
  BOOST_CHECK(bf3.toVector().isApprox(bf_vec + bf2_vec));
 
  // Test .= V6
  bf3 = bf2_vec;
  BOOST_CHECK(bf3.toVector().isApprox(bf2_vec));
 
  // Test constructor from V6
  Force bf4(bf2_vec);
  BOOST_CHECK(bf4.toVector().isApprox(bf2_vec));
 
  // Test action
  ActionMatrixType aXb = amb;
  BOOST_CHECK(amb.act(bf).toVector().isApprox(aXb.inverse().transpose() * bf_vec));
 
  // Test action inverse
  ActionMatrixType bXc = bmc;
  BOOST_CHECK(bmc.actInv(bf).toVector().isApprox(bXc.transpose() * bf_vec));
 
  // Test double action
  Force cf = Force::Random();
  bf = bmc.act(cf);
  BOOST_CHECK(amb.act(bf).toVector().isApprox(amc.act(cf).toVector()));
 
  // Simple test for cross product
  // Force vxv = bf.cross(bf);
  // ensure that (vxv.toVector().isMuchSmallerThan(bf.toVector()));
 
  Force f_not_zero(Vector6::Ones());
  BOOST_CHECK(!f_not_zero.isZero());
 
  Force f_zero(Vector6::Zero());
  BOOST_CHECK(f_zero.isZero());
 
  // Test isApprox
 
  BOOST_CHECK(bf == bf);
  bf.setRandom();
  bf2.setZero();
  BOOST_CHECK(bf == bf);
  BOOST_CHECK(bf != bf2);
  BOOST_CHECK(bf.isApprox(bf));
  BOOST_CHECK(!bf.isApprox(bf2));
 
  const double eps = 1e-6;
  Force bf_approx(bf);
  bf_approx.linear()[0] += 2. * eps;
  BOOST_CHECK(!bf_approx.isApprox(bf, eps));
 
  // Test ref() method
  {
    Force a(Force::Random());
    BOOST_CHECK(a.ref().isApprox(a));
 
    const Force b(a);
    BOOST_CHECK(b.isApprox(a.ref()));
  }
 
  // Test cast
  {
    typedef ForceTpl<float> Forcef;
    Force a(Force::Random());
    Forcef a_float = a.cast<float>();
    BOOST_CHECK(a_float.isApprox(a.cast<float>()));
 
    Forcef a_float2(a);
    BOOST_CHECK(a_float.isApprox(a_float2));
  }
 
  // Test scalar multiplication
  const double alpha = 1.5;
  Force b(Force::Random());
  Force alpha_f = alpha * b;
  Force f_alpha = b * alpha;
 
  BOOST_CHECK(alpha_f == f_alpha);
}
 
BOOST_AUTO_TEST_CASE(test_force_ref)
{
  using namespace pinocchio;
  typedef Force::Vector6 Vector6;
 
  typedef ForceRef<Vector6> ForceV6;
 
  Force f_ref(Force::Random());
  ForceV6 f(f_ref.toVector());
 
  BOOST_CHECK(f_ref.isApprox(f));
 
  ForceV6::ForcePlain f2(f * 2.);
  Force f2_ref(f_ref * 2.);
 
  BOOST_CHECK(f2_ref.isApprox(f2));
 
  f2 = f_ref + f;
  BOOST_CHECK(f2_ref.isApprox(f2));
 
  f = f2;
  BOOST_CHECK(f2.isApprox(f));
 
  f2 = f - f;
  BOOST_CHECK(f2.isApprox(Force::Zero()));
 
  SE3 M(SE3::Identity());
  f2 = M.act(f);
  BOOST_CHECK(f2.isApprox(f));
 
  f2 = M.actInv(f);
  BOOST_CHECK(f2.isApprox(f));
 
  Motion v(Motion::Random());
  f_ref.setRandom();
  f = f_ref;
  f2 = v.cross(f);
  f2_ref = v.cross(f_ref);
 
  BOOST_CHECK(f2.isApprox(f2_ref));
 
  f.setRandom();
  f.setZero();
  BOOST_CHECK(f.isApprox(Force::Zero()));
 
  // Test ref() method
  {
    Vector6 v6(Vector6::Random());
    ForceV6 a(v6);
    BOOST_CHECK(a.ref().isApprox(a));
 
    const Force b(a);
    BOOST_CHECK(b.isApprox(a.ref()));
  }
}
 
BOOST_AUTO_TEST_CASE(test_Inertia)
{
  using namespace pinocchio;
  typedef Inertia::Matrix6 Matrix6;
 
  Inertia aI = Inertia::Random();
  Matrix6 matI = aI;
  BOOST_CHECK_EQUAL(matI(0, 0), aI.mass());
  BOOST_CHECK_EQUAL(matI(1, 1), aI.mass());
  BOOST_CHECK_EQUAL(matI(2, 2), aI.mass()); // 1,1 before unifying
 
  BOOST_CHECK_SMALL(
    (matI - matI.transpose()).norm(),
    matI.norm()); // previously ensure that( (matI-matI.transpose()).isMuchSmallerThan(matI) );
  BOOST_CHECK_SMALL(
    (matI.topRightCorner<3, 3>() * aI.lever()).norm(),
    aI.lever().norm()); // previously ensure that(
                        // (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) );
 
  Inertia I1 = Inertia::Identity();
  BOOST_CHECK(I1.matrix().isApprox(Matrix6::Identity()));
 
  // Test motion-to-force map
  Motion v = Motion::Random();
  Force f = I1 * v;
  BOOST_CHECK(f.toVector().isApprox(v.toVector()));
 
  // Test Inertia group application
  SE3 bma = SE3::Random();
  Inertia bI = bma.act(aI);
  Matrix6 bXa = bma;
  BOOST_CHECK((bma.rotation() * aI.inertia().matrix() * bma.rotation().transpose())
                .isApprox(bI.inertia().matrix()));
  BOOST_CHECK((bXa.transpose().inverse() * aI.matrix() * bXa.inverse()).isApprox(bI.matrix()));
 
  // Test inverse action
  BOOST_CHECK((bXa.transpose() * bI.matrix() * bXa).isApprox(bma.actInv(bI).matrix()));
 
  // Test vxIv cross product
  v = Motion::Random();
  f = aI * v;
  Force vxf = v.cross(f);
  Force vxIv = aI.vxiv(v);
  BOOST_CHECK(vxf.toVector().isApprox(vxIv.toVector()));
 
  // Test operator+
  I1 = Inertia::Random();
  Inertia I2 = Inertia::Random();
  BOOST_CHECK((I1.matrix() + I2.matrix()).isApprox((I1 + I2).matrix()));
 
  // Test operator-
  {
    I1 = Inertia::Random();
    Inertia I2 = Inertia::Random();
    Inertia Isum = I1 + I2;
    Inertia I1_other = Isum - I2;
    BOOST_CHECK(I1_other.matrix().isApprox(I1.matrix()));
  }
 
  // Test operator +=
  Inertia I12 = I1;
  I12 += I2;
  BOOST_CHECK((I1.matrix() + I2.matrix()).isApprox(I12.matrix()));
 
  // Test operator -=
  {
    Inertia I2 = Inertia::Random();
    Inertia Isum = I1 + I2;
 
    Inertia I1_other = Isum;
    I1_other -= I2;
    BOOST_CHECK((I1_other.matrix()).isApprox(I1.matrix()));
  }
 
  // Test operator vtiv
  double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector();
  double kinetic = aI.vtiv(v);
  BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
 
  // Test constructor (Matrix6)
  Inertia I1_bis(I1.matrix());
  BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix()));
 
  // Test Inertia from ellipsoid
  const double sphere_mass = 5.;
  const double sphere_radius = 2.;
  I1 = Inertia::FromSphere(sphere_mass, sphere_radius);
  const double L_sphere = 2. / 5. * sphere_mass * sphere_radius * sphere_radius;
  BOOST_CHECK_SMALL(I1.mass() - sphere_mass, 1e-12);
  BOOST_CHECK(I1.lever().isZero());
  BOOST_CHECK(
    I1.inertia().matrix().isApprox(Symmetric3(L_sphere, 0., L_sphere, 0., 0., L_sphere).matrix()));
 
  // Test Inertia from ellipsoid
  I1 = Inertia::FromEllipsoid(2., 3., 4., 5.);
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(16.4, 0., 13.6, 0., 0., 10.).matrix()));
 
  // Test Inertia from Cylinder
  I1 = Inertia::FromCylinder(2., 4., 6.);
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(14., 0., 14., 0., 0., 16.).matrix()));
 
  // Test Inertia from Box
  I1 = Inertia::FromBox(2., 6., 12., 18.);
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(78., 0., 60., 0., 0., 30.).matrix()));
 
  // Test Inertia From Capsule
  I1 = Inertia::FromCapsule(1., 2., 3);
  BOOST_CHECK_SMALL(I1.mass() - 1, 1e-12);
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
  BOOST_CHECK(I1.inertia().matrix().isApprox(
    Symmetric3(3.79705882, 0., 3.79705882, 0., 0., 1.81176471).matrix(),
    1e-5)); // Computed with hppfcl::Capsule
 
  // Copy operator
  Inertia aI_copy(aI);
  BOOST_CHECK(aI_copy == aI);
 
  // Test isZero
  Inertia I_not_zero = Inertia::Identity();
  BOOST_CHECK(!I_not_zero.isZero());
 
  Inertia I_zero = Inertia::Zero();
  BOOST_CHECK(I_zero.isZero());
 
  // Test isApprox
  const double eps = 1e-6;
  BOOST_CHECK(aI == aI);
  BOOST_CHECK(aI.isApprox(aI));
  Inertia aI_approx(aI);
  aI_approx.mass() += eps / 2.;
  BOOST_CHECK(aI_approx.isApprox(aI, eps));
 
  // Test Variation
  Inertia::Matrix6 aIvariation = aI.variation(v);
 
  Motion::ActionMatrixType vAction = v.toActionMatrix();
  Motion::ActionMatrixType vDualAction = v.toDualActionMatrix();
 
  Inertia::Matrix6 aImatrix = aI.matrix();
  Inertia::Matrix6 aIvariation_ref = vDualAction * aImatrix - aImatrix * vAction;
 
  BOOST_CHECK(aIvariation.isApprox(aIvariation_ref));
  BOOST_CHECK(vxIv.isApprox(Force(aIvariation * v.toVector())));
 
  // Test vxI operator
  {
    typedef Inertia::Matrix6 Matrix6;
    Inertia I(Inertia::Random());
    Motion v(Motion::Random());
 
    const Matrix6 M_ref(v.toDualActionMatrix() * I.matrix());
    Matrix6 M;
    Inertia::vxi(v, I, M);
 
    BOOST_CHECK(M.isApprox(M_ref));
    BOOST_CHECK(I.vxi(v).isApprox(M_ref));
  }
 
  // Test Ivx operator
  {
    typedef Inertia::Matrix6 Matrix6;
    Inertia I(Inertia::Random());
    Motion v(Motion::Random());
 
    const Matrix6 M_ref(I.matrix() * v.toActionMatrix());
    Matrix6 M;
    Inertia::ivx(v, I, M);
 
    BOOST_CHECK(M.isApprox(M_ref));
    BOOST_CHECK(I.ivx(v).isApprox(M_ref));
  }
 
  // Test variation against vxI - Ivx operator
  {
    typedef Inertia::Matrix6 Matrix6;
    Inertia I(Inertia::Random());
    Motion v(Motion::Random());
 
    Matrix6 Ivariation = I.variation(v);
 
    Matrix6 M1;
    Inertia::vxi(v, I, M1);
    Matrix6 M2;
    Inertia::ivx(v, I, M2);
    Matrix6 M3(M1 - M2);
 
    BOOST_CHECK(M3.isApprox(Ivariation));
  }
 
  // Test inverse
  {
    Inertia I(Inertia::Random());
    Inertia::Matrix6 I_inv = I.inverse();
 
    BOOST_CHECK(I_inv.isApprox(I.matrix().inverse()));
  }
 
  // Test dynamic parameters
  {
    Inertia I(Inertia::Random());
 
    Inertia::Vector10 v = I.toDynamicParameters();
 
    BOOST_CHECK_CLOSE(v[0], I.mass(), 1e-12);
 
    BOOST_CHECK(v.segment<3>(1).isApprox(I.mass() * I.lever()));
 
    Eigen::Matrix3d I_o = I.inertia() + I.mass() * skew(I.lever()).transpose() * skew(I.lever());
    Eigen::Matrix3d I_ov;
    I_ov << v[4], v[5], v[7], v[5], v[6], v[8], v[7], v[8], v[9];
 
    BOOST_CHECK(I_o.isApprox(I_ov));
 
    Inertia I2 = Inertia::FromDynamicParameters(v);
    BOOST_CHECK(I2.isApprox(I));
  }
 
  // Test disp
  std::cout << aI << std::endl;
 
  // Test Inertia parametrizations
  {
    typedef LogCholeskyParametersTpl<double, 0> LogCholeskyParameters;
    typedef PseudoInertiaTpl<double, 0> PseudoInertia;
 
    Inertia::Vector10 eta;
    eta.setRandom();
    LogCholeskyParameters log_cholesky = LogCholeskyParameters(eta);
 
    // Convert logcholesky parametrization to pseudo-inertia
    PseudoInertia pseudo = log_cholesky.toPseudoInertia();
    Eigen::Matrix4d pseudo_matrix = pseudo.toMatrix();
    // Convert logcholesky to inertia tpl
    Inertia I_from_log_cholesky = Inertia::FromLogCholeskyParameters(log_cholesky);
 
    // Check if conversion from inertia tpl to pseudo inertia gives same result as from logcholesky
    // parametrization to pseudo-inertia
    Eigen::Matrix4d pseudo_from_inertia = I_from_log_cholesky.toPseudoInertia().toMatrix();
    BOOST_CHECK(pseudo_matrix.isApprox(pseudo_from_inertia, 1e-10));
 
    // // Check if log-cholesky parametrization to pseudo-inertia gives same result as their
    // calculations
    double alpha = log_cholesky.parameters[0];
    double d1 = log_cholesky.parameters[1];
    double d2 = log_cholesky.parameters[2];
    double d3 = log_cholesky.parameters[3];
    double s12 = log_cholesky.parameters[4];
    double s23 = log_cholesky.parameters[5];
    double s13 = log_cholesky.parameters[6];
    double t1 = log_cholesky.parameters[7];
    double t2 = log_cholesky.parameters[8];
    double t3 = log_cholesky.parameters[9];
 
    double exp_alpha = std::exp(alpha);
    double exp_d1 = std::exp(d1);
    double exp_d2 = std::exp(d2);
    double exp_d3 = std::exp(d3);
 
    Eigen::Matrix4d U;
    // clang-format off
    U << exp_d1, s12, s13, t1,
         0, exp_d2, s23, t2,
         0, 0, exp_d3, t3,
         0, 0, 0, 1;
    // clang-format on
    U *= exp_alpha;
 
    Eigen::Matrix4d pseudo_chol = U * U.transpose();
    BOOST_CHECK(pseudo_matrix.isApprox(pseudo_chol, 1e-10));
 
    // Additional checks: Convert back from pseudo-inertia to inertia and validate
    Inertia I_back = pseudo.toInertia();
    BOOST_CHECK_CLOSE(I_back.mass(), I_from_log_cholesky.mass(), 1e-12);
    BOOST_CHECK(I_back.lever().isApprox(I_from_log_cholesky.lever(), 1e-12));
    BOOST_CHECK(I_back.inertia().isApprox(I_from_log_cholesky.inertia(), 1e-12));
 
    // Convert Inertia to dynamic parameters
    Inertia::Vector10 dynamic_params_inertia = I_from_log_cholesky.toDynamicParameters();
 
    // Convert log Cholesky to dynamic parameters
    Inertia::Vector10 dynamic_params_log_cholesky = log_cholesky.toDynamicParameters();
 
    // Compare dynamic parameters from Inertia and log Cholesky
    BOOST_CHECK(dynamic_params_inertia.isApprox(dynamic_params_log_cholesky, 1e-10));
 
    // Convert Pseudo Inertia to dynamic parameters
    PseudoInertia pseudo_inertia = PseudoInertia::FromMatrix(pseudo_matrix);
    Inertia::Vector10 dynamic_params_pseudo_inertia = pseudo_inertia.toDynamicParameters();
 
    // Compare dynamic parameters from Inertia and Pseudo Inertia
    BOOST_CHECK(dynamic_params_inertia.isApprox(dynamic_params_pseudo_inertia, 1e-10));
 
    // Compare dynamic parameters from log Cholesky and Pseudo Inertia
    BOOST_CHECK(dynamic_params_log_cholesky.isApprox(dynamic_params_pseudo_inertia, 1e-10));
 
    // Calculate Jacobian of logcholesky parametrization
    Eigen::Matrix<double, 10, 10> jacobian = log_cholesky.calculateJacobian();
 
    // Check if determinant is non-zero
    BOOST_CHECK(std::abs(jacobian.determinant()) > 1e-10);
 
    // Check physical consistency by positive definiteness of pseudo inertia
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(pseudo_matrix);
    BOOST_CHECK((eigensolver.eigenvalues().array() > 0).all());
 
    // Test disp
    std::cout << I_from_log_cholesky << std::endl;
    std::cout << log_cholesky << std::endl;
    std::cout << pseudo_inertia << std::endl;
  }
}
 
BOOST_AUTO_TEST_CASE(cast_inertia)
{
  using namespace pinocchio;
  Inertia Y(Inertia::Random());
 
  typedef InertiaTpl<long double> Inertiald;
 
  BOOST_CHECK(Y.cast<double>() == Y);
  BOOST_CHECK(Y.cast<long double>().cast<double>() == Y);
 
  Inertiald Y2(Y);
  BOOST_CHECK(Y2.isApprox(Y.cast<long double>()));
}
 
BOOST_AUTO_TEST_CASE(test_ActOnSet)
{
  using namespace pinocchio;
  const int N = 20;
  typedef Eigen::Matrix<double, 6, N> Matrix6N;
  SE3 jMi = SE3::Random();
  Motion v = Motion::Random();
 
  // Forcet SET
  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
  Matrix6N iF = Matrix6N::Random(), jF, jFinv, jF_ref, jFinv_ref;
 
  // forceSet::se3Action
  forceSet::se3Action(jMi, iF, jF);
  for (int k = 0; k < N; ++k)
    BOOST_CHECK(jMi.act(Force(iF.col(k))).toVector().isApprox(jF.col(k)));
 
  jF_ref = jMi.toDualActionMatrix() * iF;
  BOOST_CHECK(jF_ref.isApprox(jF));
 
  forceSet::se3ActionInverse(jMi.inverse(), iF, jFinv);
  BOOST_CHECK(jFinv.isApprox(jF));
  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
  Matrix6N iF2 = Matrix6N::Random();
  jF_ref += jMi.toDualActionMatrix() * iF2;
 
  forceSet::se3Action<ADDTO>(jMi, iF2, jF);
  BOOST_CHECK(jF.isApprox(jF_ref));
 
  Matrix6N iF3 = Matrix6N::Random();
  jF_ref -= jMi.toDualActionMatrix() * iF3;
 
  forceSet::se3Action<RMTO>(jMi, iF3, jF);
  BOOST_CHECK(jF.isApprox(jF_ref));
 
  // forceSet::se3ActionInverse
  forceSet::se3ActionInverse(jMi, iF, jFinv);
  jFinv_ref = jMi.inverse().toDualActionMatrix() * iF;
  BOOST_CHECK(jFinv_ref.isApprox(jFinv));
 
  jFinv_ref += jMi.inverse().toDualActionMatrix() * iF2;
  forceSet::se3ActionInverse<ADDTO>(jMi, iF2, jFinv);
  BOOST_CHECK(jFinv.isApprox(jFinv_ref));
 
  jFinv_ref -= jMi.inverse().toDualActionMatrix() * iF3;
  forceSet::se3ActionInverse<RMTO>(jMi, iF3, jFinv);
  BOOST_CHECK(jFinv.isApprox(jFinv_ref));
 
  // forceSet::motionAction
  forceSet::motionAction(v, iF, jF);
  for (int k = 0; k < N; ++k)
    BOOST_CHECK(v.cross(Force(iF.col(k))).toVector().isApprox(jF.col(k)));
 
  jF_ref = v.toDualActionMatrix() * iF;
  BOOST_CHECK(jF.isApprox(jF_ref));
 
  jF_ref += v.toDualActionMatrix() * iF2;
  forceSet::motionAction<ADDTO>(v, iF2, jF);
  BOOST_CHECK(jF.isApprox(jF_ref));
 
  jF_ref -= v.toDualActionMatrix() * iF3;
  forceSet::motionAction<RMTO>(v, iF3, jF);
  BOOST_CHECK(jF.isApprox(jF_ref));
 
  // Motion SET
  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
  Matrix6N iV = Matrix6N::Random(), jV, jV_ref, jVinv, jVinv_ref;
 
  // motionSet::se3Action
  motionSet::se3Action(jMi, iV, jV);
  for (int k = 0; k < N; ++k)
    BOOST_CHECK(jMi.act(Motion(iV.col(k))).toVector().isApprox(jV.col(k)));
 
  jV_ref = jMi.toActionMatrix() * iV;
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  motionSet::se3ActionInverse(jMi.inverse(), iV, jVinv);
  BOOST_CHECK(jVinv.isApprox(jV));
  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
  Matrix6N iV2 = Matrix6N::Random();
  jV_ref += jMi.toActionMatrix() * iV2;
  motionSet::se3Action<ADDTO>(jMi, iV2, jV);
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  Matrix6N iV3 = Matrix6N::Random();
  jV_ref -= jMi.toActionMatrix() * iV3;
  motionSet::se3Action<RMTO>(jMi, iV3, jV);
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  // motionSet::se3ActionInverse
  motionSet::se3ActionInverse(jMi, iV, jVinv);
  jVinv_ref = jMi.inverse().toActionMatrix() * iV;
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));
 
  jVinv_ref += jMi.inverse().toActionMatrix() * iV2;
  motionSet::se3ActionInverse<ADDTO>(jMi, iV2, jVinv);
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));
 
  jVinv_ref -= jMi.inverse().toActionMatrix() * iV3;
  motionSet::se3ActionInverse<RMTO>(jMi, iV3, jVinv);
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));
 
  // motionSet::motionAction
  motionSet::motionAction(v, iV, jV);
  for (int k = 0; k < N; ++k)
    BOOST_CHECK(v.cross(Motion(iV.col(k))).toVector().isApprox(jV.col(k)));
 
  jV_ref = v.toActionMatrix() * iV;
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  jV_ref += v.toActionMatrix() * iV2;
  motionSet::motionAction<ADDTO>(v, iV2, jV);
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  jV_ref -= v.toActionMatrix() * iV3;
  motionSet::motionAction<RMTO>(v, iV3, jV);
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  // motionSet::inertiaAction
  const Inertia I(Inertia::Random());
  motionSet::inertiaAction(I, iV, jV);
  for (int k = 0; k < N; ++k)
    BOOST_CHECK((I * (Motion(iV.col(k)))).toVector().isApprox(jV.col(k)));
 
  jV_ref = I.matrix() * iV;
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  jV_ref += I.matrix() * iV2;
  motionSet::inertiaAction<ADDTO>(I, iV2, jV);
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  jV_ref -= I.matrix() * iV3;
  motionSet::inertiaAction<RMTO>(I, iV3, jV);
  BOOST_CHECK(jV.isApprox(jV_ref));
 
  // motionSet::act
  Force f = Force::Random();
  motionSet::act(iV, f, jF);
  for (int k = 0; k < N; ++k)
    BOOST_CHECK(Motion(iV.col(k)).cross(f).toVector().isApprox(jF.col(k)));
 
  for (int k = 0; k < N; ++k)
    jF_ref.col(k) = Force(Motion(iV.col(k)).cross(f)).toVector();
  BOOST_CHECK(jF.isApprox(jF_ref));
 
  for (int k = 0; k < N; ++k)
    jF_ref.col(k) += Force(Motion(iV2.col(k)).cross(f)).toVector();
  motionSet::act<ADDTO>(iV2, f, jF);
  BOOST_CHECK(jF.isApprox(jF_ref));
 
  for (int k = 0; k < N; ++k)
    jF_ref.col(k) -= Force(Motion(iV3.col(k)).cross(f)).toVector();
  motionSet::act<RMTO>(iV3, f, jF);
  BOOST_CHECK(jF.isApprox(jF_ref));
}
 
BOOST_AUTO_TEST_CASE(test_skew)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef Motion::Vector6 Vector6;
 
  Vector3 v3(Vector3::Random());
  Vector6 v6(Vector6::Random());
 
  Vector3 res1 = unSkew(skew(v3));
  BOOST_CHECK(res1.isApprox(v3));
 
  Vector3 res2 = unSkew(skew(v6.head<3>()));
  BOOST_CHECK(res2.isApprox(v6.head<3>()));
 
  Vector3 res3 = skew(v3) * v3;
  BOOST_CHECK(res3.isZero());
 
  Vector3 rhs(Vector3::Random());
  Vector3 res41 = skew(v3) * rhs;
  Vector3 res42 = v3.cross(rhs);
 
  BOOST_CHECK(res41.isApprox(res42));
}
 
BOOST_AUTO_TEST_CASE(test_addSkew)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;
 
  Vector3 v(Vector3::Random());
  Matrix3 M(Matrix3::Random());
  Matrix3 Mcopy(M);
 
  addSkew(v, M);
  Matrix3 Mref = Mcopy + skew(v);
  BOOST_CHECK(M.isApprox(Mref));
 
  Mref += skew(-v);
  addSkew(-v, M);
  BOOST_CHECK(M.isApprox(Mcopy));
 
  M.setZero();
  addSkew(v, M);
  BOOST_CHECK(M.isApprox(skew(v)));
}
 
BOOST_AUTO_TEST_CASE(test_skew_square)
{
  using namespace pinocchio;
  typedef SE3::Vector3 Vector3;
  typedef SE3::Matrix3 Matrix3;
 
  Vector3 u(Vector3::Random());
  Vector3 v(Vector3::Random());
 
  Matrix3 ref = skew(u) * skew(v);
 
  Matrix3 res = skewSquare(u, v);
 
  BOOST_CHECK(res.isApprox(ref));
}
 
template<int axis>
struct test_scalar_multiplication_cartesian_axis
{
  typedef pinocchio::CartesianAxis<axis> Axis;
  typedef double Scalar;
  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
 
  static void run()
  {
    const Scalar alpha = static_cast<Scalar>(rand()) / static_cast<Scalar>(RAND_MAX);
    const Vector3 r1 = Axis() * alpha;
    const Vector3 r2 = alpha * Axis();
 
    BOOST_CHECK(r1.isApprox(r2));
 
    for (int k = 0; k < Axis::dim; ++k)
    {
      if (k == axis)
      {
        BOOST_CHECK(r1[k] == alpha);
        BOOST_CHECK(r2[k] == alpha);
      }
      else
      {
        BOOST_CHECK(r1[k] == Scalar(0));
        BOOST_CHECK(r2[k] == Scalar(0));
      }
    }
  }
};
 
BOOST_AUTO_TEST_CASE(test_cartesian_axis)
{
  using namespace Eigen;
  using namespace pinocchio;
  Vector3d v(Vector3d::Random());
  const double alpha = 3;
  Vector3d v2(alpha * v);
 
  BOOST_CHECK(XAxis::cross(v).isApprox(Vector3d::Unit(0).cross(v)));
  BOOST_CHECK(YAxis::cross(v).isApprox(Vector3d::Unit(1).cross(v)));
  BOOST_CHECK(ZAxis::cross(v).isApprox(Vector3d::Unit(2).cross(v)));
  BOOST_CHECK(XAxis::alphaCross(alpha, v).isApprox(Vector3d::Unit(0).cross(v2)));
  BOOST_CHECK(YAxis::alphaCross(alpha, v).isApprox(Vector3d::Unit(1).cross(v2)));
  BOOST_CHECK(ZAxis::alphaCross(alpha, v).isApprox(Vector3d::Unit(2).cross(v2)));
 
  test_scalar_multiplication_cartesian_axis<0>::run();
  test_scalar_multiplication_cartesian_axis<1>::run();
  test_scalar_multiplication_cartesian_axis<2>::run();
 
  // test Vector value
  BOOST_CHECK(XAxis::vector() == Vector3d::UnitX());
  BOOST_CHECK(YAxis::vector() == Vector3d::UnitY());
  BOOST_CHECK(ZAxis::vector() == Vector3d::UnitZ());
}
 
template<int axis>
struct test_scalar_multiplication
{
  typedef pinocchio::SpatialAxis<axis> Axis;
  typedef double Scalar;
  typedef pinocchio::MotionTpl<Scalar> Motion;
 
  static void run()
  {
    const Scalar alpha = static_cast<Scalar>(rand()) / static_cast<Scalar>(RAND_MAX);
    const Motion r1 = Axis() * alpha;
    const Motion r2 = alpha * Axis();
 
    BOOST_CHECK(r1.isApprox(r2));
 
    for (int k = 0; k < Axis::dim; ++k)
    {
      if (k == axis)
      {
        BOOST_CHECK(r1.toVector()[k] == alpha);
        BOOST_CHECK(r2.toVector()[k] == alpha);
      }
      else
      {
        BOOST_CHECK(r1.toVector()[k] == Scalar(0));
        BOOST_CHECK(r2.toVector()[k] == Scalar(0));
      }
    }
  }
};
 
BOOST_AUTO_TEST_CASE(test_spatial_axis)
{
  using namespace pinocchio;
 
  Motion v(Motion::Random());
  Force f(Force::Random());
 
  Motion vaxis;
  vaxis << AxisVX();
  BOOST_CHECK(AxisVX::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisVX()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisVX::cross(f).isApprox(vaxis.cross(f)));
 
  vaxis << AxisVY();
  BOOST_CHECK(AxisVY::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisVY()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisVY::cross(f).isApprox(vaxis.cross(f)));
 
  vaxis << AxisVZ();
  BOOST_CHECK(AxisVZ::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisVZ()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisVZ::cross(f).isApprox(vaxis.cross(f)));
 
  vaxis << AxisWX();
  BOOST_CHECK(AxisWX::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisWX()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisWX::cross(f).isApprox(vaxis.cross(f)));
 
  vaxis << AxisWY();
  BOOST_CHECK(AxisWY::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisWY()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisWY::cross(f).isApprox(vaxis.cross(f)));
 
  vaxis << AxisWZ();
  BOOST_CHECK(AxisWZ::cross(v).isApprox(vaxis.cross(v)));
  BOOST_CHECK(v.cross(AxisWZ()).isApprox(v.cross(vaxis)));
  BOOST_CHECK(AxisWZ::cross(f).isApprox(vaxis.cross(f)));
 
  // Test operation Axis * Scalar
  test_scalar_multiplication<0>::run();
  test_scalar_multiplication<1>::run();
  test_scalar_multiplication<2>::run();
  test_scalar_multiplication<3>::run();
  test_scalar_multiplication<4>::run();
  test_scalar_multiplication<5>::run();
 
  // Operations of Constraint on forces Sxf
  typedef Motion::ActionMatrixType ActionMatrixType;
  typedef ActionMatrixType::ColXpr ColType;
  typedef ForceRef<ColType> ForceRefOnColType;
  typedef MotionRef<ColType> MotionRefOnColType;
  ActionMatrixType Sxf, Sxf_ref;
  ActionMatrixType S(ActionMatrixType::Identity());
 
  SpatialAxis<0>::cross(f, ForceRefOnColType(Sxf.col(0)));
  SpatialAxis<1>::cross(f, ForceRefOnColType(Sxf.col(1)));
  SpatialAxis<2>::cross(f, ForceRefOnColType(Sxf.col(2)));
  SpatialAxis<3>::cross(f, ForceRefOnColType(Sxf.col(3)));
  SpatialAxis<4>::cross(f, ForceRefOnColType(Sxf.col(4)));
  SpatialAxis<5>::cross(f, ForceRefOnColType(Sxf.col(5)));
 
  for (int k = 0; k < 6; ++k)
  {
    MotionRefOnColType Scol(S.col(k));
    ForceRefOnColType(Sxf_ref.col(k)) = Scol.cross(f);
  }
 
  BOOST_CHECK(Sxf.isApprox(Sxf_ref));
}
 
BOOST_AUTO_TEST_SUITE_END()