Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/joint-composite.cpp	Lines:	159	159	100.0 %
Date:	2024-01-23 21:41:47	Branches:	687	1368	50.2 %


//
// Copyright (c) 2016 CNRS
//

#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;
using namespace Eigen;

template<typename JointModel>
void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelComposite & jmodel_composite);

template<typename JointModel>
void test_joint_methods(const JointModelBase<JointModel> & jmodel)
{
  JointModelComposite jmodel_composite(jmodel);
  test_joint_methods(jmodel,jmodel_composite);
}

template<typename JointModel>
void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelComposite & jmodel_composite)
{
  typedef typename JointModelBase<JointModel>::JointDataDerived JointData;

  JointData jdata = jmodel.createData();
  JointDataComposite jdata_composite = jmodel_composite.createData();

  jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v());

  typedef typename JointModel::ConfigVector_t ConfigVector_t;
  typedef typename JointModel::TangentVector_t TangentVector_t;
  typedef typename LieGroup<JointModel>::type LieGroupType;

  typedef Eigen::Matrix<typename ConfigVector_t::Scalar, Eigen::Dynamic, 1, ConfigVector_t::Options> DynConfigVectorType;
  typedef DynConfigVectorType DynTangentVectorType;

  ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(),-M_PI));
  ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(),M_PI));
  DynConfigVectorType q = LieGroupType().randomConfiguration(ql,qu);

  BOOST_CHECK(jmodel.nv() == jmodel_composite.nv());
  BOOST_CHECK(jmodel.nq() == jmodel_composite.nq());

  jmodel.calc(jdata,q);
  jmodel_composite.calc(jdata_composite,q);

  BOOST_CHECK(jdata_composite.M.isApprox((SE3)jdata.M));
  BOOST_CHECK(jdata_composite.S.matrix().isApprox(jdata.S.matrix()));

  q = LieGroupType().randomConfiguration(ql,qu);
  DynTangentVectorType v = TangentVector_t::Random(jmodel.nv());
  jmodel.calc(jdata,q,v);
  jmodel_composite.calc(jdata_composite,q,v);

  BOOST_CHECK(jdata_composite.M.isApprox((SE3)jdata.M));
  BOOST_CHECK(jdata_composite.S.matrix().isApprox(jdata.S.matrix()));
  BOOST_CHECK(jdata_composite.v.isApprox((Motion)jdata.v));
  BOOST_CHECK(jdata_composite.c.isApprox((Motion)jdata.c));

  // TODO: Not yet checked
