GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/constraint.cpp Lines: 153 153 100.0 %
Date: 2024-01-23 21:41:47 Branches: 471 929 50.7 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
//
4
5
#include "pinocchio/spatial/se3.hpp"
6
#include "pinocchio/spatial/inertia.hpp"
7
#include "pinocchio/multibody/force-set.hpp"
8
#include "pinocchio/multibody/model.hpp"
9
#include "pinocchio/algorithm/joint-configuration.hpp"
10
11
#include "utils/macros.hpp"
12
13
#include <iostream>
14
15
#include <boost/test/unit_test.hpp>
16
#include <boost/utility/binary.hpp>
17
18
using namespace pinocchio;
19
20
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21
22
















4
BOOST_AUTO_TEST_CASE (test_ForceSet)
23
{
24
  using namespace pinocchio;
25
26
2
  SE3 amb = SE3::Random();
27
2
  SE3 bmc = SE3::Random();
28
2
  SE3 amc = amb*bmc;
29
30
4
  ForceSet F(12);
31


6
  ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
32

2
  F.block(10,2) = F2;
33




2
  BOOST_CHECK_EQUAL(F.matrix().col(10).norm() , 0.0 );
34




2
  BOOST_CHECK(std::isnan(F.matrix()(0,9)));
35
36


6
  ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
37
4
  ForceSet F4 = amb.act(F3);
38
2
  SE3::Matrix6 aXb= amb;
39






2
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix(), 1e-12));
40
41
42
4
  ForceSet bF = bmc.act(F3);
43
4
  ForceSet aF = amb.act(bF);
44
4
  ForceSet aF2 = amc.act(F3);
45




2
  BOOST_CHECK(aF.matrix().isApprox(aF2.matrix(), 1e-12));
46
47

4
  ForceSet F36 = amb.act(F3.block(3,6));
48






2
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix(), 1e-12));
49
50
51


4
  ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6));
52







2
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36full.matrix().block(0,3,6,6),
53
                                                            1e-12));
54
2
}
55
56
















4
BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
57
{
58
  using namespace pinocchio;
59
60
2
  Inertia Y = Inertia::Random();
61
2
  JointDataRX::Constraint_t S;
62
63


4
  ForceSet F1(1); F1.block(0,1) = Y*S;
64





2
  BOOST_CHECK(F1.matrix().isApprox(Y.matrix().col(3), 1e-12));
65
66


6
  ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
67


4
  Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3).matrix();
68






2
  BOOST_CHECK(StF2.isApprox(S.matrix().transpose()*F2.matrix().block(0,5,6,3)
69
                            , 1e-12));
70
2
}
71
72
template<typename JointModel>
73
36
void test_jmodel_nq_against_nq_ref(const JointModelBase<JointModel> & jmodel,
74
                                   const int & nq_ref)
75
{
76



36
  BOOST_CHECK(jmodel.nq() == nq_ref);
77
36
}
78
79
template<typename JointModel>
80
6
void test_jmodel_nq_against_nq_ref(const JointModelMimic<JointModel> & jmodel,
81
                                   const int & nq_ref)
82
{
83



6
  BOOST_CHECK(jmodel.jmodel().nq() == nq_ref);
84
6
}
85
86
template<typename JointModel, typename ConstraintDerived>
87
36
void test_nv_against_jmodel(const JointModelBase<JointModel> & jmodel,
88
                            const ConstraintBase<ConstraintDerived> & constraint)
89
{
90



36
  BOOST_CHECK(constraint.nv() == jmodel.nv());
91
36
}
92
93
template<typename JointModel, typename ConstraintDerived>
94
6
void test_nv_against_jmodel(const JointModelMimic<JointModel> & jmodel,
95
                            const ConstraintBase<ConstraintDerived> & constraint)
96
{
97



6
  BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv());
