GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/spatial.cpp Lines: 599 599 100.0 %
Date: 2024-01-23 21:41:47 Branches: 2775 5522 50.3 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2020 CNRS INRIA
3
//
4
5
#include <iostream>
6
7
#include "pinocchio/spatial/force.hpp"
8
#include "pinocchio/spatial/motion.hpp"
9
#include "pinocchio/spatial/se3.hpp"
10
#include "pinocchio/spatial/inertia.hpp"
11
#include "pinocchio/spatial/act-on-set.hpp"
12
#include "pinocchio/spatial/explog.hpp"
13
#include "pinocchio/spatial/skew.hpp"
14
#include "pinocchio/spatial/cartesian-axis.hpp"
15
#include "pinocchio/spatial/spatial-axis.hpp"
16
17
#include <boost/test/unit_test.hpp>
18
#include <boost/utility/binary.hpp>
19
20
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
21
22
















4
BOOST_AUTO_TEST_CASE ( test_SE3 )
23
{
24
  using namespace pinocchio;
25
  typedef SE3::HomogeneousMatrixType HomogeneousMatrixType;
26
  typedef SE3::ActionMatrixType ActionMatrixType;
27
  typedef SE3::Vector3 Vector3;
28
  typedef Eigen::Matrix<double,4,1> Vector4;
29
30
2
  const SE3 identity = SE3::Identity();
31
32
  typedef SE3::Quaternion Quaternion;
33
  typedef SE3::Vector4 Vector4;
34

2
  Quaternion quat(Vector4::Random().normalized());
35
36

2
  SE3 m_from_quat(quat,Vector3::Random());
37
38
2
  SE3 amb = SE3::Random();
39
2
  SE3 bmc = SE3::Random();
40
2
  SE3 amc = amb*bmc;
41
42
2
  HomogeneousMatrixType aMb = amb;
43
2
  HomogeneousMatrixType bMc = bmc;
44
45
  // Test internal product
46
2
  HomogeneousMatrixType aMc = amc;
47




2
  BOOST_CHECK(aMc.isApprox(aMb*bMc));
48
49

2
  HomogeneousMatrixType bMa = amb.inverse();
50




2
  BOOST_CHECK(bMa.isApprox(aMb.inverse()));
51
52
  // Test point action
53

2
  Vector3 p = Vector3::Random();
54


2
  Vector4 p4; p4.head(3) = p; p4[3] = 1;
55
56

2
  Vector3 Mp = (aMb*p4).head(3);
57




2
  BOOST_CHECK(amb.act(p).isApprox(Mp));
58
59


2
  Vector3 Mip = (aMb.inverse()*p4).head(3);
60




2
  BOOST_CHECK(amb.actInv(p).isApprox(Mip));
61
62
  // Test action matrix
63
2
  ActionMatrixType aXb = amb;
64
2
  ActionMatrixType bXc = bmc;
65
2
  ActionMatrixType aXc = amc;
66




2
  BOOST_CHECK(aXc.isApprox(aXb*bXc));
67
68

2
  ActionMatrixType bXa = amb.inverse();
69




2
  BOOST_CHECK(bXa.isApprox(aXb.inverse()));
70
71
2
  ActionMatrixType X_identity = identity.toActionMatrix();
72



2
  BOOST_CHECK(X_identity.isIdentity());
73
74
2
  ActionMatrixType X_identity_inverse = identity.toActionMatrixInverse();
75



2
  BOOST_CHECK(X_identity_inverse.isIdentity());
76
77
  // Test dual action matrix
78





2
  BOOST_CHECK(aXb.inverse().transpose().isApprox(amb.toDualActionMatrix()));
79
80
  // Test isIdentity
81



2
  BOOST_CHECK(identity.isIdentity());
82
83
  // Test isApprox
84



2
  BOOST_CHECK(identity.isApprox(identity));
85
86
  // Test cast
87
  typedef SE3Tpl<float> SE3f;
88

2
  SE3f::Matrix3 rot_float(amb.rotation().cast<float>());
89
2
  SE3f amb_float = amb.cast<float>();
90




2
  BOOST_CHECK(amb_float.isApprox(amb.cast<float>()));
91
92
  // Test actInv
93
2
  const SE3 M = SE3::Random();
94
2
  const SE3 Minv = M.inverse();
95
96




2
  BOOST_CHECK(Minv.actInv(Minv).isIdentity());
97




2
  BOOST_CHECK(M.actInv(identity).isApprox(Minv));
98
99
  // Test normalization
100
  {
101
2
    const double prec = Eigen::NumTraits<double>::dummy_precision();
102
2
    SE3 M(SE3::Random());
103


2
    M.rotation() += prec * SE3::Matrix3::Random();
104



2
    BOOST_CHECK(!M.isNormalized());
105
106
2
    SE3 M_normalized = M.normalized();
107



2
    BOOST_CHECK(M_normalized.isNormalized());
108
109
2
    M.normalize();
110



2
    BOOST_CHECK(M.isNormalized());
111
  }
112
113
2
}
114
115
















