Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/joint-generic.cpp	Lines:	159	159	100.0 %
Date:	2024-01-23 21:41:47	Branches:	607	1213	50.0 %


//
// Copyright (c) 2016-2019 CNRS INRIA
//

#include "pinocchio/multibody/joint/joint-generic.hpp"

#include "pinocchio/multibody/liegroup/liegroup.hpp"

#include "pinocchio/multibody/model.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"

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

using namespace pinocchio;

template <typename JointModel>
void test_joint_methods(JointModelBase<JointModel> & jmodel,
                        JointDataBase<typename JointModel::JointDataDerived> & jdata)
{
  typedef typename LieGroup<JointModel>::type LieGroupType;

  std::cout << "Testing Joint over " << jmodel.shortname() << std::endl;

  Eigen::VectorXd q1, q2;
  q1 = LieGroupType().random();
  q2 = LieGroupType().random();

  Eigen::VectorXd
  v1(Eigen::VectorXd::Random(jdata.S().nv())),
  v2(Eigen::VectorXd::Random(jdata.S().nv()));

  Inertia::Matrix6
  Ia(pinocchio::Inertia::Random().matrix()),
  Ia2(pinocchio::Inertia::Random().matrix());
  bool update_I = false;


  jmodel.calc(jdata.derived(), q1, v1);
  jmodel.calc_aba(jdata.derived(), Ia, update_I);

  pinocchio::JointModel jma(jmodel);
  BOOST_CHECK(jmodel == jma);
  BOOST_CHECK(jma == jmodel);
  BOOST_CHECK(jma.hasSameIndexes(jmodel));

  pinocchio::JointData jda(jdata);
  BOOST_CHECK(jda == jdata);
  BOOST_CHECK(jdata == jda);

  jma.calc(jda, q1, v1);
  jma.calc_aba(jda, Ia, update_I);

  pinocchio::JointData jda_other(jdata);

  jma.calc(jda_other, q2, v2);
  jma.calc_aba(jda_other, Ia2, update_I);

  BOOST_CHECK(jda_other != jda);
  BOOST_CHECK(jda != jda_other);
  BOOST_CHECK(jda_other != jdata);
  BOOST_CHECK(jdata != jda_other);

  const std::string error_prefix("JointModel on " + jma.shortname());
  BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq "));
  BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv "));

  BOOST_CHECK_MESSAGE(jmodel.idx_q() == jma.idx_q() ,std::string(error_prefix + " - Idx_q "));
  BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v() ,std::string(error_prefix + " - Idx_v "));
  BOOST_CHECK_MESSAGE(jmodel.id() == jma.id() ,std::string(error_prefix + " - JointId "));

  BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S().matrix()),std::string(error_prefix + " - ConstraintXd "));
  BOOST_CHECK_MESSAGE( (jda.M()).isApprox((jdata.M())),std::string(error_prefix + " - Joint transforms ")); // ==  or isApprox ?
  BOOST_CHECK_MESSAGE( (jda.v()).isApprox( (pinocchio::Motion(jdata.v()))),std::string(error_prefix + " - Joint motions "));
  BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c()),std::string(error_prefix + " - Joint bias "));

  BOOST_CHECK_MESSAGE((jda.U()).isApprox(jdata.U()),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
  BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox(jdata.Dinv()),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
  BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox(jdata.UDinv()),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));

  // Test vxS
  typedef typename JointModel::Constraint_t Constraint_t;
  typedef typename Constraint_t::DenseBase ConstraintDense;

  Motion v(Motion::Random());
  ConstraintDense vxS(v.cross(jdata.S()));
  ConstraintDense vxS_ref = v.toActionMatrix() * jdata.S().matrix();

  BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref),std::string(error_prefix + "- Joint vxS operation "));

  // Test Y*S
  const Inertia Isparse(Inertia::Random());
  const Inertia::Matrix6 Idense(Isparse.matrix());

  const ConstraintDense IsparseS = Isparse * jdata.S();
  const ConstraintDense IdenseS = Idense * jdata.S();

  BOOST_CHECK_MESSAGE(IdenseS.isApprox(IsparseS),std::string(error_prefix + "- Joint YS operation "));

}

template<typename JointModel_> struct init;