98
6
}
99
100
template<class JointModel>
101
struct buildModel
102
{
103
36
  static Model run(const JointModelBase<JointModel> & jmodel)
104
  {
105
36
    Model model;
106


36
    model.addJoint(0,jmodel,SE3::Identity(),"joint");
107
108
36
    return model;
109
  }
110
};
111
112
template<class JointModel>
113
struct buildModel< JointModelMimic<JointModel> >
114
{
115
  typedef JointModelMimic<JointModel> JointModel_;
116
117
6
  static Model run(const JointModel_ & jmodel)
118
  {
119
6
    Model model;
120


6
    model.addJoint(0,jmodel.jmodel(),SE3::Identity(),"joint");
121


6
    model.addJoint(0,jmodel,SE3::Identity(),"joint_mimic");
122
123
6
    return model;
124
  }
125
};
126
127
template<typename JointModel>
128
42
void test_constraint_operations(const JointModelBase<JointModel> & jmodel)
129
{
130
  typedef typename traits<JointModel>::JointDerived Joint;
131
  typedef typename traits<Joint>::Constraint_t ConstraintType;
132
  typedef typename traits<Joint>::JointDataDerived JointData;
133
  typedef Eigen::Matrix<typename JointModel::Scalar,6,Eigen::Dynamic> Matrix6x;
134
135
44
  JointData jdata = jmodel.createData();
136
  typedef typename JointModel::ConfigVector_t ConfigVector_t;
137
44
  ConfigVector_t q;
138
139
  // We need to use a model here in order to call the randomConfiguration to init q.
140
84
  Model model = buildModel<JointModel>::run(jmodel.derived());
141
142
42
  test_jmodel_nq_against_nq_ref(jmodel.derived(),model.nq);
143
144

42
  q = randomConfiguration(model,
145
42
                          ConfigVector_t::Constant(model.nq,-1.),
146
42
                          ConfigVector_t::Constant(model.nq, 1.));
147
148
  // By calling jmodel.calc, we then have jdata.S which is initialized with non NaN quantities
149
42
  jmodel.calc(jdata,q);
150
151
18
  ConstraintType constraint(jdata.S);
152
153
42
  test_nv_against_jmodel(jmodel.derived(),constraint);
154




42
  BOOST_CHECK(constraint.cols() == constraint.nv());
155



42
  BOOST_CHECK(constraint.rows() == 6);
156
157
  typedef typename JointModel::TangentVector_t TangentVector_t;
158

44
  TangentVector_t v = TangentVector_t::Random(constraint.nv());
159
160

44
  typename ConstraintType::DenseBase constraint_mat = constraint.matrix();
161

42
  Motion m = constraint * v;
162

42
  Motion m_ref = Motion(constraint_mat * v);
163
164



42
  BOOST_CHECK(m.isApprox(m_ref));
165
166
  // Test SE3 action
167
  {
168
42
    SE3 M = SE3::Random();
169
44
    typename ConstraintType::DenseBase S = M.act(constraint);
170

44
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());
171
172

114
    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
173
    {
174
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
175


72
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
176
177

72
      m_out = M.act(m_in);
178
    }
179
180



42
    BOOST_CHECK(S.isApprox(S_ref));
181
  }
182
183
  // Test SE3 action inverse
184
  {
185
42
    SE3 M = SE3::Random();
186
44
    typename ConstraintType::DenseBase S = M.actInv(constraint);
187

44
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());
188
189

114
    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
190
    {
191
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
192


72
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
193
194

72
      m_out = M.actInv(m_in);
195
    }
196
197



42
    BOOST_CHECK(S.isApprox(S_ref));
198
  }
199
200
  // Test SE3 action and SE3 action inverse
201
  {
202
42
    const SE3 M = SE3::Random();
203
42
    const SE3 Minv = M.inverse();
204
205
44
    typename ConstraintType::DenseBase S1_vice = M.actInv(constraint);
206
44
    typename ConstraintType::DenseBase S2_vice = Minv.act(constraint);
207
208



42
    BOOST_CHECK(S1_vice.isApprox(S2_vice));
209
210
44
    typename ConstraintType::DenseBase S1_versa = M.act(constraint);
211
44
    typename ConstraintType::DenseBase S2_versa = Minv.actInv(constraint);
212
213



42
    BOOST_CHECK(S1_versa.isApprox(S2_versa));
214
215
  }
216
217
  // Test Motion action
218
  {
219
42
    Motion v = Motion::Random();
220
221
44
    typename ConstraintType::DenseBase S = v.cross(constraint);
222

44
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());
223
224

114
    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
225
    {
226
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
227


72
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
228
229

72
      m_out = v.cross(m_in);
230
    }
231



42
    BOOST_CHECK(S.isApprox(S_ref));
232
  }
233
234
  // Test transpose operations
235
  {
236
42
    const Eigen::DenseIndex dim = 20;
237

84
    const Matrix6x Fin = Matrix6x::Random(6,dim);
238


84
    Eigen::MatrixXd Fout = constraint.transpose() * Fin;
239

84
    Eigen::MatrixXd Fout_ref = constraint_mat.transpose() * Fin;
240



42
    BOOST_CHECK(Fout.isApprox(Fout_ref));
241
242
42
    Force force_in(Force::Random());
243


84
    Eigen::MatrixXd Stf = (constraint.transpose() * force_in);
244


84
    Eigen::MatrixXd Stf_ref = constraint_mat.transpose() * force_in.toVector();
245



42
    BOOST_CHECK(Stf_ref.isApprox(Stf));
246
  }