4
BOOST_AUTO_TEST_CASE ( test_Motion )
116
{
117
  using namespace pinocchio;
118
  typedef SE3::ActionMatrixType ActionMatrixType;
119
  typedef Motion::Vector6 Vector6;
120
121
2
  SE3 amb = SE3::Random();
122
2
  SE3 bmc = SE3::Random();
123
2
  SE3 amc = amb*bmc;
124
125
2
  Motion bv = Motion::Random();
126
2
  Motion bv2 = Motion::Random();
127
128
  typedef MotionBase<Motion> Base;
129
130
2
  Vector6 bv_vec = bv;
131
2
  Vector6 bv2_vec = bv2;
132
133
  // std::stringstream
134
4
  std::stringstream ss;
135

2
  ss << bv << std::endl;
136



2
  BOOST_CHECK(!ss.str().empty());
137
138
  // Test .+.
139

2
  Vector6 bvPbv2_vec = bv+bv2;
140




2
  BOOST_CHECK(bvPbv2_vec.isApprox(bv_vec+bv2_vec));
141
142
2
  Motion bplus = static_cast<Base &>(bv) + static_cast<Base &>(bv2);
143




2
  BOOST_CHECK((bv+bv2).isApprox(bplus));
144
145

2
  Motion v_not_zero(Vector6::Ones());
146



2
  BOOST_CHECK(!v_not_zero.isZero());
147
148

2
  Motion v_zero(Vector6::Zero());
149



2
  BOOST_CHECK(v_zero.isZero());
150
151
  // Test == and !=
152



2
  BOOST_CHECK(bv == bv);
153



2
  BOOST_CHECK(!(bv != bv));
154
155
  // Test -.
156

2
  Vector6 Mbv_vec = -bv;
157




2
  BOOST_CHECK( Mbv_vec.isApprox(-bv_vec));
158
159
  // Test .+=.
160

2
  Motion bv3 = bv; bv3 += bv2;
161




2
  BOOST_CHECK( bv3.toVector().isApprox(bv_vec+bv2_vec));
162
163
  // Test .=V6
164
2
  bv3 = bv2_vec;
165




2
  BOOST_CHECK( bv3.toVector().isApprox(bv2_vec));
166
167
  // Test scalar*M6
168
2
  Motion twicebv(2.*bv);
169





2
  BOOST_CHECK(twicebv.isApprox(Motion(2.*bv.toVector())));
170
171
  // Test M6*scalar
172
2
  Motion bvtwice(bv*2.);
173



2
  BOOST_CHECK(bvtwice.isApprox(twicebv));
174
175
  // Test M6/scalar
176
2
  Motion bvdividedbytwo(bvtwice/2.);
177



2
  BOOST_CHECK(bvdividedbytwo.isApprox(bv));
178
179
  // Test constructor from V6
180
2
  Motion bv4(bv2_vec);
181




2
  BOOST_CHECK( bv4.toVector().isApprox(bv2_vec));
182
183
  // Test action
184
2
  ActionMatrixType aXb = amb;
185





2
  BOOST_CHECK(amb.act(bv).toVector().isApprox(aXb*bv_vec));
186
187
  // Test action inverse
188
2
  ActionMatrixType bXc = bmc;
189





2
  BOOST_CHECK(bmc.actInv(bv).toVector().isApprox(bXc.inverse()*bv_vec));
190
191
  // Test double action
192
2
  Motion cv = Motion::Random();
193
2
  bv = bmc.act(cv);
194





2
  BOOST_CHECK(amb.act(bv).toVector().isApprox(amc.act(cv).toVector()));
195
196
  // Simple test for cross product vxv
197
2
  Motion vxv = bv.cross(bv);
198




2
  BOOST_CHECK_SMALL(vxv.toVector().tail(3).norm(), 1e-3); //previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3));
199
200
  // Test Action Matrix
201
2
  Motion v2xv = bv2.cross(bv);
202
2
  Motion::ActionMatrixType actv2 = bv2.toActionMatrix();
203
204





2
  BOOST_CHECK(v2xv.toVector().isApprox(actv2*bv.toVector()));
205
206
  // Test Dual Action Matrix
207

2
  Force f(bv.toVector());
208
2
  Force v2xf = bv2.cross(f);
209
2
  Motion::ActionMatrixType dualactv2 = bv2.toDualActionMatrix();
210
211





2
  BOOST_CHECK(v2xf.toVector().isApprox(dualactv2*f.toVector()));
212




2
  BOOST_CHECK(dualactv2.isApprox(-actv2.transpose()));
213
214
  // Simple test for cross product vxf
215
2
  Force vxf = bv.cross(f);
216





2
  BOOST_CHECK(vxf.linear().isApprox(bv.angular().cross(f.linear())));
217



2
  BOOST_CHECK_SMALL(vxf.angular().norm(), 1e-3);//previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3));
218
219
  // Test frame change for vxf
220
2
  Motion av = Motion::Random();
221
2
  Force af = Force::Random();
222
2
  bv = amb.actInv(av);
223
2
  Force bf = amb.actInv(af);
224
2
  Force avxf = av.cross(af);
225
2
  Force bvxf = bv.cross(bf);
226





2
  BOOST_CHECK(avxf.toVector().isApprox(amb.act(bvxf).toVector()));
227
228
  // Test frame change for vxv
229
2
  av = Motion::Random();
230
2
  Motion aw = Motion::Random();
231
2
  bv = amb.actInv(av);
232
2
  Motion bw = amb.actInv(aw);
233
2
  Motion avxw = av.cross(aw);
234
2
  Motion bvxw = bv.cross(bw);
235





2
  BOOST_CHECK(avxw.toVector().isApprox(amb.act(bvxw).toVector()));
236
237
  // Test isApprox
238

2
  bv.toVector().setOnes();
239
2
  const double eps = 1e-6;
240



2
  BOOST_CHECK(bv == bv);
241



2
  BOOST_CHECK(bv.isApprox(bv));
242
2
  Motion bv_approx(bv);
243

2
  bv_approx.linear()[0] += eps;
244



2
  BOOST_CHECK(bv_approx.isApprox(bv,eps));
245
246
  // Test ref() method
247
  {
248
2
    Motion a(Motion::Random());
249




2
    BOOST_CHECK(a.ref().isApprox(a));
250
251
2
    const Motion b(a);
252




2
    BOOST_CHECK(b.isApprox(a.ref()));
253
  }
254
255
  // Test cast
256
  {
257
    typedef MotionTpl<float> Motionf;
258
2
    Motion a(Motion::Random());
259
2
    Motionf a_float = a.cast<float>();
260




2
    BOOST_CHECK(a_float.isApprox(a.cast<float>()));
261
  }
262
2
}
263
264
