//  {
//    VectorXd q1(jmodel.random());
//    jmodel.normalize(q1);
//    VectorXd q2(jmodel.random());
//    jmodel.normalize(q2);
//    VectorXd v(VectorXd::Random(jmodel.nv()));
//
//    BOOST_CHECK(jmodel_composite.integrate(q1,v).isApprox(jmodel.integrate(q1,v)));
//
//    TangentVector_t v1 = jmodel_composite.difference(q1,q2);
//    TangentVector_t v2 = jmodel.difference(q1,q2);
//
//    BOOST_CHECK(v1.isApprox(v2));
//
//    const double alpha = 0.2;
//    BOOST_CHECK(jmodel_composite.interpolate(q1,q2,alpha).isApprox(jmodel.interpolate(q1,q2,alpha)));
//    BOOST_CHECK(math::fabs(jmodel_composite.distance(q1,q2)-jmodel.distance(q1,q2))<= NumTraits<double>::dummy_precision());
//  }

  Inertia::Matrix6 I0(Inertia::Random().matrix());

  Inertia::Matrix6 I1 = I0;
  Inertia::Matrix6 I2 = I0;

  jmodel.calc_aba(jdata,I1,true);
  jmodel_composite.calc_aba(jdata_composite,I2,true);

  double prec = 1e-10; // higher tolerance to errors due to possible numerical imprecisions

  BOOST_CHECK(jdata.U.isApprox(jdata_composite.U,prec));
  BOOST_CHECK(jdata.Dinv.isApprox(jdata_composite.Dinv,prec));
  BOOST_CHECK(jdata.UDinv.isApprox(jdata_composite.UDinv,prec));

  // Checking the inertia was correctly updated
  // We use isApprox as usual, except for the freeflyer,
  // where the correct result is exacly zero and isApprox would fail.
  // Only for this single case, we use the infinity norm of the difference
  if(jmodel.shortname() == "JointModelFreeFlyer")
    BOOST_CHECK((I1-I2).lpNorm<Eigen::Infinity>() < prec);
  else
    BOOST_CHECK(I1.isApprox(I2,prec));

  // Test operator=
  JointModelComposite jmodel_composite2 = jmodel_composite;
  JointDataComposite jdata_composite2 = jmodel_composite2.createData();

  jmodel_composite2.calc(jdata_composite2,q,v);

  Inertia::Matrix6 I3 = I0;
  jmodel_composite2.calc_aba(jdata_composite2,I3,true);

  if(jmodel.shortname() == "JointModelFreeFlyer")
    BOOST_CHECK((I3-I2).lpNorm<Eigen::Infinity>() < prec);
  else
    BOOST_CHECK(I3.isApprox(I2,prec));

  BOOST_CHECK(jmodel_composite2 == jmodel_composite);

  BOOST_CHECK(jdata_composite2.U.isApprox(jdata_composite.U,prec));
  BOOST_CHECK(jdata_composite2.Dinv.isApprox(jdata_composite.Dinv,prec));
  BOOST_CHECK(jdata_composite2.UDinv.isApprox(jdata_composite.UDinv,prec));

  // Test operator <<
  std::cout << "JointModelComposite operator<<:\n" << jmodel_composite << std::endl;

  // Test block operators
  const int m = 100;
  Data::Matrix6x mat(Data::Matrix6x::Ones(6,m));
  const Data::Matrix6x mat_const(Data::Matrix6x::Ones(6,m));
  Model::ConfigVectorType vec(Model::ConfigVectorType::Ones(m));
  const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m));

  BOOST_CHECK(jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec));
  BOOST_CHECK(jmodel.jointConfigSelector(vec_const) == jmodel_composite.jointConfigSelector(vec_const));

  BOOST_CHECK(jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec));
  BOOST_CHECK(jmodel.jointVelocitySelector(vec_const) == jmodel_composite.jointVelocitySelector(vec_const));

  BOOST_CHECK(jmodel.jointCols(mat) == jmodel_composite.jointCols(mat));
  BOOST_CHECK(jmodel.jointCols(mat_const) == jmodel_composite.jointCols(mat_const));
}

struct TestJointComposite{