template<typename JointModel_>
struct init
{
  static JointModel_ run()
  {
    JointModel_ jmodel;
    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    JointModel jmodel((JointModelRX()));

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
    JointModel jmodel((JointModelRX()));
    jmodel.addJoint(JointModelRY());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename JointModel_>
struct init<pinocchio::JointModelMimic<JointModel_> >
{
  typedef pinocchio::JointModelMimic<JointModel_> JointModel;

  static JointModel run()
  {
    JointModel_ jmodel_ref = init<JointModel_>::run();

    JointModel jmodel(jmodel_ref,1.,0.);
    jmodel.setIndexes(0,0,0);

    return jmodel;
  }
};

struct TestJoint{

  template <typename JointModel>
  void operator()(const JointModelBase<JointModel> & ) const
  {
    JointModel jmodel = init<JointModel>::run();
    jmodel.setIndexes(0,0,0);
    typename JointModel::JointDataDerived jdata = jmodel.createData();

    test_joint_methods(jmodel, jdata);
  }

  void operator()(const pinocchio::JointModelComposite & ) const
  {

  }

};

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointTest;
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointModelTest;
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointDataTest;

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  struct traits< JointDataTest<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  struct traits< JointModelTest<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
  struct traits< JointTest<_Scalar,_Options,JointCollectionTpl> >
  {
    enum {
      Options = _Options,
      NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
      NV = Eigen::Dynamic
    };
    typedef _Scalar Scalar;

    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
    typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionTpl<Scalar,Options>  Motion_t;
    typedef MotionTpl<Scalar,Options>  Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;

    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
  };

  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
  struct JointModelTest
  : JointCollectionTpl<_Scalar,_Options>::JointModelVariant
  , JointModelBase< JointModelTest<_Scalar,_Options,JointCollectionTpl> >
  {
    typedef JointTest<_Scalar,_Options,JointCollectionTpl> JointDerived;
    typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
    typedef typename JointCollection::JointModelVariant VariantBase;
    typedef typename JointCollection::JointModelVariant JointModelVariant;

    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    JointModelTest(const JointModelVariant & jmodel)
    : VariantBase(jmodel)
    {}

  };

}

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(test_joint_from_joint_composite)
{
  typedef JointCollectionDefault JointCollection;
  typedef JointCollection::JointModelVariant JointModelVariant;

  JointModelRX jmodel_revolute_x;
  JointModel jmodel_generic(jmodel_revolute_x);
  JointModelVariant jmodel_variant(jmodel_revolute_x);

  JointModelTest<double,0,JointCollectionDefaultTpl> jmodel_test(jmodel_revolute_x);
  std::vector< JointModelTest<double,0,JointCollectionDefaultTpl> > jmodel_test_vector;
  jmodel_test_vector.push_back(JointModelTest<double,0,JointCollectionDefaultTpl>(jmodel_revolute_x));

  std::vector<JointModelVariant> jmodel_variant_vector;
  jmodel_variant_vector.push_back(jmodel_revolute_x);

  std::vector<JointModel> jmodel_generic_vector;
  jmodel_generic_vector.push_back((JointModel)jmodel_revolute_x);
  JointModelComposite jmodel_composite(jmodel_revolute_x);
}

BOOST_AUTO_TEST_CASE ( test_all_joints )
{
  boost::mpl::for_each<JointModelVariant::types>(TestJoint());
}

BOOST_AUTO_TEST_CASE(test_empty_model)
{
  JointModel jmodel;
  std::cout << "nq " << jmodel.nq() << std::endl;
  std::cout << "nv " << jmodel.nv() << std::endl;
  std::cout << "idx_q " << jmodel.idx_q() << std::endl;
  std::cout << "idx_v " << jmodel.idx_v() << std::endl;
  std::cout << "id " << jmodel.id() << std::endl;
  std::cout << "name " << jmodel.shortname() << std::endl;

  BOOST_CHECK(jmodel.idx_q() == -1);
  BOOST_CHECK(jmodel.idx_v() == -1);
}

BOOST_AUTO_TEST_CASE(isEqual)
{
  JointModelRX joint_revolutex;
  JointModelRY joint_revolutey;

  BOOST_CHECK(joint_revolutex != joint_revolutey);

  JointModel jmodelx(joint_revolutex);
  jmodelx.setIndexes(0,0,0);

  JointModel jmodelx_copy = jmodelx;
  BOOST_CHECK(jmodelx_copy == jmodelx);
  BOOST_CHECK(jmodelx_copy == jmodelx.derived());

  JointModel jmodely(joint_revolutey);
  // TODO: the comparison of two variants is not supported by some old version of BOOST
//  BOOST_CHECK(jmodely.toVariant() != jmodelx.toVariant());
  BOOST_CHECK(jmodely != jmodelx);
}

BOOST_AUTO_TEST_CASE(cast)
{
  JointModelRX joint_revolutex;

  JointModel jmodelx(joint_revolutex);
  jmodelx.setIndexes(0,0,0);

  BOOST_CHECK(jmodelx.cast<double>() == jmodelx);
  BOOST_CHECK(jmodelx.cast<long double>().cast<double>() == jmodelx);
}

struct TestJointOperatorEqual
{