4
BOOST_AUTO_TEST_CASE (test_motion_ref)
265
{
266
  using namespace pinocchio;
267
  typedef Motion::Vector6 Vector6;
268
269
  typedef MotionRef<Vector6> MotionV6;
270
271
2
  Motion v_ref(Motion::Random());
272

2
  MotionV6 v(v_ref.toVector());
273
274



2
  BOOST_CHECK(v_ref.isApprox(v));
275
276
2
  MotionV6::MotionPlain v2(v*2.);
277
2
  Motion v2_ref(v_ref*2.);
278
279



2
  BOOST_CHECK(v2_ref.isApprox(v2));
280
281
2
  v2 = v_ref + v;
282



2
  BOOST_CHECK(v2_ref.isApprox(v2));
283
284
2
  v = v2;
285



2
  BOOST_CHECK(v2.isApprox(v));
286
287
2
  v2 = v - v;
288




2
  BOOST_CHECK(v2.isApprox(Motion::Zero()));
289
290
2
  SE3 M(SE3::Identity());
291
2
  v2 = M.act(v);
292



2
  BOOST_CHECK(v2.isApprox(v));
293
294
2
  v2 = M.actInv(v);
295



2
  BOOST_CHECK(v2.isApprox(v));
296
297
2
  Motion v3(Motion::Random());
298
2
  v_ref.setRandom();
299
2
  v = v_ref;
300
2
  v2 = v.cross(v3);
301
2
  v2_ref = v_ref.cross(v3);
302
303



2
  BOOST_CHECK(v2.isApprox(v2_ref));
304
305
2
  v.setRandom();
306
2
  v.setZero();
307




2
  BOOST_CHECK(v.isApprox(Motion::Zero()));
308
309
  // Test ref() method
310
  {
311

2
    Vector6 v6(Vector6::Random());
312
2
    MotionV6 a(v6);
313



2
    BOOST_CHECK(a.ref().isApprox(a));
314
315
2
    const Motion b(a);
316



2
    BOOST_CHECK(b.isApprox(a.ref()));
317
  }
318
319
2
}
320
321
















4
BOOST_AUTO_TEST_CASE(test_motion_zero)
322
{
323
  using namespace pinocchio;
324
2
  Motion v((MotionZero()));
325
326




2
  BOOST_CHECK(v.toVector().isZero());
327




2
  BOOST_CHECK(MotionZero() == Motion::Zero());
328
329
  // SE3.act
330
2
  SE3 m(SE3::Random());
331




2
  BOOST_CHECK(m.act(MotionZero()) == Motion::Zero());
332




2
  BOOST_CHECK(m.actInv(MotionZero()) == Motion::Zero());
333
334
  // Motion.cross
335
2
  Motion v2(Motion::Random());
336




2
  BOOST_CHECK(v2.cross(MotionZero()) == Motion::Zero());
337
2
}
338
339
















4
BOOST_AUTO_TEST_CASE ( test_Force )
340
{
341
  using namespace pinocchio;
342
  typedef SE3::ActionMatrixType ActionMatrixType;
343
  typedef Force::Vector6 Vector6;
344
345
2
  SE3 amb = SE3::Random();
346
2
  SE3 bmc = SE3::Random();
347
2
  SE3 amc = amb*bmc;
348
349
2
  Force bf = Force::Random();
350
2
  Force bf2 = Force::Random();
351
352
2
  Vector6 bf_vec = bf;
353
2
  Vector6 bf2_vec = bf2;
354
355
  // std::stringstream
356
4
  std::stringstream ss;
357

2
  ss << bf << std::endl;
358



2
  BOOST_CHECK(!ss.str().empty());
359
360
  // Test .+.
361

2
  Vector6 bfPbf2_vec = bf+bf2;
362




2
  BOOST_CHECK(bfPbf2_vec.isApprox(bf_vec+bf2_vec));
363
364
  // Test -.
365

2
  Vector6 Mbf_vec = -bf;
366




2
  BOOST_CHECK(Mbf_vec.isApprox(-bf_vec));
367
368
  // Test .+=.
369

2
  Force bf3 = bf; bf3 += bf2;
370




2
  BOOST_CHECK(bf3.toVector().isApprox(bf_vec+bf2_vec));
371
372
  // Test .= V6
373
2
  bf3 = bf2_vec;
374




2
  BOOST_CHECK(bf3.toVector().isApprox(bf2_vec));
375
376
  // Test constructor from V6
377
2
  Force bf4(bf2_vec);
378




2
  BOOST_CHECK(bf4.toVector().isApprox(bf2_vec));
379
380
  // Test action
381
2
  ActionMatrixType aXb = amb;
382






2
  BOOST_CHECK(amb.act(bf).toVector().isApprox(aXb.inverse().transpose()*bf_vec));
383
384
  // Test action inverse
385
2
  ActionMatrixType bXc = bmc;
386





2
  BOOST_CHECK(bmc.actInv(bf).toVector().isApprox(bXc.transpose()*bf_vec));
387
388
  // Test double action
389
2
  Force cf = Force::Random();
390

2
  bf = bmc.act(cf);
391





2
  BOOST_CHECK(amb.act(bf).toVector().isApprox(amc.act(cf).toVector()));
392
393
  // Simple test for cross product
394
  // Force vxv = bf.cross(bf);
395
  // ensure that (vxv.toVector().isMuchSmallerThan(bf.toVector()));
396
397

2
  Force f_not_zero(Vector6::Ones());
398



2
  BOOST_CHECK(!f_not_zero.isZero());
399
400

2
  Force f_zero(Vector6::Zero());
401



2
  BOOST_CHECK(f_zero.isZero());
402
403
  // Test isApprox
404
405



2
  BOOST_CHECK(bf == bf);
406
2
  bf.setRandom();
407
2
  bf2.setZero();
408



2
  BOOST_CHECK(bf == bf);
409



2
  BOOST_CHECK(bf != bf2);
410



2
  BOOST_CHECK(bf.isApprox(bf));
411



2
  BOOST_CHECK(!bf.isApprox(bf2));
412
413
2
  const double eps = 1e-6;
414
2
  Force bf_approx(bf);
415

2
  bf_approx.linear()[0] += 2.*eps;
416



2
  BOOST_CHECK(!bf_approx.isApprox(bf,eps));
417
418
  // Test ref() method
419
  {
420
2
    Force a(Force::Random());
421




2
    BOOST_CHECK(a.ref().isApprox(a));
422
423
2
    const Force b(a);
424




2
    BOOST_CHECK(b.isApprox(a.ref()));
425
  }
426
427
  // Test cast
428
  {
429
    typedef ForceTpl<float> Forcef;
430
2
    Force a(Force::Random());
431
2
    Forcef a_float = a.cast<float>();
432




2
    BOOST_CHECK(a_float.isApprox(a.cast<float>()));
433
  }
434
435
  // Test scalar multiplication
436
2
  const double alpha = 1.5;
437
2
  Force b(Force::Random());
438
2
  Force alpha_f = alpha * b;
439
2
  Force f_alpha = b * alpha;
440
441



2
  BOOST_CHECK(alpha_f == f_alpha);
442
2
}
443
444
















