Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/joint-prismatic.cpp	Lines:	161	161	100.0 %
Date:	2024-01-23 21:41:47	Branches:	731	1462	50.0 %


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


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4			//
5
6			#include "pinocchio/math/fwd.hpp"
7			#include "pinocchio/multibody/joint/joints.hpp"
8			#include "pinocchio/algorithm/rnea.hpp"
9			#include "pinocchio/algorithm/aba.hpp"
10			#include "pinocchio/algorithm/crba.hpp"
11			#include "pinocchio/algorithm/jacobian.hpp"
12			#include "pinocchio/algorithm/compute-all-terms.hpp"
13
14			#include <boost/test/unit_test.hpp>
15			#include <iostream>
16
17			using namespace pinocchio;
18
19			template<typename D>
20		8	void addJointAndBody(Model & model,
21			const JointModelBase<D> & jmodel,
22			const Model::JointIndex parent_id,
23			const SE3 & joint_placement,
24			const std::string & joint_name,
25			const Inertia & Y)
26			{
27			Model::JointIndex idx;
28
29	✓✗	8	idx = model.addJoint(parent_id,jmodel,joint_placement,joint_name);
30	✓✗	8	model.appendBodyToJoint(idx,Y);
31		8	}
32
33			BOOST_AUTO_TEST_SUITE( JointPrismatic )
34
35	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(spatial)
36			{
37			typedef TransformPrismaticTpl<double,0,0> TransformX;
38			typedef TransformPrismaticTpl<double,0,1> TransformY;
39			typedef TransformPrismaticTpl<double,0,2> TransformZ;
40
41			typedef SE3::Vector3 Vector3;
42
43		2	const double displacement = 0.2;
44	✓✗✓✗	2	SE3 Mplain, Mrand(SE3::Random());
45
46		2	TransformX Mx(displacement);
47	✓✗	2	Mplain = Mx;
48	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Mplain.translation().isApprox(Vector3(displacement,0,0)));
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(Mplain.rotation().isIdentity());
50	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((MrandMplain).isApprox(MrandMx));
51
52		2	TransformY My(displacement);
53	✓✗	2	Mplain = My;
54	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,displacement,0)));
55	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(Mplain.rotation().isIdentity());
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((MrandMplain).isApprox(MrandMy));
57
58		2	TransformZ Mz(displacement);
59	✓✗	2	Mplain = Mz;
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Mplain.translation().isApprox(Vector3(0,0,displacement)));
61	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(Mplain.rotation().isIdentity());
62	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((MrandMplain).isApprox(MrandMz));
63
64	✓✗	2	SE3 M(SE3::Random());
65	✓✗	2	Motion v(Motion::Random());
66
67		2	MotionPrismaticTpl<double,0,0> mp_x(2.);
68	✓✗	2	Motion mp_dense_x(mp_x);
69
70	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.act(mp_x).isApprox(M.act(mp_dense_x)));
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.actInv(mp_x).isApprox(M.actInv(mp_dense_x)));
72
73	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v.cross(mp_x).isApprox(v.cross(mp_dense_x)));
74
75		2	MotionPrismaticTpl<double,0,1> mp_y(2.);
76	✓✗	2	Motion mp_dense_y(mp_y);
77
78	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.act(mp_y).isApprox(M.act(mp_dense_y)));
79	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.actInv(mp_y).isApprox(M.actInv(mp_dense_y)));
80
81	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v.cross(mp_y).isApprox(v.cross(mp_dense_y)));
82
83		2	MotionPrismaticTpl<double,0,2> mp_z(2.);
84	✓✗	2	Motion mp_dense_z(mp_z);
85
86	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.act(mp_z).isApprox(M.act(mp_dense_z)));
87	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.actInv(mp_z).isApprox(M.actInv(mp_dense_z)));
88
89	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v.cross(mp_z).isApprox(v.cross(mp_dense_z)));
90		2	}
91
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE( test_kinematics )
93			{
94			using namespace pinocchio;
95
96
97	✓✗	2	Motion expected_v_J(Motion::Zero());
98	✓✗	2	Motion expected_c_J(Motion::Zero());
99
100	✓✗	2	SE3 expected_configuration(SE3::Identity());
101
102	✓✗	2	JointDataPX joint_data;
103	✓✗	2	JointModelPX joint_model;
104
105	✓✗	2	joint_model.setIndexes(0, 0, 0);
106
107	✓✗✓✗	4	Eigen::VectorXd q(Eigen::VectorXd::Zero(1));
108	✓✗✓✗	4	Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1));
109
110			// -------
111	✓✗	2	q << 0. ;
112	✓✗	2	q_dot << 0.;
113
114	✓✗	2	joint_model.calc(joint_data, q, q_dot);
115
116	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(expected_configuration.rotation().isApprox(joint_data.M.rotation(), 1e-12));
117	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(expected_configuration.translation().isApprox(joint_data.M.translation(), 1e-12));
118	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
119	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(expected_c_J.isApprox((Motion) joint_data.c, 1e-12));
120
121			// -------
122	✓✗	2	q << 1.;
123	✓✗	2	q_dot << 1.;
124
125	✓✗	2	joint_model.calc(joint_data, q, q_dot);
126
127	✓✗✓✗ ✓✗✓✗	2	expected_configuration.translation() << 1, 0, 0;
128
129	✓✗✓✗ ✓✗✓✗	2	expected_v_J.linear() << 1., 0., 0.;
130
131	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(expected_configuration.rotation().isApprox(joint_data.M.rotation(), 1e-12));
132	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(expected_configuration.translation().isApprox(joint_data.M.translation(), 1e-12));
133	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(expected_v_J.toVector().isApprox(((Motion) joint_data.v).toVector(), 1e-12));
134	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(expected_c_J.isApprox((Motion) joint_data.c, 1e-12));
135		2	}
136
137	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE( test_rnea )
138			{
139			using namespace pinocchio;
140			typedef SE3::Vector3 Vector3;
141			typedef SE3::Matrix3 Matrix3;
142
143	✓✗	4	Model model;
144	✓✗✓✗ ✓✗✓✗	2	Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
145
146	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
147
148	✓✗	4	Data data(model);
149
150	✓✗✓✗	4	Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
151	✓✗✓✗	4	Eigen::VectorXd v(Eigen::VectorXd::Zero(model.nv));
152	✓✗✓✗	4	Eigen::VectorXd a(Eigen::VectorXd::Zero(model.nv));
153
154	✓✗	2	rnea(model, data, q, v, a);
155
156	✓✗✓✗	4	Eigen::VectorXd tau_expected(Eigen::VectorXd::Zero(model.nq));
157	✓✗	2	tau_expected << 0;
158
159	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-14));
160
161			// -----
162	✓✗✓✗	2	q = Eigen::VectorXd::Ones(model.nq);
163	✓✗✓✗	2	v = Eigen::VectorXd::Ones(model.nv);
164	✓✗✓✗	2	a = Eigen::VectorXd::Ones(model.nv);
165
166	✓✗	2	rnea(model, data, q, v, a);
167	✓✗	2	tau_expected << 1;
168
169	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
170
171	✓✗	2	q << 3;
172	✓✗✓✗	2	v = Eigen::VectorXd::Ones(model.nv);
173	✓✗✓✗	2	a = Eigen::VectorXd::Ones(model.nv);
174
175	✓✗	2	rnea(model, data, q, v, a);
176	✓✗	2	tau_expected << 1;
177
178	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_expected.isApprox(data.tau, 1e-12));
179		2	}
180
181	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE( test_crba )
182			{
183			using namespace pinocchio;
184			using namespace std;
185			typedef SE3::Vector3 Vector3;
186			typedef SE3::Matrix3 Matrix3;
187
188	✓✗	4	Model model;
189	✓✗✓✗ ✓✗✓✗	2	Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
190
191	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	2	addJointAndBody(model,JointModelPX(),model.getJointId("universe"),SE3::Identity(),"root",inertia);
192
193	✓✗	4	Data data(model);
194
195	✓✗✓✗	4	Eigen::VectorXd q(Eigen::VectorXd::Zero(model.nq));
196	✓✗	4	Eigen::MatrixXd M_expected(model.nv,model.nv);
197
198	✓✗	2	crba(model, data, q);
199	✓✗	2	M_expected << 1.0;
200
201	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M_expected.isApprox(data.M, 1e-14));
202
203	✓✗✓✗	2	q = Eigen::VectorXd::Ones(model.nq);
204
205	✓✗	2	crba(model, data, q);
206
207	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M_expected.isApprox(data.M, 1e-12));
208
209	✓✗	2	q << 3;
210
211	✓✗	2	crba(model, data, q);
212
213	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M_expected.isApprox(data.M, 1e-10));
214		2	}
215
216			BOOST_AUTO_TEST_SUITE_END()
217
218			BOOST_AUTO_TEST_SUITE(JointPrismaticUnaligned)
219
220	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(spatial)
221			{
222	✓✗	2	SE3 M(SE3::Random());
223	✓✗	2	Motion v(Motion::Random());
224
225	✓✗✓✗	2	MotionPrismaticUnaligned mp(MotionPrismaticUnaligned::Vector3(1.,2.,3.),6.);
226	✓✗	2	Motion mp_dense(mp);
227
228	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.act(mp).isApprox(M.act(mp_dense)));
229	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(M.actInv(mp).isApprox(M.actInv(mp_dense)));
230
231	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v.cross(mp).isApprox(v.cross(mp_dense)));
232		2	}
233
234	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(vsPX)
235			{
236			using namespace pinocchio;
237			typedef SE3::Vector3 Vector3;
238			typedef SE3::Matrix3 Matrix3;
239
240	✓✗	2	Eigen::Vector3d axis;
241	✓✗✓✗ ✓✗	2	axis << 1.0, 0.0, 0.0;
242
243	✓✗✓✗	4	Model modelPX, modelPrismaticUnaligned;
244
245	✓✗✓✗ ✓✗✓✗	2	Inertia inertia(1., Vector3(0.5, 0., 0.0), Matrix3::Identity());
246	✓✗✓✗ ✓✗	2	SE3 pos(1); pos.translation() = SE3::LinearType(1.,0.,0.);
247
248	✓✗	2	JointModelPrismaticUnaligned joint_model_PU(axis);
249
250	✓✗✓✗ ✓✗	2	addJointAndBody(modelPX,JointModelPX(),0,pos,"px",inertia);
251	✓✗✓✗	2	addJointAndBody(modelPrismaticUnaligned,joint_model_PU,0,pos,"prismatic-unaligned",inertia);
252
253	✓✗	4	Data dataPX(modelPX);
254	✓✗	4	Data dataPrismaticUnaligned(modelPrismaticUnaligned);
255
256	✓✗✓✗	4	Eigen::VectorXd q = Eigen::VectorXd::Ones(modelPX.nq);
257	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Ones(modelPX.nv);
258	✓✗✓✗	4	Eigen::VectorXd tauPX = Eigen::VectorXd::Ones(modelPX.nv);
259	✓✗✓✗	4	Eigen::VectorXd tauPrismaticUnaligned = Eigen::VectorXd::Ones(modelPrismaticUnaligned.nv);
260	✓✗✓✗	4	Eigen::VectorXd aPX = Eigen::VectorXd::Ones(modelPX.nv);
261	✓✗	4	Eigen::VectorXd aPrismaticUnaligned(aPX);
262
263	✓✗	2	forwardKinematics(modelPX, dataPX, q, v);
264	✓✗	2	forwardKinematics(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
265
266	✓✗	2	computeAllTerms(modelPX, dataPX, q, v);
267	✓✗	2	computeAllTerms(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v);
268
269	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataPrismaticUnaligned.oMi[1].isApprox(dataPX.oMi[1]));
270	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataPrismaticUnaligned.liMi[1].isApprox(dataPX.liMi[1]));
271	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataPrismaticUnaligned.Ycrb[1].matrix().isApprox(dataPX.Ycrb[1].matrix()));
272	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataPrismaticUnaligned.f[1].toVector().isApprox(dataPX.f[1].toVector()));
273
274	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataPrismaticUnaligned.nle.isApprox(dataPX.nle));
275	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataPrismaticUnaligned.com[0].isApprox(dataPX.com[0]));
276
277			// InverseDynamics == rnea
278	✓✗✓✗	2	tauPX = rnea(modelPX, dataPX, q, v, aPX);
279	✓✗✓✗	2	tauPrismaticUnaligned = rnea(modelPrismaticUnaligned, dataPrismaticUnaligned, q, v, aPrismaticUnaligned);
280
281	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tauPX.isApprox(tauPrismaticUnaligned));
282
283			// ForwardDynamics == aba
284	✓✗✓✗	4	Eigen::VectorXd aAbaPX = aba(modelPX,dataPX, q, v, tauPX);
285	✓✗✓✗	4	Eigen::VectorXd aAbaPrismaticUnaligned = aba(modelPrismaticUnaligned,dataPrismaticUnaligned, q, v, tauPrismaticUnaligned);
286
287	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(aAbaPX.isApprox(aAbaPrismaticUnaligned));
288
289			// crba
290	✓✗	2	crba(modelPX, dataPX,q);
291	✓✗	2	crba(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
292
293	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dataPX.M.isApprox(dataPrismaticUnaligned.M));
294
295			// Jacobian
296	✓✗✓✗ ✓✗	4	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
297	✓✗✓✗ ✓✗	4	Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
298	✓✗	2	computeJointJacobians(modelPX, dataPX, q);
299	✓✗	2	computeJointJacobians(modelPrismaticUnaligned, dataPrismaticUnaligned, q);
300	✓✗	2	getJointJacobian(modelPX, dataPX, 1, LOCAL, jacobianPX);
301	✓✗	2	getJointJacobian(modelPrismaticUnaligned, dataPrismaticUnaligned, 1, LOCAL, jacobianPrismaticUnaligned);
302
303	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
304		2	}
305
306			BOOST_AUTO_TEST_SUITE_END()