{

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());

}

{

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));

}

{

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()));

}

{

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());

{

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);

{

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()));

}

{

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;

}

{

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>()));

{

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));