4
BOOST_AUTO_TEST_CASE (test_force_ref)
445
{
446
  using namespace pinocchio;
447
  typedef Force::Vector6 Vector6;
448
449
  typedef ForceRef<Vector6> ForceV6;
450
451
2
  Force f_ref(Force::Random());
452

2
  ForceV6 f(f_ref.toVector());
453
454



2
  BOOST_CHECK(f_ref.isApprox(f));
455
456
2
  ForceV6::ForcePlain f2(f*2.);
457
2
  Force f2_ref(f_ref*2.);
458
459



2
  BOOST_CHECK(f2_ref.isApprox(f2));
460
461

2
  f2 = f_ref + f;
462



2
  BOOST_CHECK(f2_ref.isApprox(f2));
463
464
2
  f = f2;
465



2
  BOOST_CHECK(f2.isApprox(f));
466
467

2
  f2 = f - f;
468




2
  BOOST_CHECK(f2.isApprox(Force::Zero()));
469
470
2
  SE3 M(SE3::Identity());
471

2
  f2 = M.act(f);
472



2
  BOOST_CHECK(f2.isApprox(f));
473
474

2
  f2 = M.actInv(f);
475



2
  BOOST_CHECK(f2.isApprox(f));
476
477
2
  Motion v(Motion::Random());
478
2
  f_ref.setRandom();
479
2
  f = f_ref;
480

2
  f2 = v.cross(f);
481

2
  f2_ref = v.cross(f_ref);
482
483



2
  BOOST_CHECK(f2.isApprox(f2_ref));
484
485
2
  f.setRandom();
486
2
  f.setZero();
487




2
  BOOST_CHECK(f.isApprox(Force::Zero()));
488
489
  // Test ref() method
490
  {
491

2
    Vector6 v6(Vector6::Random());
492
2
    ForceV6 a(v6);
493



2
    BOOST_CHECK(a.ref().isApprox(a));
494
495
2
    const Force b(a);
496



2
    BOOST_CHECK(b.isApprox(a.ref()));
497
  }
498
2
}
499
500
















4
BOOST_AUTO_TEST_CASE ( test_Inertia )
501
{
502
  using namespace pinocchio;
503
  typedef Inertia::Matrix6 Matrix6;
504
505
2
  Inertia aI = Inertia::Random();
506
2
  Matrix6 matI = aI;
507



2
  BOOST_CHECK_EQUAL(matI(0,0), aI.mass());
508



2
  BOOST_CHECK_EQUAL(matI(1,1), aI.mass());
509



2
  BOOST_CHECK_EQUAL(matI(2,2), aI.mass()); // 1,1 before unifying
510
511




2
  BOOST_CHECK_SMALL((matI-matI.transpose()).norm(),matI.norm()); //previously ensure that( (matI-matI.transpose()).isMuchSmallerThan(matI) );
512




2
  BOOST_CHECK_SMALL((matI.topRightCorner<3,3>()*aI.lever()).norm(),
513
            aI.lever().norm()); //previously ensure that( (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) );
514
515
2
  Inertia I1 = Inertia::Identity();
516




2
  BOOST_CHECK(I1.matrix().isApprox(Matrix6::Identity()));
517
518
  // Test motion-to-force map
519
2
  Motion v = Motion::Random();
520
2
  Force f = I1 * v;
521




2
  BOOST_CHECK(f.toVector().isApprox(v.toVector()));
522
523
  // Test Inertia group application
524
2
  SE3 bma = SE3::Random();
525
2
  Inertia bI = bma.act(aI);
526
2
  Matrix6 bXa = bma;
527







2
  BOOST_CHECK((bma.rotation()*aI.inertia().matrix()*bma.rotation().transpose())
528
                               .isApprox(bI.inertia().matrix()));
529







2
  BOOST_CHECK((bXa.transpose().inverse() * aI.matrix() * bXa.inverse())
530
                              .isApprox(bI.matrix()));
531
532
  // Test inverse action
533






2
  BOOST_CHECK((bXa.transpose() * bI.matrix() * bXa)
534
                              .isApprox(bma.actInv(bI).matrix()));
535
536
  // Test vxIv cross product
537
2
  v = Motion::Random();
538

2
  f = aI*v;
539
2
  Force vxf = v.cross(f);
540
2
  Force vxIv = aI.vxiv(v);
541




2
  BOOST_CHECK(vxf.toVector().isApprox(vxIv.toVector()));
542
543
  // Test operator+
544

2
  I1 = Inertia::Random();
545
2
  Inertia I2 = Inertia::Random();
546






2
  BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox((I1+I2).matrix()));
547
548
  // operator +=
549
2
  Inertia I12 = I1;
550
2
  I12 += I2;
551





2
  BOOST_CHECK((I1.matrix()+I2.matrix()).isApprox(I12.matrix()));
552
553
  // Test operator vtiv
554



2
  double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector();
555
2
  double kinetic = aI.vtiv(v);
556


2
  BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12);
557
558
  // Test constructor (Matrix6)
559

2
  Inertia I1_bis(I1.matrix());
560




2
  BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix()));
561
562
  // Test Inertia from ellipsoid
563
2
  const double sphere_mass = 5.;
564
2
  const double sphere_radius = 2.;
565

2
  I1 = Inertia::FromSphere(sphere_mass, sphere_radius);
566
2
  const double L_sphere = 2./5. * sphere_mass * sphere_radius * sphere_radius;
567


2
  BOOST_CHECK_SMALL(I1.mass() - sphere_mass, 1e-12);
568



2
  BOOST_CHECK(I1.lever().isZero());
569





2
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(L_sphere, 0., L_sphere , 0., 0., L_sphere).matrix()));
570
571
  // Test Inertia from ellipsoid