  template <typename JointModel>
  void operator()(const JointModelBase<JointModel> &) const
  {
    JointModel jmodel;
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

//  void operator()(const JointModelBase<JointModelComposite> &) const
//  {
//    JointModelComposite jmodel_composite;
//    jmodel_composite.addJoint(pinocchio::JointModelRX());
//    jmodel_composite.addJoint(pinocchio::JointModelRY());
//    jmodel_composite.setIndexes(0,0,0);
//
//    test_joint_methods(jmodel_composite);
//  }

  void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const
  {
    JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

  void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const
  {
    JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);

    test_joint_methods(jmodel);
  }

};

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(test_basic)
{
  typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
  , JointModelSpherical, JointModelSphericalZYX
  , JointModelPX, JointModelPY, JointModelPZ
  , JointModelPrismaticUnaligned
  , JointModelFreeFlyer
  , JointModelPlanar
  , JointModelTranslation
  , JointModelRUBX, JointModelRUBY, JointModelRUBZ
  > Variant;

  boost::mpl::for_each<Variant::types>(TestJointComposite());
}

BOOST_AUTO_TEST_CASE(chain)
{
  JointModelComposite jmodel_composite;
  jmodel_composite.addJoint(JointModelRZ()).addJoint(JointModelRY(),SE3::Random()).addJoint(JointModelRX());
  BOOST_CHECK_MESSAGE( jmodel_composite.nq() == 3, "Chain did not work");
  BOOST_CHECK_MESSAGE( jmodel_composite.nv() == 3, "Chain did not work");
  BOOST_CHECK_MESSAGE( jmodel_composite.njoints == 3, "Chain did not work");
}

BOOST_AUTO_TEST_CASE(vsZYX)
{
  JointModelSphericalZYX jmodel_spherical;
  jmodel_spherical.setIndexes(0,0,0);

  JointModelComposite jmodel_composite((JointModelRZ()));
  jmodel_composite.addJoint(JointModelRY());
  jmodel_composite.addJoint(JointModelRX());

  test_joint_methods(jmodel_spherical, jmodel_composite);
}

BOOST_AUTO_TEST_CASE(vsTranslation)
{
  JointModelTranslation jmodel_translation;
  jmodel_translation.setIndexes(0,0,0);

  JointModelComposite jmodel_composite((JointModelPX()));
  jmodel_composite.addJoint(JointModelPY());
  jmodel_composite.addJoint(JointModelPZ());

  test_joint_methods(jmodel_translation, jmodel_composite);
}

BOOST_AUTO_TEST_CASE (test_recursive_variant)
{
  /// Create joint composite with two joints,
  JointModelComposite jmodel_composite_two_rx((JointModelRX()));
  jmodel_composite_two_rx.addJoint(JointModelRY());

  /// Create Joint composite with three joints, and add a composite in it, to test the recursive_wrapper
  JointModelComposite jmodel_composite_recursive((JointModelFreeFlyer()));
  jmodel_composite_recursive.addJoint(JointModelPlanar());
  jmodel_composite_recursive.addJoint(jmodel_composite_two_rx);
}


BOOST_AUTO_TEST_CASE(test_copy)
{
  JointModelComposite jmodel_composite_planar((JointModelPX()));
  jmodel_composite_planar.addJoint(JointModelPY());
  jmodel_composite_planar.addJoint(JointModelRZ());
  jmodel_composite_planar.setIndexes(0,0,0);

  JointDataComposite jdata_composite_planar = jmodel_composite_planar.createData();

  VectorXd q1(VectorXd::Random(3));
  VectorXd q1_dot(VectorXd::Random(3));

  JointModelComposite model_copy = jmodel_composite_planar;
  JointDataComposite data_copy = model_copy.createData();

  BOOST_CHECK_MESSAGE( model_copy.nq() == jmodel_composite_planar.nq(), "Test Copy Composite, nq are differents");
  BOOST_CHECK_MESSAGE( model_copy.nv() == jmodel_composite_planar.nv(), "Test Copy Composite, nv are differents");

  jmodel_composite_planar.calc(jdata_composite_planar,q1, q1_dot);
  model_copy.calc(data_copy,q1, q1_dot);

}

BOOST_AUTO_TEST_CASE(test_kinematics)
{
  Model model;
  JointModelComposite jmodel_composite;

  SE3 config=SE3::Random();
  JointIndex parent=0;

  for(int i=0; i<10; i++)
  {
    parent = model.addJoint(parent, JointModelRX(), config, "joint");
    jmodel_composite.addJoint(JointModelRX(),config);

    config.setRandom();
  }

  Data data(model);

  Model model_c;
  model_c.addJoint(0,jmodel_composite,SE3::Identity(),"joint");

  Data data_c(model_c);

  BOOST_CHECK(model.nv == model_c.nv);
  BOOST_CHECK(model.nq == model_c.nq);

  VectorXd q(VectorXd::Random(model.nv));
  forwardKinematics(model,data,q);
  forwardKinematics(model_c,data_c,q);

  BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));

  q.setRandom(model.nq);
  VectorXd v(VectorXd::Random(model.nv));
  forwardKinematics(model,data,q,v);
  forwardKinematics(model_c,data_c,q,v);

  BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));
  BOOST_CHECK(data.v.back().isApprox(data_c.v.back()));

  q.setRandom(model.nq);
  v.setRandom(model.nv);
  VectorXd a(VectorXd::Random(model.nv));
  forwardKinematics(model,data,q,v,a);
  forwardKinematics(model_c,data_c,q,v,a);

  BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));
  BOOST_CHECK(data.v.back().isApprox(data_c.v.back()));
  BOOST_CHECK(data.a.back().isApprox(data_c.a.back()));
}

