Verified Commit 8dd9a152 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

test: split joint tests

parent eb058111
......@@ -89,21 +89,29 @@ IF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(python)
ENDIF(BUILD_PYTHON_INTERFACE)
# Test over the joints
ADD_PINOCCHIO_UNIT_TEST(joints)
ADD_PINOCCHIO_UNIT_TEST(joint-revolute)
ADD_PINOCCHIO_UNIT_TEST(joint-prismatic)
ADD_PINOCCHIO_UNIT_TEST(joint-planar)
ADD_PINOCCHIO_UNIT_TEST(joint-free-flyer)
ADD_PINOCCHIO_UNIT_TEST(joint-spherical)
ADD_PINOCCHIO_UNIT_TEST(joint-translation)
ADD_PINOCCHIO_UNIT_TEST(joint-generic)
ADD_PINOCCHIO_UNIT_TEST(joint-composite)
ADD_PINOCCHIO_UNIT_TEST(joint-mimic)
ADD_PINOCCHIO_UNIT_TEST(model)
ADD_PINOCCHIO_UNIT_TEST(data)
ADD_PINOCCHIO_UNIT_TEST(constraint)
ADD_PINOCCHIO_UNIT_TEST(joints)
ADD_PINOCCHIO_UNIT_TEST(compute-all-terms)
ADD_PINOCCHIO_UNIT_TEST(energy)
ADD_PINOCCHIO_UNIT_TEST(frames)
ADD_PINOCCHIO_UNIT_TEST(joint-configurations)
ADD_PINOCCHIO_UNIT_TEST(joint-generic)
ADD_PINOCCHIO_UNIT_TEST(explog)
ADD_PINOCCHIO_UNIT_TEST(finite-differences)
ADD_PINOCCHIO_UNIT_TEST(visitor)
ADD_PINOCCHIO_UNIT_TEST(algo-check)
ADD_PINOCCHIO_UNIT_TEST(joint-composite)
ADD_PINOCCHIO_UNIT_TEST(joint-mimic)
ADD_PINOCCHIO_UNIT_TEST(liegroups)
ADD_PINOCCHIO_UNIT_TEST(regressor)
......
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
template<typename D>
void addJointAndBody(Model & model,
const JointModelBase<D> & jmodel,
const Model::JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
const Inertia & Y)
{
Model::JointIndex idx;
idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
model.appendBodyToJoint(idx,Y);
}
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
template<typename D>
void addJointAndBody(Model & model,
const JointModelBase<D> & jmodel,
const Model::JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
const Inertia & Y)
{
Model::JointIndex idx;
idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
model.appendBodyToJoint(idx,Y);
}
BOOST_AUTO_TEST_SUITE(JointPlanar)
BOOST_AUTO_TEST_CASE(spatial)
{
SE3 M(SE3::Random());
Motion v(Motion::Random());
MotionPlanar mp(1.,2.,3.);
Motion mp_dense(mp);
BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}
BOOST_AUTO_TEST_CASE(vsFreeFlyer)
{
using namespace pinocchio;
typedef SE3::Vector3 Vector3;
typedef Eigen::Matrix <double, 6, 1> Vector6;
typedef Eigen::Matrix <double, 4, 1> VectorPl;
typedef Eigen::Matrix <double, 7, 1> VectorFF;
typedef SE3::Matrix3 Matrix3;
Model modelPlanar, modelFreeflyer;
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
addJointAndBody(modelPlanar,JointModelPlanar(),0,SE3::Identity(),"planar",inertia);
addJointAndBody(modelFreeflyer,JointModelFreeFlyer(),0,SE3::Identity(),"free-flyer",inertia);
Data dataPlanar(modelPlanar);
Data dataFreeFlyer(modelFreeflyer);
VectorPl q; q << 1, 1, 0, 1; // Angle is PI /2;
VectorFF qff; qff << 1, 1, 0, 0, 0, sqrt(2)/2, sqrt(2)/2 ;
Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPlanar.nv);
Vector6 vff; vff << 1, 1, 0, 0, 0, 1;
Eigen::VectorXd tauPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
Eigen::VectorXd tauff = Eigen::VectorXd::Ones(modelFreeflyer.nv);
Eigen::VectorXd aPlanar = Eigen::VectorXd::Ones(modelPlanar.nv);
Eigen::VectorXd aff(vff);
forwardKinematics(modelPlanar, dataPlanar, q, v);
forwardKinematics(modelFreeflyer, dataFreeFlyer, qff, vff);
computeAllTerms(modelPlanar, dataPlanar, q, v);
computeAllTerms(modelFreeflyer, dataFreeFlyer, qff, vff);
BOOST_CHECK(dataFreeFlyer.oMi[1].isApprox(dataPlanar.oMi[1]));
BOOST_CHECK(dataFreeFlyer.liMi[1].isApprox(dataPlanar.liMi[1]));
BOOST_CHECK(dataFreeFlyer.Ycrb[1].matrix().isApprox(dataPlanar.Ycrb[1].matrix()));
BOOST_CHECK(dataFreeFlyer.f[1].toVector().isApprox(dataPlanar.f[1].toVector()));
Eigen::VectorXd nle_expected_ff(3); nle_expected_ff << dataFreeFlyer.nle[0],
dataFreeFlyer.nle[1],
dataFreeFlyer.nle[5]
;
BOOST_CHECK(nle_expected_ff.isApprox(dataPlanar.nle));
BOOST_CHECK(dataFreeFlyer.com[0].isApprox(dataPlanar.com[0]));
// InverseDynamics == rnea
tauPlanar = rnea(modelPlanar, dataPlanar, q, v, aPlanar);
tauff = rnea(modelFreeflyer, dataFreeFlyer, qff, vff, aff);
Vector3 tau_expected; tau_expected << tauff(0), tauff(1), tauff(5);
BOOST_CHECK(tauPlanar.isApprox(tau_expected));
// ForwardDynamics == aba
Eigen::VectorXd aAbaPlanar = aba(modelPlanar,dataPlanar, q, v, tauPlanar);
Eigen::VectorXd aAbaFreeFlyer = aba(modelFreeflyer,dataFreeFlyer, qff, vff, tauff);
Vector3 a_expected; a_expected << aAbaFreeFlyer[0],
aAbaFreeFlyer[1],
aAbaFreeFlyer[5]
;
BOOST_CHECK(aAbaPlanar.isApprox(a_expected));
// crba
crba(modelPlanar, dataPlanar,q);
crba(modelFreeflyer, dataFreeFlyer, qff);
Eigen::Matrix<double, 3, 3> M_expected;
M_expected.block<2,2>(0,0) = dataFreeFlyer.M.block<2,2>(0,0);
M_expected.block<1,2>(2,0) = dataFreeFlyer.M.block<1,2>(5,0);
M_expected.block<2,1>(0,2) = dataFreeFlyer.M.col(5).head<2>();
M_expected.block<1,1>(2,2) = dataFreeFlyer.M.col(5).tail<1>();
BOOST_CHECK(dataPlanar.M.isApprox(M_expected));
// Jacobian
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_planar;jacobian_planar.resize(6,3); jacobian_planar.setZero();
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobian_ff;jacobian_ff.resize(6,6);jacobian_ff.setZero();
computeJointJacobians(modelPlanar, dataPlanar, q);
computeJointJacobians(modelFreeflyer, dataFreeFlyer, qff);
getJointJacobian(modelPlanar, dataPlanar, 1, LOCAL, jacobian_planar);
getJointJacobian(modelFreeflyer, dataFreeFlyer, 1, LOCAL, jacobian_ff);
Eigen::Matrix<double, 6, 3> jacobian_expected; jacobian_expected << jacobian_ff.col(0),
jacobian_ff.col(1),
jacobian_ff.col(5)
;
BOOST_CHECK(jacobian_planar.isApprox(jacobian_expected));
}
BOOST_AUTO_TEST_SUITE_END()
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
template<typename D>
void addJointAndBody(Model & model,
const JointModelBase<D> & jmodel,
const Model::JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
const Inertia & Y)
{
Model::JointIndex idx;
idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
model.appendBodyToJoint(idx,Y);
}
BOOST_AUTO_TEST_SUITE( JointPrismatic )
BOOST_AUTO_TEST_CASE(spatial)
{
typedef TransformPrismaticTpl<double,0,0> TransformX;
typedef TransformPrismaticTpl<double,0,1> TransformY;
typedef TransformPrismaticTpl<double,0,2> TransformZ;
typedef SE3::Vector3 Vector3;
const double displacement = 0.2;
SE3 Mplain, Mrand(SE3::Random());
TransformX Mx(displacement);
Mplain = Mx;
BOOST_CHECK(Mplain.translation().isApprox(Vector3(displacement,0,0)));
BOOST_CHECK(Mplain.rotation().isIdentity());
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mx));
TransformY My(displacement);
Mplain = My;
BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,displacement,0)));
BOOST_CHECK(Mplain.rotation().isIdentity());
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*My));
TransformZ Mz(displacement);
Mplain = Mz;
BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,0,displacement)));
BOOST_CHECK(Mplain.rotation().isIdentity());
BOOST_CHECK((Mrand*Mplain).isApprox(Mrand*Mz));
SE3 M(SE3::Random());
Motion v(Motion::Random());
MotionPrismaticTpl<double,0,0> mp_x(2.);
Motion mp_dense_x(mp_x);
BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
MotionPrismaticTpl<double,0,1> mp_y(2.);
Motion mp_dense_y(mp_y);
BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
MotionPrismaticTpl<double,0,2> mp_z(2.);
Motion mp_dense_z(mp_z);
BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
}
BOOST_AUTO_TEST_CASE( test_kinematics )
{
using namespace pinocchio;
Motion expected_v_J(Motion::Zero());
Motion expected_c_J(Motion::Zero());
SE3 expected_configuration(SE3::Identity());
JointDataPX joint_data;
JointModelPX joint_model;
joint_model.setIndexes(0, 0, 0);
Eigen::VectorXd q(Eigen::VectorXd::Zero(1));
Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1));
// -------
q << 0. ;
q_dot << 0.;
joint_model.calc(joint_data, q, q_dot);
BOOST_CHECK(expected_configuration.rotation().isApprox(joint_data.M.rotation(), 1e-12));
BOOST_CHECK(expected_configuration.translation().isApprox(joint_data.M.translation(), 1e-12));
BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
BOOST_CHECK(expected_c_J.isApprox((Motion) joint_data.c, 1e-12));
// -------
q << 1.;
q_dot << 1.;
joint_model.calc(joint_data, q, q_dot);
expected_configuration.translation() << 1, 0, 0;
expected_v_J.linear() << 1., 0., 0.;
BOOST_CHECK(expected_configuration.rotation().isApprox(joint_data.M.rotation(), 1e-12));
BOOST_CHECK(expected_configuration.translation().isApprox(joint_data.M.translation(), 1e-12));
BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
BOOST_CHECK(expected_c_J.isApprox((Motion) joint_data.c, 1e-12));
}
BOOST_AUTO_TEST_CASE( test_rnea )
{
using namespace pinocchio;
typedef SE3::Vector3 Vector3;
typedef SE3::Matrix3 Matrix3;
Model model;
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
Data data(model);
Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
Eigen::VectorXd v(Eigen::VectorXd::Zero(model.nv));
Eigen::VectorXd a(Eigen::VectorXd::Zero(model.nv));
rnea(model, data, q, v, a);
Eigen::VectorXd tau_expected(Eigen::VectorXd::Zero(model.nq));
tau_expected << 0;
BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));
// -----
q = Eigen::VectorXd::Ones(model.nq);
v = Eigen::VectorXd::Ones(model.nv);
a = Eigen::VectorXd::Ones(model.nv);
rnea(model, data, q, v, a);
tau_expected << 1;
BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
q << 3;
v = Eigen::VectorXd::Ones(model.nv);
a = Eigen::VectorXd::Ones(model.nv);
rnea(model, data, q, v, a);
tau_expected << 1;
BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
}
BOOST_AUTO_TEST_CASE( test_crba )
{
using namespace pinocchio;
using namespace std;
typedef SE3::Vector3 Vector3;
typedef SE3::Matrix3 Matrix3;
Model model;
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
Data data(model);
Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
Eigen::MatrixXd M_expected(model.nv,model.nv);
crba(model, data, q);
M_expected << 1.0;
BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));
q = Eigen::VectorXd::Ones(model.nq);
crba(model, data, q);
BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));
q << 3;
crba(model, data, q);
BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
}
BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE(JointPrismaticUnaligned)
BOOST_AUTO_TEST_CASE(spatial)
{
SE3 M(SE3::Random());
Motion v(Motion::Random());
MotionPrismaticUnaligned mp(MotionPrismaticUnaligned::Vector3(1.,2.,3.),6.);
Motion mp_dense(mp);
BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
}
BOOST_AUTO_TEST_CASE(vsPX)
{
using namespace pinocchio;
typedef SE3::Vector3 Vector3;
typedef SE3::Matrix3 Matrix3;
Eigen::Vector3d axis;
axis << 1.0, 0.0, 0.0;
Model modelPX, modelPrismaticUnaligned;
Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
JointModelPrismaticUnaligned joint_model_PU(axis);
addJointAndBody(modelPX,JointModelPX(),0,pos,"px",inertia);
addJointAndBody(modelPrismaticUnaligned,joint_model_PU,0,pos,"prismatic-unaligned",inertia);
Data dataPX(modelPX);
Data dataPrismaticUnaligned(modelPrismaticUnaligned);
Eigen::VectorXd q = Eigen::VectorXd::Ones(modelPX.nq);
Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPX.nv);
Eigen::VectorXd tauPX = Eigen::VectorXd::Ones(modelPX.nv);
Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones(modelPrismaticUnaligned.nv);
Eigen::VectorXd aPX = Eigen::VectorXd::Ones(modelPX.nv);
Eigen::VectorXd aPrismaticUnaligned(aPX);
forwardKinematics(modelPX, dataPX, q, v);
forwardKinematics(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
computeAllTerms(modelPX, dataPX, q, v);
computeAllTerms(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
BOOST_CHECK(dataPrismaticUnaligned.oMi[1].isApprox(dataPX.oMi[1]));
BOOST_CHECK(dataPrismaticUnaligned.liMi[1].isApprox(dataPX.liMi[1]));
BOOST_CHECK(dataPrismaticUnaligned.Ycrb[1].matrix().isApprox(dataPX.Ycrb[1].matrix()));
BOOST_CHECK(dataPrismaticUnaligned.f[1].toVector().isApprox(dataPX.f[1].toVector()));
BOOST_CHECK(dataPrismaticUnaligned.nle.isApprox(dataPX.nle));
BOOST_CHECK(dataPrismaticUnaligned.com[0].isApprox(dataPX.com[0]));
// InverseDynamics == rnea
tauPX = rnea(modelPX, dataPX, q, v, aPX);
tauPrismaticUnaligned = rnea(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v, aPrismaticUnaligned);
BOOST_CHECK(tauPX.isApprox(tauPrismaticUnaligned));
// ForwardDynamics == aba
Eigen::VectorXd aAbaPX = aba(modelPX,dataPX, q, v, tauPX);
Eigen::VectorXd aAbaPrismaticUnaligned = aba(modelPrismaticUnaligned,dataPrismaticUnaligned, q, v, tauPrismaticUnaligned);
BOOST_CHECK(aAbaPX.isApprox(aAbaPrismaticUnaligned));
// crba
crba(modelPX, dataPX,q);
crba(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
BOOST_CHECK(dataPX.M.isApprox(dataPrismaticUnaligned.M));
// Jacobian
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
computeJointJacobians(modelPX, dataPX, q);
computeJointJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
getJointJacobian(modelPX, dataPX, 1, LOCAL, jacobianPX);
getJointJacobian(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, LOCAL, jacobianPrismaticUnaligned);
BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
}
BOOST_AUTO_TEST_SUITE_END()
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#include "pinocchio/math/fwd.hpp"
#include "pinocchio/multibody/joint/joints.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include <boost/test/unit_test.hpp>
#include <iostream>
using namespace pinocchio;
template<typename D>
void addJointAndBody(Model & model,
const JointModelBase<D> & jmodel,
const Model::JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
const Inertia & Y)
{
Model::JointIndex idx;
idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
model.appendBodyToJoint(idx,Y);
}
BOOST_AUTO_TEST_SUITE(JointRevoluteUnaligned)
BOOST_AUTO_TEST_CASE(vsRX)
{
using namespace pinocchio;
typedef SE3::Vector3 Vector3;
typedef SE3::Matrix3 Matrix3;
Vector3 axis;
axis << 1.0, 0.0, 0.0;
Model modelRX, modelRevoluteUnaligned;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
JointModelRevoluteUnaligned joint_model_RU(axis);
addJointAndBody(modelRX,JointModelRX(),0,pos,"rx",inertia);
addJointAndBody(modelRevoluteUnaligned,joint_model_RU,0,pos,"revolute-unaligned",inertia);
Data dataRX(modelRX);
Data dataRevoluteUnaligned(modelRevoluteUnaligned);
Eigen::VectorXd q = Eigen::VectorXd::Ones (modelRX.nq);
Eigen::VectorXd v = Eigen::VectorXd::Ones (modelRX.nv);
Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv);
Eigen::VectorXd tauRevoluteUnaligned = Eigen::VectorXd::Ones (modelRevoluteUnaligned.nv);
Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv);
Eigen::VectorXd aRevoluteUnaligned(aRX);
forwardKinematics(modelRX, dataRX, q, v);
forwardKinematics(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);
computeAllTerms(modelRX, dataRX, q, v);
computeAllTerms(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v);
BOOST_CHECK(dataRevoluteUnaligned.oMi[1].isApprox(dataRX.oMi[1]));
BOOST_CHECK(dataRevoluteUnaligned.liMi[1].isApprox(dataRX.liMi[1]));
BOOST_CHECK(dataRevoluteUnaligned.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
BOOST_CHECK(dataRevoluteUnaligned.f[1].toVector().isApprox(dataRX.f[1].toVector()));
BOOST_CHECK(dataRevoluteUnaligned.nle.isApprox(dataRX.nle));
BOOST_CHECK(dataRevoluteUnaligned.com[0].isApprox(dataRX.com[0]));
// InverseDynamics == rnea
tauRX = rnea(modelRX, dataRX, q, v, aRX);
tauRevoluteUnaligned = rnea(modelRevoluteUnaligned, dataRevoluteUnaligned, q, v, aRevoluteUnaligned);