GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/joint-generic.cpp Lines: 159 159 100.0 %
Date: 2024-04-26 13:14:21 Branches: 607 1213 50.0 %

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