BOOST_AUTO_TEST_SUITE_END ()



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016 CNRS
3			//
4
5			#include "pinocchio/multibody/joint/joint-composite.hpp"
6			#include "pinocchio/algorithm/joint-configuration.hpp"
7			#include "pinocchio/algorithm/kinematics.hpp"
8
9			#include <boost/test/unit_test.hpp>
10			#include <boost/utility/binary.hpp>
11
12			using namespace pinocchio;
13			using namespace Eigen;
14
15			template<typename JointModel>
16			void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelComposite & jmodel_composite);
17
18			template<typename JointModel>
19		32	void test_joint_methods(const JointModelBase<JointModel> & jmodel)
20			{
21	✓✗✓✗	64	JointModelComposite jmodel_composite(jmodel);
22	✓✗	32	test_joint_methods(jmodel,jmodel_composite);
23		32	}
24
25			template<typename JointModel>
26		36	void test_joint_methods(const JointModelBase<JointModel> & jmodel, JointModelComposite & jmodel_composite)
27			{
28			typedef typename JointModelBase<JointModel>::JointDataDerived JointData;
29
30	✓✗	36	JointData jdata = jmodel.createData();
31	✓✗	72	JointDataComposite jdata_composite = jmodel_composite.createData();
32
33	✓✗✓✗ ✓✗✓✗	36	jmodel_composite.setIndexes(jmodel.id(), jmodel.idx_q(), jmodel.idx_v());
34
35			typedef typename JointModel::ConfigVector_t ConfigVector_t;
36			typedef typename JointModel::TangentVector_t TangentVector_t;
37			typedef typename LieGroup<JointModel>::type LieGroupType;
38
39			typedef Eigen::Matrix<typename ConfigVector_t::Scalar, Eigen::Dynamic, 1, ConfigVector_t::Options> DynConfigVectorType;
40			typedef DynConfigVectorType DynTangentVectorType;
41
42	✓✗✓✗ ✓✗	36	ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(),-M_PI));
43	✓✗✓✗ ✓✗	36	ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(),M_PI));
44	✓✗✓✗ ✓✗	72	DynConfigVectorType q = LieGroupType().randomConfiguration(ql,qu);
45
46	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	36	BOOST_CHECK(jmodel.nv() == jmodel_composite.nv());
47	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	36	BOOST_CHECK(jmodel.nq() == jmodel_composite.nq());
48
49	✓✗	36	jmodel.calc(jdata,q);
50	✓✗	36	jmodel_composite.calc(jdata_composite,q);
51
52	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	36	BOOST_CHECK(jdata_composite.M.isApprox((SE3)jdata.M));
53	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata_composite.S.matrix().isApprox(jdata.S.matrix()));
54
55	✓✗✓✗ ✓✗	36	q = LieGroupType().randomConfiguration(ql,qu);
56	✓✗✓✗ ✓✗	72	DynTangentVectorType v = TangentVector_t::Random(jmodel.nv());
57	✓✗	36	jmodel.calc(jdata,q,v);
58	✓✗	36	jmodel_composite.calc(jdata_composite,q,v);
59
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	36	BOOST_CHECK(jdata_composite.M.isApprox((SE3)jdata.M));
61	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata_composite.S.matrix().isApprox(jdata.S.matrix()));
62	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	36	BOOST_CHECK(jdata_composite.v.isApprox((Motion)jdata.v));
63	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	36	BOOST_CHECK(jdata_composite.c.isApprox((Motion)jdata.c));
64
65			// TODO: Not yet checked
66			// {
67			// VectorXd q1(jmodel.random());
68			// jmodel.normalize(q1);
69			// VectorXd q2(jmodel.random());
70			// jmodel.normalize(q2);
71			// VectorXd v(VectorXd::Random(jmodel.nv()));
72			//
73			// BOOST_CHECK(jmodel_composite.integrate(q1,v).isApprox(jmodel.integrate(q1,v)));
74			//
75			// TangentVector_t v1 = jmodel_composite.difference(q1,q2);
76			// TangentVector_t v2 = jmodel.difference(q1,q2);
77			//
78			// BOOST_CHECK(v1.isApprox(v2));
79			//
80			// const double alpha = 0.2;
81			// BOOST_CHECK(jmodel_composite.interpolate(q1,q2,alpha).isApprox(jmodel.interpolate(q1,q2,alpha)));
82			// BOOST_CHECK(math::fabs(jmodel_composite.distance(q1,q2)-jmodel.distance(q1,q2))<= NumTraits<double>::dummy_precision());
83			// }
84
85	✓✗✓✗	36	Inertia::Matrix6 I0(Inertia::Random().matrix());
86
87	✓✗	36	Inertia::Matrix6 I1 = I0;
88	✓✗	36	Inertia::Matrix6 I2 = I0;
89
90	✓✗	36	jmodel.calc_aba(jdata,I1,true);
91	✓✗	36	jmodel_composite.calc_aba(jdata_composite,I2,true);
92
93		36	double prec = 1e-10; // higher tolerance to errors due to possible numerical imprecisions
94
95	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata.U.isApprox(jdata_composite.U,prec));
96	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata.Dinv.isApprox(jdata_composite.Dinv,prec));
97	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata.UDinv.isApprox(jdata_composite.UDinv,prec));
98
99			// Checking the inertia was correctly updated
100			// We use isApprox as usual, except for the freeflyer,
101			// where the correct result is exacly zero and isApprox would fail.
102			// Only for this single case, we use the infinity norm of the difference
103	✓✗✓✓	36	if(jmodel.shortname() == "JointModelFreeFlyer")
104	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((I1-I2).lpNorm<Eigen::Infinity>() < prec);
105			else
106	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	34	BOOST_CHECK(I1.isApprox(I2,prec));
107
108			// Test operator=
109	✓✗	72	JointModelComposite jmodel_composite2 = jmodel_composite;
110	✓✗	72	JointDataComposite jdata_composite2 = jmodel_composite2.createData();
111
112	✓✗	36	jmodel_composite2.calc(jdata_composite2,q,v);
113
114	✓✗	36	Inertia::Matrix6 I3 = I0;
115	✓✗	36	jmodel_composite2.calc_aba(jdata_composite2,I3,true);
116
117	✓✗✓✓	36	if(jmodel.shortname() == "JointModelFreeFlyer")
118	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((I3-I2).lpNorm<Eigen::Infinity>() < prec);
119			else
120	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	34	BOOST_CHECK(I3.isApprox(I2,prec));
121
122	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jmodel_composite2 == jmodel_composite);
123
124	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata_composite2.U.isApprox(jdata_composite.U,prec));
125	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata_composite2.Dinv.isApprox(jdata_composite.Dinv,prec));
126	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata_composite2.UDinv.isApprox(jdata_composite.UDinv,prec));
127
128			// Test operator <<
129	✓✗✓✗ ✓✗	36	std::cout << "JointModelComposite operator<<:\n" << jmodel_composite << std::endl;
130
131			// Test block operators
132		36	const int m = 100;
133	✓✗✓✗	72	Data::Matrix6x mat(Data::Matrix6x::Ones(6,m));
134	✓✗✓✗	72	const Data::Matrix6x mat_const(Data::Matrix6x::Ones(6,m));
135	✓✗✓✗	72	Model::ConfigVectorType vec(Model::ConfigVectorType::Ones(m));
136	✓✗✓✗	72	const Model::ConfigVectorType vec_const(Model::ConfigVectorType::Ones(m));
137
138	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jmodel.jointConfigSelector(vec) == jmodel_composite.jointConfigSelector(vec));
139	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jmodel.jointConfigSelector(vec_const) == jmodel_composite.jointConfigSelector(vec_const));
140
141	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jmodel.jointVelocitySelector(vec) == jmodel_composite.jointVelocitySelector(vec));
142	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jmodel.jointVelocitySelector(vec_const) == jmodel_composite.jointVelocitySelector(vec_const));
143
144	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jmodel.jointCols(mat) == jmodel_composite.jointCols(mat));
145	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jmodel.jointCols(mat_const) == jmodel_composite.jointCols(mat_const));
146		36	}
147
148			struct TestJointComposite{
149
150			template <typename JointModel>
151		28	void operator()(const JointModelBase<JointModel> &) const
152			{
153	✓✗	28	JointModel jmodel;
154	✓✗	28	jmodel.setIndexes(0,0,0);
155
156	✓✗	28	test_joint_methods(jmodel);
157		28	}
158
159			// void operator()(const JointModelBase<JointModelComposite> &) const
160			// {
161			// JointModelComposite jmodel_composite;
162			// jmodel_composite.addJoint(pinocchio::JointModelRX());
163			// jmodel_composite.addJoint(pinocchio::JointModelRY());
164			// jmodel_composite.setIndexes(0,0,0);
165			//
166			// test_joint_methods(jmodel_composite);
167			// }
168
169		1	void operator()(const JointModelBase<JointModelRevoluteUnaligned> &) const
170			{
171	✓✗	1	JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
172	✓✗	1	jmodel.setIndexes(0,0,0);
173
174	✓✗	1	test_joint_methods(jmodel);
175		1	}
176
177		1	void operator()(const JointModelBase<JointModelPrismaticUnaligned> &) const
178			{
179	✓✗	1	JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
180	✓✗	1	jmodel.setIndexes(0,0,0);
181
182	✓✗	1	test_joint_methods(jmodel);
183		1	}
184
185			};
186
187			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
188
189	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_basic)
190			{
191			typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned
192			, JointModelSpherical, JointModelSphericalZYX
193			, JointModelPX, JointModelPY, JointModelPZ
194			, JointModelPrismaticUnaligned
195			, JointModelFreeFlyer
196			, JointModelPlanar
197			, JointModelTranslation
198			, JointModelRUBX, JointModelRUBY, JointModelRUBZ
199			> Variant;
200
201	✓✗	2	boost::mpl::for_each<Variant::types>(TestJointComposite());
202		2	}
203
204	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(chain)
205			{
206	✓✗	4	JointModelComposite jmodel_composite;
207	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2	jmodel_composite.addJoint(JointModelRZ()).addJoint(JointModelRY(),SE3::Random()).addJoint(JointModelRX());
208	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_MESSAGE( jmodel_composite.nq() == 3, "Chain did not work");
209	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK_MESSAGE( jmodel_composite.nv() == 3, "Chain did not work");
210	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK_MESSAGE( jmodel_composite.njoints == 3, "Chain did not work");
211		2	}
212
213	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(vsZYX)
214			{
215	✓✗	2	JointModelSphericalZYX jmodel_spherical;
216	✓✗	2	jmodel_spherical.setIndexes(0,0,0);
217
218	✓✗✓✗ ✓✗	4	JointModelComposite jmodel_composite((JointModelRZ()));
219	✓✗✓✗ ✓✗	2	jmodel_composite.addJoint(JointModelRY());
220	✓✗✓✗ ✓✗	2	jmodel_composite.addJoint(JointModelRX());
221
222	✓✗	2	test_joint_methods(jmodel_spherical, jmodel_composite);
223		2	}
224
225	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(vsTranslation)
226			{
227	✓✗	2	JointModelTranslation jmodel_translation;
228	✓✗	2	jmodel_translation.setIndexes(0,0,0);
229
230	✓✗✓✗ ✓✗	4	JointModelComposite jmodel_composite((JointModelPX()));
231	✓✗✓✗ ✓✗	2	jmodel_composite.addJoint(JointModelPY());
232	✓✗✓✗ ✓✗	2	jmodel_composite.addJoint(JointModelPZ());
233
234	✓✗	2	test_joint_methods(jmodel_translation, jmodel_composite);
235		2	}
236
237	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE (test_recursive_variant)
238			{
239			/// Create joint composite with two joints,
240	✓✗✓✗ ✓✗	4	JointModelComposite jmodel_composite_two_rx((JointModelRX()));
241	✓✗✓✗ ✓✗	2	jmodel_composite_two_rx.addJoint(JointModelRY());
242
243			/// Create Joint composite with three joints, and add a composite in it, to test the recursive_wrapper
244	✓✗✓✗ ✓✗	2	JointModelComposite jmodel_composite_recursive((JointModelFreeFlyer()));
245	✓✗✓✗ ✓✗	2	jmodel_composite_recursive.addJoint(JointModelPlanar());
246	✓✗✓✗	2	jmodel_composite_recursive.addJoint(jmodel_composite_two_rx);
247		2	}
248
249
250	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_copy)
251			{
252	✓✗✓✗ ✓✗	4	JointModelComposite jmodel_composite_planar((JointModelPX()));
253	✓✗✓✗ ✓✗	2	jmodel_composite_planar.addJoint(JointModelPY());
254	✓✗✓✗ ✓✗	2	jmodel_composite_planar.addJoint(JointModelRZ());
255	✓✗	2	jmodel_composite_planar.setIndexes(0,0,0);
256
257	✓✗	4	JointDataComposite jdata_composite_planar = jmodel_composite_planar.createData();
258
259	✓✗✓✗	4	VectorXd q1(VectorXd::Random(3));
260	✓✗✓✗	4	VectorXd q1_dot(VectorXd::Random(3));
261
262	✓✗	4	JointModelComposite model_copy = jmodel_composite_planar;
263	✓✗	4	JointDataComposite data_copy = model_copy.createData();
264
265	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK_MESSAGE( model_copy.nq() == jmodel_composite_planar.nq(), "Test Copy Composite, nq are differents");
266	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK_MESSAGE( model_copy.nv() == jmodel_composite_planar.nv(), "Test Copy Composite, nv are differents");
267
268	✓✗	2	jmodel_composite_planar.calc(jdata_composite_planar,q1, q1_dot);
269	✓✗	2	model_copy.calc(data_copy,q1, q1_dot);
270
271		2	}
272
273	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_kinematics)
274			{
275	✓✗	4	Model model;
276	✓✗	4	JointModelComposite jmodel_composite;
277
278	✓✗	2	SE3 config=SE3::Random();
279		2	JointIndex parent=0;
280
281	✓✓	22	for(int i=0; i<10; i++)
282			{
283	✓✗✓✗ ✓✗✓✗	20	parent = model.addJoint(parent, JointModelRX(), config, "joint");
284	✓✗✓✗	20	jmodel_composite.addJoint(JointModelRX(),config);
285
286	✓✗	20	config.setRandom();
287			}
288
289	✓✗	4	Data data(model);
290
291	✓✗	4	Model model_c;
292	✓✗✓✗ ✓✗✓✗	2	model_c.addJoint(0,jmodel_composite,SE3::Identity(),"joint");
293
294	✓✗	4	Data data_c(model_c);
295
296	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model.nv == model_c.nv);
297	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(model.nq == model_c.nq);
298
299	✓✗✓✗	4	VectorXd q(VectorXd::Random(model.nv));
300	✓✗	2	forwardKinematics(model,data,q);
301	✓✗	2	forwardKinematics(model_c,data_c,q);
302
303	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));
304
305	✓✗	2	q.setRandom(model.nq);
306	✓✗✓✗	4	VectorXd v(VectorXd::Random(model.nv));
307	✓✗	2	forwardKinematics(model,data,q,v);
308	✓✗	2	forwardKinematics(model_c,data_c,q,v);
309
310	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));
311	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.v.back().isApprox(data_c.v.back()));
312
313	✓✗	2	q.setRandom(model.nq);
314	✓✗	2	v.setRandom(model.nv);
315	✓✗✓✗	4	VectorXd a(VectorXd::Random(model.nv));
316	✓✗	2	forwardKinematics(model,data,q,v,a);
317	✓✗	2	forwardKinematics(model_c,data_c,q,v,a);
318
319	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.oMi.back().isApprox(data_c.oMi.back()));
320	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.v.back().isApprox(data_c.v.back()));
321	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.a.back().isApprox(data_c.a.back()));
322		2	}
323
324			BOOST_AUTO_TEST_SUITE_END ()
325