572

2
  I1 = Inertia::FromEllipsoid(2., 3., 4., 5.);
573


2
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
574



2
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
575





2
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(
576
          16.4, 0., 13.6, 0., 0., 10.).matrix()));
577
578
  // Test Inertia from Cylinder
579

2
  I1 = Inertia::FromCylinder(2., 4., 6.);
580


2
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
581



2
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
582





2
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(
583
        14., 0., 14., 0., 0., 16.).matrix()));
584
585
  // Test Inertia from Box
586

2
  I1 = Inertia::FromBox(2., 6., 12., 18.);
587


2
  BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12);
588



2
  BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12);
589





2
  BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(
590
        78., 0., 60., 0., 0., 30.).matrix()));
591
592
  // Copy operator
593
2
  Inertia aI_copy(aI);
594



2
  BOOST_CHECK(aI_copy == aI);
595
596
  // Test isZero
597
2
  Inertia I_not_zero = Inertia::Identity();
598



2
  BOOST_CHECK(!I_not_zero.isZero());
599
600
2
  Inertia I_zero = Inertia::Zero();
601



2
  BOOST_CHECK(I_zero.isZero());
602
603
  // Test isApprox
604
2
  const double eps = 1e-6;
605



2
  BOOST_CHECK(aI == aI);
606



2
  BOOST_CHECK(aI.isApprox(aI));
607
2
  Inertia aI_approx(aI);
608
2
  aI_approx.mass() += eps/2.;
609



2
  BOOST_CHECK(aI_approx.isApprox(aI,eps));
610
611
  // Test Variation
612
2
  Inertia::Matrix6 aIvariation = aI.variation(v);
613
614
2
  Motion::ActionMatrixType vAction = v.toActionMatrix();
615
2
  Motion::ActionMatrixType vDualAction = v.toDualActionMatrix();
616
617
2
  Inertia::Matrix6 aImatrix = aI.matrix();
618


2
  Inertia::Matrix6 aIvariation_ref = vDualAction * aImatrix - aImatrix * vAction;
619
620



2
  BOOST_CHECK(aIvariation.isApprox(aIvariation_ref));
621





2
  BOOST_CHECK(vxIv.isApprox(Force(aIvariation*v.toVector())));
622
623
  // Test vxI operator
624
  {
625
    typedef Inertia::Matrix6 Matrix6;
626
2
    Inertia I(Inertia::Random());
627
2
    Motion v(Motion::Random());
628
629


2
    const Matrix6 M_ref(v.toDualActionMatrix()*I.matrix());
630

2
    Matrix6 M; Inertia::vxi(v,I,M);
631
632



2
    BOOST_CHECK(M.isApprox(M_ref));
633




2
    BOOST_CHECK(I.vxi(v).isApprox(M_ref));
634
  }
635
636
  // Test Ivx operator
637
  {
638
    typedef Inertia::Matrix6 Matrix6;
639
2
    Inertia I(Inertia::Random());
640
2
    Motion v(Motion::Random());
641
642


2
    const Matrix6 M_ref(I.matrix()*v.toActionMatrix());
643

2
    Matrix6 M; Inertia::ivx(v,I,M);
644
645



2
    BOOST_CHECK(M.isApprox(M_ref));
646




2
    BOOST_CHECK(I.ivx(v).isApprox(M_ref));
647
  }
648
649
  // Test variation against vxI - Ivx operator
650
  {
651
    typedef Inertia::Matrix6 Matrix6;
652
2
    Inertia I(Inertia::Random());
653
2
    Motion v(Motion::Random());
654
655
2
    Matrix6 Ivariation = I.variation(v);
656
657

2
    Matrix6 M1; Inertia::vxi(v,I,M1);
658

2
    Matrix6 M2; Inertia::ivx(v,I,M2);
659

2
    Matrix6 M3(M1-M2);
660
661



2
    BOOST_CHECK(M3.isApprox(Ivariation));
662
  }
663
664
  // Test dynamic parameters
665
  {
666
2
    Inertia I(Inertia::Random());
667
668
2
    Inertia::Vector10 v = I.toDynamicParameters();
669
670



2
    BOOST_CHECK_CLOSE(v[0], I.mass(), 1e-12);
671
672




2
    BOOST_CHECK(v.segment<3>(1).isApprox(I.mass()*I.lever()));
673
674




2
    Eigen::Matrix3d I_o = I.inertia() + I.mass()*skew(I.lever()).transpose()*skew(I.lever());
675
2
    Eigen::Matrix3d I_ov;
676



4
    I_ov << v[4], v[5], v[7],
677



2
            v[5], v[6], v[8],
678



2
            v[7], v[8], v[9];
679
680



2
    BOOST_CHECK(I_o.isApprox(I_ov));
681
682
2
    Inertia I2 = Inertia::FromDynamicParameters(v);
683



2
    BOOST_CHECK(I2.isApprox(I));
684
685
  }
686
687
  // Test disp
688

2
  std::cout << aI << std::endl;
689
690
2
}
691
692
















4
BOOST_AUTO_TEST_CASE(cast_inertia)
693
{
694
  using namespace pinocchio;
695
2
  Inertia Y(Inertia::Random());
696
697




2
  BOOST_CHECK(Y.cast<double>() == Y);
698




2
  BOOST_CHECK(Y.cast<long double>().cast<double>() == Y);
699
2
}
700
701
