  template <typename JointModel>
  void operator()(const JointModelBase<JointModel> & ) const
  {
    JointModel jmodel_init = init<JointModel>::run();
    typedef typename JointModel::JointDataDerived JointData;

    Model model;
    model.addJoint(0,jmodel_init,SE3::Random(),"toto");
    model.lowerPositionLimit.fill(-1.);
    model.upperPositionLimit.fill( 1.);

    const JointModel & jmodel = boost::get<JointModel>(model.joints[1]);

    Eigen::VectorXd q = randomConfiguration(model);
    Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);

    JointData jdata = jmodel.createData();

    jmodel.calc(jdata,q,v);
    Inertia::Matrix6 I = Inertia::Matrix6::Identity();
    jmodel.calc_aba(jdata,I,false);
    test(jdata);
  }

  template <typename JointModel>
  void operator()(const pinocchio::JointModelMimic<JointModel> & ) const
  {

  }

  template<typename JointData>
  static void test(const JointData & jdata)
  {
    pinocchio::JointData jdata_generic1(jdata);

    std::cout << "name: " << jdata_generic1.shortname() << std::endl;
    BOOST_CHECK(jdata_generic1 == jdata_generic1);
  }

};

BOOST_AUTO_TEST_CASE(test_operator_equal)
{
  boost::mpl::for_each<JointModelVariant::types>(TestJointOperatorEqual());
}