247
248
  // CRBA operations
249
  {
250
42
    const Inertia Y = Inertia::Random();
251

84
    Eigen::MatrixXd YS = Y * constraint;
252

84
    Eigen::MatrixXd YS_ref = Y.matrix() * constraint_mat;
253



42
    BOOST_CHECK(YS.isApprox(YS_ref));
254
  }
255
256
  // ABA operations
257
  {
258
42
    const Inertia Y = Inertia::Random();
259
42
    const Inertia::Matrix6 Y_mat = Y.matrix();
260

84
    Eigen::MatrixXd YS = Y_mat * constraint;
261

84
    Eigen::MatrixXd YS_ref = Y_mat * constraint_mat;
262



42
    BOOST_CHECK(YS.isApprox(YS_ref));
263
  }
264
265
42
}
266
267
template<typename JointModel_> struct init;
268
269
template<typename JointModel_>
270
struct init
271
{
272
34
  static JointModel_ run()
273
  {
274
34
    JointModel_ jmodel;
275
34
    jmodel.setIndexes(0,0,0);
276
34
    return jmodel;
277
  }
278
};
279
280
template<typename Scalar, int Options>
281
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
282
{
283
  typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;
284
285
1
  static JointModel run()
286
  {
287
    typedef typename JointModel::Vector3 Vector3;
288

1
    JointModel jmodel(Vector3::Random().normalized());
289
290
1
    jmodel.setIndexes(0,0,0);
291
1
    return jmodel;
292
  }
293
};
294
295
template<typename Scalar, int Options>
296
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
297
{
298
  typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;
299
300
1
  static JointModel run()
301
  {
302
    typedef typename JointModel::Vector3 Vector3;
303

1
    JointModel jmodel(Vector3::Random().normalized());
304
305
1
    jmodel.setIndexes(0,0,0);
306
1
    return jmodel;
307
  }
308
};
309
310
template<typename Scalar, int Options>
311
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
312
{
313
  typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;
314
315
1
  static JointModel run()
316
  {
317
    typedef typename JointModel::Vector3 Vector3;
318

1
    JointModel jmodel(Vector3::Random().normalized());
319
320
1
    jmodel.setIndexes(0,0,0);
321
1
    return jmodel;
322
  }
323
};
324
325
template<typename Scalar, int Options, template<typename,int> class JointCollection>
326
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
327
{
328
  typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;
329
330
  static JointModel run()
331
  {
332
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
333
    JointModel jmodel((JointModelRX()));
334
335
    jmodel.setIndexes(0,0,0);
336
    return jmodel;
337
  }
338
};
339
340
template<typename Scalar, int Options, template<typename,int> class JointCollection>
341
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
342
{
343
  typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;
344
345
1
  static JointModel run()
346
  {
347
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
348
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
349
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ;
350
351

1
    JointModel jmodel(JointModelRX(),SE3::Random());
352

1
    jmodel.addJoint(JointModelRY(),SE3::Random());
353

1
    jmodel.addJoint(JointModelRZ(),SE3::Random());
354
355
1
    jmodel.setIndexes(0,0,0);
356
357
1
    return jmodel;
358
  }
359
};
360
361
template<typename JointModel_>
362
struct init<pinocchio::JointModelMimic<JointModel_> >
363
{
364
  typedef pinocchio::JointModelMimic<JointModel_> JointModel;
365
366
6
  static JointModel run()
367
  {
368
6
    JointModel_ jmodel_ref = init<JointModel_>::run();
369
370
6
    JointModel jmodel(jmodel_ref,1.,0.);
371
6
    jmodel.setIndexes(0,0,0);
372
373
12
    return jmodel;
374
  }
375
};
376
377
struct TestJointConstraint
378
{
379
380
  template <typename JointModel>
381
42
  void operator()(const JointModelBase<JointModel> &) const
382
  {
383
44
    JointModel jmodel = init<JointModel>::run();
384
42
    jmodel.setIndexes(0,0,0);
385
386
42
    test_constraint_operations(jmodel);
387
42
  }
388
389
};
390
391
















4
BOOST_AUTO_TEST_CASE(test_joint_constraint_operations)
392
{
393
  typedef JointCollectionDefault::JointModelVariant Variant;
394
2
  boost::mpl::for_each<Variant::types>(TestJointConstraint());
395
2
}
396
397
BOOST_AUTO_TEST_SUITE_END()