4
BOOST_AUTO_TEST_CASE ( test_ActOnSet )
702
{
703
  using namespace pinocchio;
704
2
  const int N = 20;
705
  typedef Eigen::Matrix<double,6,N> Matrix6N;
706
2
  SE3 jMi = SE3::Random();
707
2
  Motion v = Motion::Random();
708
709
  // Forcet SET
710



2
  Matrix6N iF = Matrix6N::Random(),jF,jFinv,jF_ref,jFinv_ref;
711
712
  // forceSet::se3Action
713
2
  forceSet::se3Action(jMi,iF,jF);
714
42
  for( int k=0;k<N;++k )
715






40
    BOOST_CHECK(jMi.act(Force(iF.col(k))).toVector().isApprox(jF.col(k)));
716
717

2
  jF_ref = jMi.toDualActionMatrix()*iF;
718



2
  BOOST_CHECK(jF_ref.isApprox(jF));
719
720

2
  forceSet::se3ActionInverse(jMi.inverse(),iF,jFinv);
721



2
  BOOST_CHECK(jFinv.isApprox(jF));
722
723

2
  Matrix6N iF2 = Matrix6N::Random();
724

2
  jF_ref += jMi.toDualActionMatrix() * iF2;
725
726
2
  forceSet::se3Action<ADDTO>(jMi,iF2,jF);
727



2
  BOOST_CHECK(jF.isApprox(jF_ref));
728
729

2
  Matrix6N iF3 = Matrix6N::Random();
730

2
  jF_ref -= jMi.toDualActionMatrix() * iF3;
731
732
2
  forceSet::se3Action<RMTO>(jMi,iF3,jF);
733



2
  BOOST_CHECK(jF.isApprox(jF_ref));
734
735
  // forceSet::se3ActionInverse
736
2
  forceSet::se3ActionInverse(jMi,iF,jFinv);
737


2
  jFinv_ref = jMi.inverse().toDualActionMatrix() * iF;
738



2
  BOOST_CHECK(jFinv_ref.isApprox(jFinv));
739
740


2
  jFinv_ref += jMi.inverse().toDualActionMatrix() * iF2;
741
2
  forceSet::se3ActionInverse<ADDTO>(jMi,iF2,jFinv);
742



2
  BOOST_CHECK(jFinv.isApprox(jFinv_ref));
743
744


2
  jFinv_ref -= jMi.inverse().toDualActionMatrix() * iF3;
745
2
  forceSet::se3ActionInverse<RMTO>(jMi,iF3,jFinv);
746



2
  BOOST_CHECK(jFinv.isApprox(jFinv_ref));
747
748
  // forceSet::motionAction
749
2
  forceSet::motionAction(v,iF,jF);
750
42
  for( int k=0;k<N;++k )
751






40
    BOOST_CHECK(v.cross(Force(iF.col(k))).toVector().isApprox(jF.col(k)));
752
753

2
  jF_ref = v.toDualActionMatrix() * iF;
754



2
  BOOST_CHECK(jF.isApprox(jF_ref));
755
756

2
  jF_ref += v.toDualActionMatrix() * iF2;
757
2
  forceSet::motionAction<ADDTO>(v,iF2,jF);
758



2
  BOOST_CHECK(jF.isApprox(jF_ref));
759
760

2
  jF_ref -= v.toDualActionMatrix() * iF3;
761
2
  forceSet::motionAction<RMTO>(v,iF3,jF);
762



2
  BOOST_CHECK(jF.isApprox(jF_ref));
763
764
  // Motion SET
765



2
  Matrix6N iV = Matrix6N::Random(),jV,jV_ref,jVinv,jVinv_ref;
766
767
  // motionSet::se3Action
768
2
  motionSet::se3Action(jMi,iV,jV);
769
42
  for( int k=0;k<N;++k )
770






40
    BOOST_CHECK(jMi.act(Motion(iV.col(k))).toVector().isApprox(jV.col(k)));
771
772

2
  jV_ref = jMi.toActionMatrix()*iV;
773



2
  BOOST_CHECK(jV.isApprox(jV_ref));
774
775

2
  motionSet::se3ActionInverse(jMi.inverse(),iV,jVinv);
776



2
  BOOST_CHECK(jVinv.isApprox(jV));
777
778

2
  Matrix6N iV2 = Matrix6N::Random();
779

2
  jV_ref += jMi.toActionMatrix()*iV2;
780
2
  motionSet::se3Action<ADDTO>(jMi,iV2,jV);
781



2
  BOOST_CHECK(jV.isApprox(jV_ref));
782
783

2
  Matrix6N iV3 = Matrix6N::Random();
784

2
  jV_ref -= jMi.toActionMatrix()*iV3;
785
2
  motionSet::se3Action<RMTO>(jMi,iV3,jV);
786



2
  BOOST_CHECK(jV.isApprox(jV_ref));
787
788
  // motionSet::se3ActionInverse
789
2
  motionSet::se3ActionInverse(jMi,iV,jVinv);
790


2
  jVinv_ref = jMi.inverse().toActionMatrix() * iV;
791



2
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));
792
793


2
  jVinv_ref += jMi.inverse().toActionMatrix()*iV2;
794
2
  motionSet::se3ActionInverse<ADDTO>(jMi,iV2,jVinv);
795



2
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));
796
797


2
  jVinv_ref -= jMi.inverse().toActionMatrix()*iV3;
798
2
  motionSet::se3ActionInverse<RMTO>(jMi,iV3,jVinv);
799



2
  BOOST_CHECK(jVinv.isApprox(jVinv_ref));
800
801
  // motionSet::motionAction
802
2
  motionSet::motionAction(v,iV,jV);
803
42
  for( int k=0;k<N;++k )
804






40
    BOOST_CHECK(v.cross(Motion(iV.col(k))).toVector().isApprox(jV.col(k)));
805
806

2
  jV_ref = v.toActionMatrix()*iV;
807



2
  BOOST_CHECK(jV.isApprox(jV_ref));
808
809

2
  jV_ref += v.toActionMatrix()*iV2;
810
2
  motionSet::motionAction<ADDTO>(v,iV2,jV);
811



2
  BOOST_CHECK(jV.isApprox(jV_ref));
812
813

2
  jV_ref -= v.toActionMatrix()*iV3;
814
2
  motionSet::motionAction<RMTO>(v,iV3,jV);
815



2
  BOOST_CHECK(jV.isApprox(jV_ref));
816
817
  // motionSet::inertiaAction
818
2
  const Inertia I(Inertia::Random());
819
2
  motionSet::inertiaAction(I,iV,jV);
820
42
  for( int k=0;k<N;++k )
821






40
    BOOST_CHECK((I*(Motion(iV.col(k)))).toVector().isApprox(jV.col(k)));
822
823

2
  jV_ref = I.matrix()*iV;
824



2
  BOOST_CHECK(jV.isApprox(jV_ref));
825
826

2
  jV_ref += I.matrix()*iV2;