BOOST_AUTO_TEST_SUITE_END ()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2019 CNRS INRIA
3			//
4
5			#include "pinocchio/multibody/joint/joint-generic.hpp"
6
7			#include "pinocchio/multibody/liegroup/liegroup.hpp"
8
9			#include "pinocchio/multibody/model.hpp"
10
11			#include "pinocchio/algorithm/joint-configuration.hpp"
12
13			#include <boost/test/unit_test.hpp>
14			#include <boost/utility/binary.hpp>
15
16			using namespace pinocchio;
17
18			template <typename JointModel>
19		40	void test_joint_methods(JointModelBase<JointModel> & jmodel,
20			JointDataBase<typename JointModel::JointDataDerived> & jdata)
21			{
22			typedef typename LieGroup<JointModel>::type LieGroupType;
23
24	✓✗✓✗ ✓✗✓✗	40	std::cout << "Testing Joint over " << jmodel.shortname() << std::endl;
25
26	✓✗✓✗	80	Eigen::VectorXd q1, q2;
27	✓✗✓✗ ✓✗	40	q1 = LieGroupType().random();
28	✓✗✓✗ ✓✗	40	q2 = LieGroupType().random();
29
30			Eigen::VectorXd
31	✓✗✓✗ ✓✗✓✗	80	v1(Eigen::VectorXd::Random(jdata.S().nv())),
32	✓✗✓✗ ✓✗✓✗	80	v2(Eigen::VectorXd::Random(jdata.S().nv()));
33
34			Inertia::Matrix6
35	✓✗✓✗	40	Ia(pinocchio::Inertia::Random().matrix()),
36	✓✗✓✗	40	Ia2(pinocchio::Inertia::Random().matrix());
37		40	bool update_I = false;
38
39
40	✓✗	40	jmodel.calc(jdata.derived(), q1, v1);
41	✓✗	40	jmodel.calc_aba(jdata.derived(), Ia, update_I);
42
43	✓✗	80	pinocchio::JointModel jma(jmodel);
44	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(jmodel == jma);
45	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(jma == jmodel);
46	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(jma.hasSameIndexes(jmodel));
47
48	✓✗	80	pinocchio::JointData jda(jdata);
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(jda == jdata);
50	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(jdata == jda);
51
52	✓✗	40	jma.calc(jda, q1, v1);
53	✓✗	40	jma.calc_aba(jda, Ia, update_I);
54
55	✓✗	80	pinocchio::JointData jda_other(jdata);
56
57	✓✗	40	jma.calc(jda_other, q2, v2);
58	✓✗	40	jma.calc_aba(jda_other, Ia2, update_I);
59
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(jda_other != jda);
61	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(jda != jda_other);
62	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	40	BOOST_CHECK(jda_other != jdata);
63	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(jdata != jda_other);
64
65	✓✗✓✗	120	const std::string error_prefix("JointModel on " + jma.shortname());
66	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq "));
67	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv "));
68
69	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK_MESSAGE(jmodel.idx_q() == jma.idx_q() ,std::string(error_prefix + " - Idx_q "));
70	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v() ,std::string(error_prefix + " - Idx_v "));
71	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK_MESSAGE(jmodel.id() == jma.id() ,std::string(error_prefix + " - JointId "));
72
73	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	40	BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S().matrix()),std::string(error_prefix + " - ConstraintXd "));
74	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓✗✓	40	BOOST_CHECK_MESSAGE( (jda.M()).isApprox((jdata.M())),std::string(error_prefix + " - Joint transforms ")); // == or isApprox ?
75	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK_MESSAGE( (jda.v()).isApprox( (pinocchio::Motion(jdata.v()))),std::string(error_prefix + " - Joint motions "));
76	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	40	BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c()),std::string(error_prefix + " - Joint bias "));
77
78	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	40	BOOST_CHECK_MESSAGE((jda.U()).isApprox(jdata.U()),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
79	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	40	BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox(jdata.Dinv()),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
80	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	40	BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox(jdata.UDinv()),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));
81
82			// Test vxS
83			typedef typename JointModel::Constraint_t Constraint_t;
84			typedef typename Constraint_t::DenseBase ConstraintDense;
85
86	✓✗	40	Motion v(Motion::Random());
87	✓✗✓✗	40	ConstraintDense vxS(v.cross(jdata.S()));
88	✓✗✓✗ ✓✗✓✗ ✓✗	40	ConstraintDense vxS_ref = v.toActionMatrix() * jdata.S().matrix();
89
90	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	40	BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref),std::string(error_prefix + "- Joint vxS operation "));
91
92			// Test Y*S
93	✓✗	40	const Inertia Isparse(Inertia::Random());
94	✓✗	40	const Inertia::Matrix6 Idense(Isparse.matrix());
95
96	✓✗✓✗	40	const ConstraintDense IsparseS = Isparse * jdata.S();
97	✓✗✓✓ ✗✓✗	40	const ConstraintDense IdenseS = Idense * jdata.S();
98
99	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	40	BOOST_CHECK_MESSAGE(IdenseS.isApprox(IsparseS),std::string(error_prefix + "- Joint YS operation "));
100
101		40	}
102
103			template<typename JointModel_> struct init;
104
105			template<typename JointModel_>
106			struct init
107			{
108		62	static JointModel_ run()
109			{
110		62	JointModel_ jmodel;
111		62	jmodel.setIndexes(0,0,0);
112		62	return jmodel;
113			}
114			};
115
116			template<typename Scalar, int Options>
117			struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
118			{
119			typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;
120
121		2	static JointModel run()
122			{
123			typedef typename JointModel::Vector3 Vector3;
124	✓✗✓✗	2	JointModel jmodel(Vector3::Random().normalized());
125
126		2	jmodel.setIndexes(0,0,0);
127		2	return jmodel;
128			}
129			};
130
131			template<typename Scalar, int Options>
132			struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
133			{
134			typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;
135
136		2	static JointModel run()
137			{
138			typedef typename JointModel::Vector3 Vector3;
139	✓✗✓✗	2	JointModel jmodel(Vector3::Random().normalized());
140
141		2	jmodel.setIndexes(0,0,0);
142		2	return jmodel;
143			}
144			};
145
146			template<typename Scalar, int Options>
147			struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
148			{
149			typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;
150
151		2	static JointModel run()
152			{
153			typedef typename JointModel::Vector3 Vector3;
154	✓✗✓✗	2	JointModel jmodel(Vector3::Random().normalized());
155
156		2	jmodel.setIndexes(0,0,0);
157		2	return jmodel;
158			}
159			};
160
161			template<typename Scalar, int Options, template<typename,int> class JointCollection>
162			struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
163			{
164			typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;
165
166			static JointModel run()
167			{
168			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
169			JointModel jmodel((JointModelRX()));
170
171			jmodel.setIndexes(0,0,0);
172			return jmodel;
173			}
174			};
175
176			template<typename Scalar, int Options, template<typename,int> class JointCollection>
177			struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
178			{
179			typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;
180
181		1	static JointModel run()
182			{
183			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
184			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
185	✓✗✓✗	1	JointModel jmodel((JointModelRX()));
186	✓✗✓✗ ✓✗	1	jmodel.addJoint(JointModelRY());
187
188	✓✗	1	jmodel.setIndexes(0,0,0);
189		1	return jmodel;
190			}
191			};
192
193			template<typename JointModel_>
194			struct init<pinocchio::JointModelMimic<JointModel_> >
195			{
196			typedef pinocchio::JointModelMimic<JointModel_> JointModel;
197
198		6	static JointModel run()
199			{
200	✓✗	6	JointModel_ jmodel_ref = init<JointModel_>::run();
201
202	✓✗	6	JointModel jmodel(jmodel_ref,1.,0.);
203	✓✗	6	jmodel.setIndexes(0,0,0);
204
205		12	return jmodel;
206			}
207			};
208
209			struct TestJoint{
210
211			template <typename JointModel>
212		40	void operator()(const JointModelBase<JointModel> & ) const
213			{
214	✓✗	40	JointModel jmodel = init<JointModel>::run();
215	✓✗	40	jmodel.setIndexes(0,0,0);
216	✓✗	40	typename JointModel::JointDataDerived jdata = jmodel.createData();
217
218	✓✗	40	test_joint_methods(jmodel, jdata);
219		40	}
220
221		1	void operator()(const pinocchio::JointModelComposite & ) const
222			{
223
224		1	}
225
226			};
227
228			namespace pinocchio
229			{
230
231			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointTest;
232			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointModelTest;
233			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointDataTest;
234
235			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
236			struct traits< JointDataTest<Scalar,Options,JointCollectionTpl> >
237			{ typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
238
239			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
240			struct traits< JointModelTest<Scalar,Options,JointCollectionTpl> >
241			{ typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };
242
243			template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
244			struct traits< JointTest<_Scalar,_Options,JointCollectionTpl> >
245			{
246			enum {
247			Options = _Options,
248			NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
249			NV = Eigen::Dynamic
250			};
251			typedef _Scalar Scalar;
252
253			typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
254			typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
255			typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
256			typedef SE3Tpl<Scalar,Options> Transformation_t;
257			typedef MotionTpl<Scalar,Options> Motion_t;
258			typedef MotionTpl<Scalar,Options> Bias_t;
259
260			// [ABA]
261			typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
262			typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
263			typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
264
265			typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
266			typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
267			};
268
269			template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
270			struct JointModelTest
271			: JointCollectionTpl<_Scalar,_Options>::JointModelVariant
272			, JointModelBase< JointModelTest<_Scalar,_Options,JointCollectionTpl> >
273			{
274			typedef JointTest<_Scalar,_Options,JointCollectionTpl> JointDerived;
275			typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
276			typedef typename JointCollection::JointModelVariant VariantBase;
277			typedef typename JointCollection::JointModelVariant JointModelVariant;
278
279			PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
280
281		2	JointModelTest(const JointModelVariant & jmodel)
282		2	: VariantBase(jmodel)
283		2	{}
284
285			};
286
287			}
288
289			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
290
291	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_joint_from_joint_composite)
292			{
293			typedef JointCollectionDefault JointCollection;
294			typedef JointCollection::JointModelVariant JointModelVariant;
295
296	✓✗	2	JointModelRX jmodel_revolute_x;
297	✓✗	4	JointModel jmodel_generic(jmodel_revolute_x);
298	✓✗	4	JointModelVariant jmodel_variant(jmodel_revolute_x);
299
300	✓✗✓✗	4	JointModelTest<double,0,JointCollectionDefaultTpl> jmodel_test(jmodel_revolute_x);
301		4	std::vector< JointModelTest<double,0,JointCollectionDefaultTpl> > jmodel_test_vector;
302	✓✗✓✗ ✓✗	2	jmodel_test_vector.push_back(JointModelTest<double,0,JointCollectionDefaultTpl>(jmodel_revolute_x));
303
304		4	std::vector<JointModelVariant> jmodel_variant_vector;
305	✓✗✓✗	2	jmodel_variant_vector.push_back(jmodel_revolute_x);
306
307		4	std::vector<JointModel> jmodel_generic_vector;
308	✓✗✓✗	2	jmodel_generic_vector.push_back((JointModel)jmodel_revolute_x);
309	✓✗✓✗	2	JointModelComposite jmodel_composite(jmodel_revolute_x);
310		2	}
311
312	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_all_joints )
313			{
314	✓✗	2	boost::mpl::for_each<JointModelVariant::types>(TestJoint());
315		2	}
316
317	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_empty_model)
318			{
319	✓✗	4	JointModel jmodel;
320	✓✗✓✗ ✓✗✓✗	2	std::cout << "nq " << jmodel.nq() << std::endl;
321	✓✗✓✗ ✓✗✓✗	2	std::cout << "nv " << jmodel.nv() << std::endl;
322	✓✗✓✗ ✓✗✓✗	2	std::cout << "idx_q " << jmodel.idx_q() << std::endl;
323	✓✗✓✗ ✓✗✓✗	2	std::cout << "idx_v " << jmodel.idx_v() << std::endl;
324	✓✗✓✗ ✓✗✓✗	2	std::cout << "id " << jmodel.id() << std::endl;
325	✓✗✓✗ ✓✗✓✗	2	std::cout << "name " << jmodel.shortname() << std::endl;
326
327	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jmodel.idx_q() == -1);
328	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jmodel.idx_v() == -1);
329		2	}
330
331	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(isEqual)
332			{
333	✓✗	2	JointModelRX joint_revolutex;
334	✓✗	2	JointModelRY joint_revolutey;
335
336	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(joint_revolutex != joint_revolutey);
337
338	✓✗	4	JointModel jmodelx(joint_revolutex);
339	✓✗	2	jmodelx.setIndexes(0,0,0);
340
341	✓✗	4	JointModel jmodelx_copy = jmodelx;
342	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jmodelx_copy == jmodelx);
343	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jmodelx_copy == jmodelx.derived());
344
345	✓✗	4	JointModel jmodely(joint_revolutey);
346			// TODO: the comparison of two variants is not supported by some old version of BOOST
347			// BOOST_CHECK(jmodely.toVariant() != jmodelx.toVariant());
348	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jmodely != jmodelx);
349		2	}
350
351	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(cast)
352			{
353	✓✗	2	JointModelRX joint_revolutex;
354
355	✓✗	4	JointModel jmodelx(joint_revolutex);
356	✓✗	2	jmodelx.setIndexes(0,0,0);
357
358	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(jmodelx.cast<double>() == jmodelx);
359	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(jmodelx.cast<long double>().cast<double>() == jmodelx);
360		2	}
361
362			struct TestJointOperatorEqual
363			{
364
365			template <typename JointModel>
366		36	void operator()(const JointModelBase<JointModel> & ) const
367			{
368	✓✗	38	JointModel jmodel_init = init<JointModel>::run();
369			typedef typename JointModel::JointDataDerived JointData;
370
371	✓✗	72	Model model;
372	✓✗✓✗ ✓✗✓✗	36	model.addJoint(0,jmodel_init,SE3::Random(),"toto");
373	✓✗	36	model.lowerPositionLimit.fill(-1.);
374	✓✗	36	model.upperPositionLimit.fill( 1.);
375
376	✓✗	36	const JointModel & jmodel = boost::get<JointModel>(model.joints[1]);
377
378	✓✗	72	Eigen::VectorXd q = randomConfiguration(model);
379	✓✗✓✗	72	Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
380
381	✓✗	38	JointData jdata = jmodel.createData();
382
383	✓✗	36	jmodel.calc(jdata,q,v);
384	✓✗✓✗	36	Inertia::Matrix6 I = Inertia::Matrix6::Identity();
385	✓✗	36	jmodel.calc_aba(jdata,I,false);
386	✓✗	36	test(jdata);
387		36	}
388
389			template <typename JointModel>
390		6	void operator()(const pinocchio::JointModelMimic<JointModel> & ) const
391			{
392
393		6	}
394
395			template<typename JointData>
396		36	static void test(const JointData & jdata)
397			{
398	✓✗	72	pinocchio::JointData jdata_generic1(jdata);
399
400	✓✗✓✗ ✓✗✓✗	36	std::cout << "name: " << jdata_generic1.shortname() << std::endl;
401	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(jdata_generic1 == jdata_generic1);
402		36	}
403
404			};
405
406	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_operator_equal)
407			{
408	✓✗	2	boost::mpl::for_each<JointModelVariant::types>(TestJointOperatorEqual());
409		2	}
410
411			BOOST_AUTO_TEST_SUITE_END ()