827
2
  motionSet::inertiaAction<ADDTO>(I,iV2,jV);
828



2
  BOOST_CHECK(jV.isApprox(jV_ref));
829
830

2
  jV_ref -= I.matrix()*iV3;
831
2
  motionSet::inertiaAction<RMTO>(I,iV3,jV);
832



2
  BOOST_CHECK(jV.isApprox(jV_ref));
833
834
  // motionSet::act
835
2
  Force f = Force::Random();
836
2
  motionSet::act(iV,f,jF);
837
42
  for( int k=0;k<N;++k )
838






40
    BOOST_CHECK(Motion(iV.col(k)).cross(f).toVector().isApprox(jF.col(k)));
839
840
42
  for( int k=0;k<N;++k )
841



40
    jF_ref.col(k) = Force(Motion(iV.col(k)).cross(f)).toVector();
842



2
  BOOST_CHECK(jF.isApprox(jF_ref));
843
844
42
  for( int k=0;k<N;++k )
845



40
    jF_ref.col(k) += Force(Motion(iV2.col(k)).cross(f)).toVector();
846
2
  motionSet::act<ADDTO>(iV2,f,jF);
847



2
  BOOST_CHECK(jF.isApprox(jF_ref));
848
849
42
  for( int k=0;k<N;++k )
850



40
    jF_ref.col(k) -= Force(Motion(iV3.col(k)).cross(f)).toVector();
851
2
  motionSet::act<RMTO>(iV3,f,jF);
852



2
  BOOST_CHECK(jF.isApprox(jF_ref));
853
2
}
854
855
















4
BOOST_AUTO_TEST_CASE(test_skew)
856
{
857
  using namespace pinocchio;
858
  typedef SE3::Vector3 Vector3;
859
  typedef Motion::Vector6 Vector6;
860
861

2
  Vector3 v3(Vector3::Random());
862

2
  Vector6 v6(Vector6::Random());
863
864

2
  Vector3 res1 = unSkew(skew(v3));
865



2
  BOOST_CHECK(res1.isApprox(v3));
866
867

2
  Vector3 res2 = unSkew(skew(v6.head<3>()));
868




2
  BOOST_CHECK(res2.isApprox(v6.head<3>()));
869
870

2
  Vector3 res3 = skew(v3)*v3;
871



2
  BOOST_CHECK(res3.isZero());
872
873

2
  Vector3 rhs(Vector3::Random());
874

2
  Vector3 res41 = skew(v3)*rhs;
875
2
  Vector3 res42 = v3.cross(rhs);
876
877



2
  BOOST_CHECK(res41.isApprox(res42));
878
879
2
}
880
881
















4
BOOST_AUTO_TEST_CASE(test_addSkew)
882
{
883
  using namespace pinocchio;
884
  typedef SE3::Vector3 Vector3;
885
  typedef SE3::Matrix3 Matrix3;
886
887

2
  Vector3 v(Vector3::Random());
888

2
  Matrix3 M(Matrix3::Random());
889
2
  Matrix3 Mcopy(M);
890
891
2
  addSkew(v,M);
892

2
  Matrix3 Mref = Mcopy + skew(v);
893



2
  BOOST_CHECK(M.isApprox(Mref));
894
895

2
  Mref += skew(-v);
896

2
  addSkew(-v,M);
897



2
  BOOST_CHECK(M.isApprox(Mcopy));
898
899
2
  M.setZero();
900
2
  addSkew(v,M);
901




2
  BOOST_CHECK(M.isApprox(skew(v)));
902
2
}
903
904
















4
BOOST_AUTO_TEST_CASE(test_skew_square)
905
{
906
  using namespace pinocchio;
907
  typedef SE3::Vector3 Vector3;
908
  typedef SE3::Matrix3 Matrix3;
909
910

2
  Vector3 u(Vector3::Random());
911

2
  Vector3 v(Vector3::Random());
912
913


2
  Matrix3 ref = skew(u) * skew(v);
914
915
2
  Matrix3 res = skewSquare(u,v);
916
917



2
  BOOST_CHECK(res.isApprox(ref));
918
2
}
919
920
template<int axis>
921
struct test_scalar_multiplication_cartesian_axis
922
{
923
  typedef pinocchio::CartesianAxis<axis> Axis;
924
  typedef double Scalar;
925
  typedef Eigen::Matrix<Scalar,3,1> Vector3;
926
927
6
  static void run()
928
  {
929
6
    const Scalar alpha = static_cast <Scalar> (rand()) / static_cast <Scalar> (RAND_MAX);
930
6
    const Vector3 r1 = Axis() * alpha;
931
6
    const Vector3 r2 = alpha * Axis();
932
933



6
    BOOST_CHECK(r1.isApprox(r2));
934
935
24
    for(int k = 0; k < Axis::dim; ++k)
936
    {
937
18
      if(k==axis)
938
      {
939



6
        BOOST_CHECK(r1[k] == alpha);
940



6
        BOOST_CHECK(r2[k] == alpha);
941
      }
942
      else
943
      {
944



12
        BOOST_CHECK(r1[k] == Scalar(0));
945



12
        BOOST_CHECK(r2[k] == Scalar(0));
946
      }
947
    }
948
6
  }
949
};
950
951
















4
BOOST_AUTO_TEST_CASE(test_cartesian_axis)
952
{
953
  using namespace Eigen;
954
  using namespace pinocchio;
955

2
  Vector3d v(Vector3d::Random());
956
2
  const double alpha = 3;
957

2
  Vector3d v2(alpha*v);
958
959





2
  BOOST_CHECK(AxisX::cross(v).isApprox(Vector3d::Unit(0).cross(v)));
960





2
  BOOST_CHECK(AxisY::cross(v).isApprox(Vector3d::Unit(1).cross(v)));
961





2
  BOOST_CHECK(AxisZ::cross(v).isApprox(Vector3d::Unit(2).cross(v)));
962





2
  BOOST_CHECK(AxisX::alphaCross(alpha,v).isApprox(Vector3d::Unit(0).cross(v2)));
963





2
  BOOST_CHECK(AxisY::alphaCross(alpha,v).isApprox(Vector3d::Unit(1).cross(v2)));
964





2
  BOOST_CHECK(AxisZ::alphaCross(alpha,v).isApprox(Vector3d::Unit(2).cross(v2)));
965
966
2
  test_scalar_multiplication_cartesian_axis<0>::run();
967
2
  test_scalar_multiplication_cartesian_axis<1>::run();
968
2
  test_scalar_multiplication_cartesian_axis<2>::run();
969
2
}
970
971
template<int axis>
972
struct test_scalar_multiplication
973
{
974
  typedef pinocchio::SpatialAxis<axis> Axis;
975
  typedef double Scalar;
976
  typedef pinocchio::MotionTpl<Scalar> Motion;
977
978
12
  static void run()
979
  {
980
12
    const Scalar alpha = static_cast <Scalar> (rand()) / static_cast <Scalar> (RAND_MAX);
981
12
    const Motion r1 = Axis() * alpha;
982
12
    const Motion r2 = alpha * Axis();
983
984



12
    BOOST_CHECK(r1.isApprox(r2));
985
986
84
    for(int k = 0; k < Axis::dim; ++k)
987
    {
988
72
      if(k==axis)
989
      {
990




12
        BOOST_CHECK(r1.toVector()[k] == alpha);
991




12
        BOOST_CHECK(r2.toVector()[k] == alpha);
992
      }
993
      else
994
      {
995




60
        BOOST_CHECK(r1.toVector()[k] == Scalar(0));
996




60
        BOOST_CHECK(r2.toVector()[k] == Scalar(0));
997
      }
998
    }
999
12
  }
1000
};
1001
1002
















4
BOOST_AUTO_TEST_CASE(test_spatial_axis)
1003
{
1004
  using namespace pinocchio;
1005
1006
2
  Motion v(Motion::Random());
1007
2
  Force f(Force::Random());
1008
1009
2
  Motion vaxis;
1010
2
  vaxis << AxisVX();
1011




2
  BOOST_CHECK(AxisVX::cross(v).isApprox(vaxis.cross(v)));
1012




2
  BOOST_CHECK(v.cross(AxisVX()).isApprox(v.cross(vaxis)));
1013




2
  BOOST_CHECK(AxisVX::cross(f).isApprox(vaxis.cross(f)));
1014
1015
2
  vaxis << AxisVY();
1016




2
  BOOST_CHECK(AxisVY::cross(v).isApprox(vaxis.cross(v)));
1017




2
  BOOST_CHECK(v.cross(AxisVY()).isApprox(v.cross(vaxis)));
1018




2
  BOOST_CHECK(AxisVY::cross(f).isApprox(vaxis.cross(f)));
1019
1020
2
  vaxis << AxisVZ();
1021




2
  BOOST_CHECK(AxisVZ::cross(v).isApprox(vaxis.cross(v)));
1022




2
  BOOST_CHECK(v.cross(AxisVZ()).isApprox(v.cross(vaxis)));
1023




2
  BOOST_CHECK(AxisVZ::cross(f).isApprox(vaxis.cross(f)));
1024
1025
2
  vaxis << AxisWX();
1026




2
  BOOST_CHECK(AxisWX::cross(v).isApprox(vaxis.cross(v)));
1027




2
  BOOST_CHECK(v.cross(AxisWX()).isApprox(v.cross(vaxis)));
1028




2
  BOOST_CHECK(AxisWX::cross(f).isApprox(vaxis.cross(f)));
1029
1030
2
  vaxis << AxisWY();
1031




2
  BOOST_CHECK(AxisWY::cross(v).isApprox(vaxis.cross(v)));
1032




2
  BOOST_CHECK(v.cross(AxisWY()).isApprox(v.cross(vaxis)));
1033




2
  BOOST_CHECK(AxisWY::cross(f).isApprox(vaxis.cross(f)));
1034
1035
2
  vaxis << AxisWZ();
1036




2
  BOOST_CHECK(AxisWZ::cross(v).isApprox(vaxis.cross(v)));
1037




2
  BOOST_CHECK(v.cross(AxisWZ()).isApprox(v.cross(vaxis)));
1038




2
  BOOST_CHECK(AxisWZ::cross(f).isApprox(vaxis.cross(f)));
1039
1040
  // Test operation Axis * Scalar
1041
2
  test_scalar_multiplication<0>::run();
1042
2
  test_scalar_multiplication<1>::run();
1043
2
  test_scalar_multiplication<2>::run();
1044
2
  test_scalar_multiplication<3>::run();
1045
2
  test_scalar_multiplication<4>::run();
1046
2
  test_scalar_multiplication<5>::run();
1047
1048
  // Operations of Constraint on forces Sxf
1049
  typedef Motion::ActionMatrixType ActionMatrixType;
1050
  typedef ActionMatrixType::ColXpr ColType;
1051
  typedef ForceRef<ColType> ForceRefOnColType;
1052
  typedef MotionRef<ColType> MotionRefOnColType;
1053

2
  ActionMatrixType Sxf,Sxf_ref;
1054

2
  ActionMatrixType S(ActionMatrixType::Identity());
1055
1056

2
  SpatialAxis<0>::cross(f,ForceRefOnColType(Sxf.col(0)));
1057

2
  SpatialAxis<1>::cross(f,ForceRefOnColType(Sxf.col(1)));
1058

2
  SpatialAxis<2>::cross(f,ForceRefOnColType(Sxf.col(2)));
1059

2
  SpatialAxis<3>::cross(f,ForceRefOnColType(Sxf.col(3)));
1060

2
  SpatialAxis<4>::cross(f,ForceRefOnColType(Sxf.col(4)));
1061

2
  SpatialAxis<5>::cross(f,ForceRefOnColType(Sxf.col(5)));
1062
1063
14
  for(int k = 0; k < 6; ++k)
1064
  {
1065

12
    MotionRefOnColType Scol(S.col(k));
1066


12
    ForceRefOnColType(Sxf_ref.col(k)) = Scol.cross(f);
1067
  }
1068
1069



2
  BOOST_CHECK(Sxf.isApprox(Sxf_ref));
1070
2
}
1071
1072
BOOST_AUTO_TEST_SUITE_END ()