GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/model.cpp Lines: 338 346 97.7 %
Date: 2024-04-26 13:14:21 Branches: 1341 2704 49.6 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2016-2022 CNRS INRIA
3
//
4
5
#include "pinocchio/multibody/data.hpp"
6
#include "pinocchio/multibody/model.hpp"
7
8
#include "pinocchio/algorithm/check.hpp"
9
#include "pinocchio/algorithm/model.hpp"
10
#include "pinocchio/algorithm/kinematics.hpp"
11
#include "pinocchio/algorithm/frames.hpp"
12
#include "pinocchio/algorithm/joint-configuration.hpp"
13
#include "pinocchio/algorithm/geometry.hpp"
14
15
#include "pinocchio/parsers/sample-models.hpp"
16
17
#include <boost/test/unit_test.hpp>
18
#include <boost/utility/binary.hpp>
19
20
using namespace pinocchio;
21
22
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
23
24
















4
  BOOST_AUTO_TEST_CASE(test_model_subtree)
25
  {
26
4
    Model model;
27
2
    buildModels::humanoidRandom(model);
28
29

2
    Model::JointIndex idx_larm1 = model.getJointId("larm1_joint");
30



2
    BOOST_CHECK(idx_larm1<(Model::JointIndex)model.njoints);
31
4
    Model::IndexVector subtree = model.subtrees[idx_larm1];
32



2
    BOOST_CHECK(subtree.size()==6);
33
34
12
    for(size_t i=1; i<subtree.size();++i)
35
    {
36



10
      BOOST_CHECK(model.parents[subtree[i]]==subtree[i-1]);
37
    }
38
39
    // Check that i starts subtree[i]
40
56
    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
41
    {
42



54
      BOOST_CHECK(model.subtrees[joint_id][0] == joint_id);
43
    }
44
45
    // Check that subtree[0] contains all joint ids
46
56
    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
47
    {
48



54
      BOOST_CHECK(model.subtrees[0][joint_id-1] == joint_id);
49
    }
50
2
  }
51
52
















4
  BOOST_AUTO_TEST_CASE(test_model_get_frame_id)
53
  {
54
4
    Model model;
55
2
    buildModels::humanoidRandom(model);
56
57
112
    for(FrameIndex i=0; i<static_cast<FrameIndex>(model.nframes); i++)
58
    {
59



110
      BOOST_CHECK_EQUAL(i, model.getFrameId(model.frames[i].name));
60
    }
61



2
    BOOST_CHECK_EQUAL(model.nframes, model.getFrameId("NOT_A_FRAME"));
62
2
  }
63
64
















4
  BOOST_AUTO_TEST_CASE(test_model_support)
65
  {
66
4
    Model model;
67
2
    buildModels::humanoidRandom(model);
68
4
    const Model::IndexVector support0_ref(1,0);
69



2
    BOOST_CHECK(model.supports[0] == support0_ref);
70
71
    // Check that i ends supports[i]
72
56
    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
73
    {
74



54
      BOOST_CHECK(model.supports[joint_id].back() == joint_id);
75
54
      Model::IndexVector & support = model.supports[joint_id];
76
77
54
      size_t current_id = support.size()-2;
78
54
      for(JointIndex parent_id = model.parents[joint_id];
79
276
          parent_id > 0;
80
222
          parent_id = model.parents[parent_id],
81
          current_id--)
82
      {
83



222
        BOOST_CHECK(parent_id == support[current_id]);
84
      }
85
    }
86
2
  }
87
88
















4
  BOOST_AUTO_TEST_CASE(test_model_subspace_dimensions)
89
  {
90
4
    Model model;
91
2
    buildModels::humanoidRandom(model);
92
93
    // Check that i ends supports[i]
94
56
    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
95
    {
96
54
      const Model::JointModel & jmodel = model.joints[joint_id];
97
98



54
      BOOST_CHECK(model.nqs[joint_id] == jmodel.nq());
99



54
      BOOST_CHECK(model.idx_qs[joint_id] == jmodel.idx_q());
100
101



54
      BOOST_CHECK(model.nvs[joint_id] == jmodel.nv());
102



54
      BOOST_CHECK(model.idx_vs[joint_id] == jmodel.idx_v());
103
    }
104
2
  }
105
106
















4
  BOOST_AUTO_TEST_CASE(comparison)
107
  {
108
4
    Model model;
109
2
    buildModels::humanoidRandom(model);
110
111



2
    BOOST_CHECK(model == model);
112
2
  }
113
114
















4
  BOOST_AUTO_TEST_CASE(cast)
115
  {
116
4
    Model model;
117
2
    buildModels::humanoidRandom(model);
118
119




2
    BOOST_CHECK(model.cast<double>() == model.cast<double>());
120





2
    BOOST_CHECK(model.cast<double>().cast<long double>() == model.cast<long double>());
121
2
  }
122
123
















4
  BOOST_AUTO_TEST_CASE(test_std_vector_of_Model)
124
  {
125
4
    Model model;
126
2
    buildModels::humanoid(model);
127
128
4
    PINOCCHIO_ALIGNED_STD_VECTOR(Model) models;
129
42
    for(size_t k = 0; k < 20; ++k)
130
    {
131

40
      models.push_back(Model());
132
40
      buildModels::humanoid(models.back());
133
134



40
      BOOST_CHECK(model == models.back());
135
    }
136
2
  }
137
138
#ifdef PINOCCHIO_WITH_HPP_FCL
139
  struct AddPrefix {
140
    std::string p;
141
35
    std::string operator() (const std::string& n) { return p + n; }
142
74
    Frame operator() (const Frame& _f) { Frame f (_f); f.name = p + f.name; return f; }
143
2
    AddPrefix (const char* _p) : p(_p) {}
144
  };
145
146
















4
  BOOST_AUTO_TEST_CASE(append)
147
  {
148

4
    Model manipulator, humanoid;
149

4
    GeometryModel geomManipulator, geomHumanoid;
150
151
2
    buildModels::manipulator(manipulator);
152
2
    buildModels::manipulatorGeometries(manipulator, geomManipulator);
153
2
    geomManipulator.addAllCollisionPairs();
154
    // Add prefix to joint and frame names
155
4
    AddPrefix addManipulatorPrefix ("manipulator/");
156
2
    std::transform (++manipulator.names.begin(), manipulator.names.end(),
157

4
        ++manipulator.names.begin(), addManipulatorPrefix);
158
2
    std::transform (++manipulator.frames.begin(), manipulator.frames.end(),
159

4
        ++manipulator.frames.begin(), addManipulatorPrefix);
160
161


2
    BOOST_TEST_MESSAGE(manipulator);
162
163
2
    buildModels::humanoid(humanoid);
164
2
    buildModels::humanoidGeometries(humanoid, geomHumanoid);
165
2
    geomHumanoid.addAllCollisionPairs();
166
    // Add prefix to joint and frame names
167
4
    AddPrefix addHumanoidPrefix ("humanoid/");
168
2
    std::transform (++humanoid.names.begin(), humanoid.names.end(),
169

4
        ++humanoid.names.begin(), addHumanoidPrefix);
170
2
    std::transform (++humanoid.frames.begin(), humanoid.frames.end(),
171

4
        ++humanoid.frames.begin(), addHumanoidPrefix);
172
173


2
    BOOST_TEST_MESSAGE(humanoid);
174
175
    //TODO fix inertia of the base
176
2
    manipulator.inertias[0].setRandom();
177
2
    SE3 aMb = SE3::Random();
178
179
    // First append a model to the universe frame.
180
4
    Model model1;
181
4
    GeometryModel geomModel1;
182
2
    FrameIndex fid = 0;
183
2
    appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid,
184
2
        SE3::Identity(), model1, geomModel1);
185
186
    {
187

4
      Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity());
188
4
      Model model3;
189

2
      appendModel(humanoid, manipulator, fid, SE3::Identity(), model3);
190



2
      BOOST_CHECK(model1 == model2);
191



2
      BOOST_CHECK(model1 == model3);
192



2
      BOOST_CHECK(model2 == model3);
193
    }
194
195
4
    Data data1 (model1);
196



2
    BOOST_CHECK(model1.check(data1));
197
198


2
    BOOST_TEST_MESSAGE(model1);
199
200
    // Second, append a model to a moving frame.
201
4
    Model model;
202
203
4
    GeometryModel geomModel;
204




2
    fid = humanoid.addFrame (Frame ("humanoid/add_manipulator",
205
          humanoid.getJointId("humanoid/chest2_joint"),
206
          humanoid.getFrameId("humanoid/chest2_joint"), aMb,
207
          OP_FRAME));
208
209
    // Append manipulator to chest2_joint of humanoid
210
2
    appendModel (humanoid, manipulator, geomHumanoid, geomManipulator, fid,
211
2
        SE3::Identity(), model, geomModel);
212
213
    {
214

4
      Model model2 = appendModel(humanoid, manipulator, fid, SE3::Identity());
215
4
      Model model3;
216

2
      appendModel(humanoid, manipulator, fid, SE3::Identity(), model3);
217



2
      BOOST_CHECK(model == model2);
218



2
      BOOST_CHECK(model == model3);
219



2
      BOOST_CHECK(model2 == model3);
220
    }
221
222


2
    BOOST_TEST_MESSAGE(model);
223
224
4
    Data data (model);
225



2
    BOOST_CHECK(model.check(data));
226
227
    // Check the model
228




2
    BOOST_CHECK_EQUAL(model.getJointId("humanoid/chest2_joint"),
229
        model.parents[model.getJointId("manipulator/shoulder1_joint")]);
230
231
    // check the joint order and the inertias
232
    // All the joints of the manipulator should be at the end of the merged model
233
2
    JointIndex hnj = (JointIndex) humanoid.njoints;
234

2
    JointIndex chest2 = model.getJointId("humanoid/chest2_joint");
235
60
    for (JointIndex jid = 1; jid < hnj; ++jid) {
236




58
      BOOST_TEST_MESSAGE("Checking joint " << jid << " " << model.names[jid]);
237


58
      BOOST_CHECK_EQUAL(model.names[jid], humanoid.names[jid]);
238
58
      if (jid != chest2)
239


56
	BOOST_CHECK_EQUAL(model.inertias[jid], humanoid.inertias[jid]);
240
      else
241






2
	BOOST_CHECK_MESSAGE(model.inertias[jid].isApprox(manipulator.inertias[0].se3Action(aMb) +
242
							    humanoid.inertias[jid]),
243
	  model.inertias[jid] << " != " << manipulator.inertias[0].se3Action(aMb) +
244
	  humanoid.inertias[jid]);
245
246


58
      BOOST_CHECK_EQUAL(model.jointPlacements[jid], humanoid.jointPlacements[jid]);
247
    }
248
12
    for (JointIndex jid = 1; jid < manipulator.joints.size()-1; ++jid) {
249




10
      BOOST_TEST_MESSAGE("Checking joint " << hnj -1 +jid << " " << model.names[hnj+jid]);
250


10
      BOOST_CHECK_EQUAL(model.names[hnj-1+jid], manipulator.names[jid]);
251


10
      BOOST_CHECK_EQUAL(model.inertias[hnj-1+jid], manipulator.inertias[jid]);
252
10
      if (jid==1)
253



2
        BOOST_CHECK_EQUAL(model.jointPlacements[hnj-1+jid], aMb*manipulator.jointPlacements[jid]);
254
      else
255


8
        BOOST_CHECK_EQUAL(model.jointPlacements[hnj-1+jid], manipulator.jointPlacements[jid]);
256
    }
257
    // Check the frames
258
126
    for (FrameIndex fid = 1; fid < humanoid.frames.size(); ++fid) {
259
124
      const Frame& frame  = humanoid.frames[fid],
260
248
                   parent = humanoid.frames[frame.previousFrame];
261



124
      BOOST_CHECK(model.existFrame (frame.name, frame.type));
262
124
      const Frame& nframe  = model.frames[model.getFrameId(frame.name, frame.type)],
263
248
                   nparent = model.frames[nframe.previousFrame];
264


124
      BOOST_CHECK_EQUAL(parent.name, nparent.name);
265


124
      BOOST_CHECK_EQUAL(frame.placement, nframe.placement);
266
    }
267
28
    for (FrameIndex fid = 1; fid < manipulator.frames.size(); ++fid) {
268
26
      const Frame& frame  = manipulator.frames[fid],
269
52
                   parent = manipulator.frames[frame.previousFrame];
270



26
      BOOST_CHECK(model.existFrame (frame.name, frame.type));
271
26
      const Frame& nframe  = model.frames[model.getFrameId(frame.name, frame.type)],
272
52
                   nparent = model.frames[nframe.previousFrame];
273
26
      if (frame.previousFrame > 0) {
274


24
        BOOST_CHECK_EQUAL(parent.name, nparent.name);
275


24
        BOOST_CHECK_EQUAL(frame.placement, nframe.placement);
276
      }
277
    }
278
2
  }
279
#endif
280
281
















4
BOOST_AUTO_TEST_CASE(test_buildReducedModel_empty)
282
{
283
4
  Model humanoid_model;
284
2
  buildModels::humanoid(humanoid_model);
285
286

2
  static const std::vector<JointIndex> empty_index_vector;
287
288

2
  humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
289

2
  humanoid_model.upperPositionLimit.head<3>().fill( 1.);
290
291

2
  humanoid_model.referenceConfigurations.insert(std::pair<std::string,Eigen::VectorXd>("neutral",neutral(humanoid_model)));
292
4
  Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model);
293
4
  Model humanoid_copy_model = buildReducedModel(humanoid_model,empty_index_vector,reference_config_humanoid);
294
295



2
  BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names);
296



2
  BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints);
297



2
  BOOST_CHECK(humanoid_copy_model == humanoid_model);
298





2
  BOOST_CHECK(humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model)));
299
300
4
  const std::vector<JointIndex> empty_joints_to_lock;
301
302
4
  const Model reduced_humanoid_model = buildReducedModel(humanoid_model,empty_joints_to_lock,reference_config_humanoid);
303



2
  BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints);
304



2
  BOOST_CHECK(reduced_humanoid_model.frames == humanoid_model.frames);
305



2
  BOOST_CHECK(reduced_humanoid_model.jointPlacements == humanoid_model.jointPlacements);
306



2
  BOOST_CHECK(reduced_humanoid_model.joints == humanoid_model.joints);
307
308
60
  for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
309
  {
310



58
    BOOST_CHECK(reduced_humanoid_model.inertias[joint_id].isApprox(humanoid_model.inertias[joint_id]));
311
  }
312
2
}
313
314
















4
BOOST_AUTO_TEST_CASE(test_buildReducedModel)
315
{
316
4
  Model humanoid_model;
317
2
  buildModels::humanoid(humanoid_model);
318
319

2
  static const std::vector<JointIndex> empty_index_vector;
320
321

2
  humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
322

2
  humanoid_model.upperPositionLimit.head<3>().fill( 1.);
323
324

2
  humanoid_model.referenceConfigurations.insert(std::pair<std::string,Eigen::VectorXd>("neutral",neutral(humanoid_model)));
325
4
  Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model);
326
4
  Model humanoid_copy_model = buildReducedModel(humanoid_model,empty_index_vector,reference_config_humanoid);
327
328



2
  BOOST_CHECK(humanoid_copy_model.names == humanoid_model.names);
329



2
  BOOST_CHECK(humanoid_copy_model.joints == humanoid_model.joints);
330



2
  BOOST_CHECK(humanoid_copy_model == humanoid_model);
331





2
  BOOST_CHECK(humanoid_copy_model.referenceConfigurations["neutral"].isApprox(neutral(humanoid_copy_model)));
332
333
4
  std::vector<JointIndex> joints_to_lock;
334
4
  const std::string joint1_to_lock = "rarm_shoulder2_joint";
335

2
  joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock));
336
4
  const std::string joint2_to_lock = "larm_shoulder2_joint";
337

2
  joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock));
338
339
4
  Model reduced_humanoid_model = buildReducedModel(humanoid_model,joints_to_lock,reference_config_humanoid);
340



2
  BOOST_CHECK(reduced_humanoid_model.njoints == humanoid_model.njoints-(int)joints_to_lock.size());
341
342



2
  BOOST_CHECK(reduced_humanoid_model != humanoid_model);
343
344
4
  Model reduced_humanoid_model_other_signature;
345

2
  buildReducedModel(humanoid_model,joints_to_lock,reference_config_humanoid,reduced_humanoid_model_other_signature);
346



2
  BOOST_CHECK(reduced_humanoid_model == reduced_humanoid_model_other_signature);
347
348
  // Check that forward kinematics give same results
349

4
  Data data(humanoid_model), reduced_data(reduced_humanoid_model);
350
4
  Eigen::VectorXd q = reference_config_humanoid;
351
4
  Eigen::VectorXd reduced_q(reduced_humanoid_model.nq);
352
353
56
  for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
354
  {
355
54
    const JointIndex reference_joint_id = humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]);
356
357
54
    reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) =
358

108
    humanoid_model.joints[reference_joint_id].jointConfigSelector(q);
359
  }
360
361





2
  BOOST_CHECK(reduced_humanoid_model.referenceConfigurations["neutral"].isApprox(neutral(reduced_humanoid_model)));
362
363
2
  framesForwardKinematics(humanoid_model,data,q);
364
2
  framesForwardKinematics(reduced_humanoid_model,reduced_data,reduced_q);
365
366
126
  for(size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id)
367
  {
368
124
    const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id];
369
124
    switch(reduced_frame.type)
370
    {
371
60
      case JOINT:
372
      case FIXED_JOINT:
373
      {
374
        // May not be present in the original model
375

60
        if(humanoid_model.existJointName(reduced_frame.name))
376
        {
377
60
          const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name);
378



60
          BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id]));
379
        }
380
        else if(humanoid_model.existFrame(reduced_frame.name))
381
        {
382
          const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
383
          BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
384
        }
385
        else
386
        {
387
          BOOST_CHECK_MESSAGE(false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model");
388
        }
389
60
        break;
390
      }
391
64
      default:
392
      {
393



64
        BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name));
394
64
        const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
395



64
        BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
396
64
        break;
397
      }
398
399
    }
400
  }
401
2
}
402
403
















4
BOOST_AUTO_TEST_CASE(test_aligned_vector_of_model)
404
{
405
  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Model) VectorOfModels;
406
407
4
  VectorOfModels models;
408
202
  for(size_t k = 0; k < 100; ++k)
409
  {
410

200
    models.push_back(Model());
411
200
    buildModels::humanoidRandom(models[k]);
412
  }
413
2
}
414
415
#ifdef PINOCCHIO_WITH_HPP_FCL
416
















4
BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom)
417
{
418
4
  Model humanoid_model;
419
2
  buildModels::humanoid(humanoid_model);
420
421

2
  humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
422

2
  humanoid_model.upperPositionLimit.head<3>().fill( 1.);
423
4
  const Eigen::VectorXd reference_config_humanoid = randomConfiguration(humanoid_model);
424
425
4
  GeometryModel humanoid_geometry;
426
2
  buildModels::humanoidGeometries(humanoid_model, humanoid_geometry);
427
428

2
  static const std::vector<JointIndex> empty_index_vector;
429
430

4
  Model humanoid_copy_model; GeometryModel humanoid_copy_geometry;
431
2
  buildReducedModel(humanoid_model,humanoid_geometry,empty_index_vector,
432
                    reference_config_humanoid,humanoid_copy_model,humanoid_copy_geometry);
433
434



2
  BOOST_CHECK(humanoid_copy_model == humanoid_model);
435



2
  BOOST_CHECK(humanoid_copy_geometry == humanoid_geometry);
436
437
4
  std::vector<JointIndex> joints_to_lock;
438
4
  const std::string joint1_to_lock = "rarm_shoulder2_joint";
439

2
  joints_to_lock.push_back(humanoid_model.getJointId(joint1_to_lock));
440
4
  const std::string joint2_to_lock = "larm_shoulder2_joint";
441

2
  joints_to_lock.push_back(humanoid_model.getJointId(joint2_to_lock));
442
443

4
  Model reduced_humanoid_model; GeometryModel reduced_humanoid_geometry;
444
2
  buildReducedModel(humanoid_model,humanoid_geometry,joints_to_lock,
445
                    reference_config_humanoid,reduced_humanoid_model,reduced_humanoid_geometry);
446
447



2
  BOOST_CHECK(reduced_humanoid_geometry.ngeoms == humanoid_geometry.ngeoms);
448



2
  BOOST_CHECK(reduced_humanoid_geometry.collisionPairs == humanoid_geometry.collisionPairs);
449



2
  BOOST_CHECK(reduced_humanoid_geometry.geometryObjects.size() == humanoid_geometry.geometryObjects.size());
450
451
56
  for(Index i = 0; i < humanoid_geometry.geometryObjects.size(); ++i)
452
  {
453
54
    const GeometryObject & go1 = humanoid_geometry.geometryObjects[i];
454
54
    const GeometryObject & go2 = reduced_humanoid_geometry.geometryObjects[i];
455


54
    BOOST_CHECK_EQUAL(go1.name, go2.name);
456


54
    BOOST_CHECK_EQUAL(go1.geometry, go2.geometry);
457


54
    BOOST_CHECK_EQUAL(go1.meshPath, go2.meshPath);
458


54
    BOOST_CHECK_EQUAL(go1.meshScale, go2.meshScale);
459


54
    BOOST_CHECK_EQUAL(go1.overrideMaterial, go2.overrideMaterial);
460


54
    BOOST_CHECK_EQUAL(go1.meshColor, go2.meshColor);
461


54
    BOOST_CHECK_EQUAL(go1.meshTexturePath, go2.meshTexturePath);
462


54
    BOOST_CHECK_EQUAL(go1.parentFrame, go2.parentFrame);
463


54
    BOOST_CHECK_EQUAL(humanoid_model.frames[go1.parentFrame].name,
464
                      reduced_humanoid_model.frames[go2.parentFrame].name);
465
  }
466
467

4
  Data data(humanoid_model), reduced_data(reduced_humanoid_model);
468
4
  const Eigen::VectorXd q = reference_config_humanoid;
469
4
  Eigen::VectorXd reduced_q(reduced_humanoid_model.nq);
470
471
56
  for(JointIndex joint_id = 1; joint_id < (JointIndex)reduced_humanoid_model.njoints; ++joint_id)
472
  {
473
54
    const JointIndex reference_joint_id = humanoid_model.getJointId(reduced_humanoid_model.names[joint_id]);
474
475
54
    reduced_humanoid_model.joints[joint_id].jointConfigSelector(reduced_q) =
476

108
    humanoid_model.joints[reference_joint_id].jointConfigSelector(q);
477
  }
478
479
2
  framesForwardKinematics(humanoid_model,data,q);
480
2
  framesForwardKinematics(reduced_humanoid_model,reduced_data,reduced_q);
481
482
126
  for(size_t frame_id = 0; frame_id < reduced_humanoid_model.frames.size(); ++frame_id)
483
  {
484
124
    const Frame & reduced_frame = reduced_humanoid_model.frames[frame_id];
485
124
    switch(reduced_frame.type)
486
    {
487
60
      case JOINT:
488
      case FIXED_JOINT:
489
      {
490
        // May not be present in the original model
491

60
        if(humanoid_model.existJointName(reduced_frame.name))
492
        {
493
60
          const JointIndex joint_id = humanoid_model.getJointId(reduced_frame.name);
494



60
          BOOST_CHECK(data.oMi[joint_id].isApprox(reduced_data.oMf[frame_id]));
495
        }
496
        else if(humanoid_model.existFrame(reduced_frame.name))
497
        {
498
          const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
499
          BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
500
        }
501
        else
502
        {
503
          BOOST_CHECK_MESSAGE(false, "The frame " << reduced_frame.name << " is not presend in the humanoid_model");
504
        }
505
60
        break;
506
      }
507
64
      default:
508
      {
509



64
        BOOST_CHECK(humanoid_model.existFrame(reduced_frame.name));
510
64
        const FrameIndex humanoid_frame_id = humanoid_model.getFrameId(reduced_frame.name);
511



64
        BOOST_CHECK(data.oMf[humanoid_frame_id].isApprox(reduced_data.oMf[frame_id]));
512
64
        break;
513
      }
514
515
    }
516
  }
517
518
  // Test GeometryObject placements
519

4
  GeometryData geom_data(humanoid_geometry), reduded_geom_data(reduced_humanoid_geometry);
520
2
  updateGeometryPlacements(humanoid_model,data,humanoid_geometry,geom_data);
521
2
  updateGeometryPlacements(reduced_humanoid_model,reduced_data,reduced_humanoid_geometry,reduded_geom_data);
522
523



2
  BOOST_CHECK(geom_data.oMg.size() == reduded_geom_data.oMg.size());
524
56
  for(FrameIndex i = 0; i < geom_data.oMg.size(); ++i)
525
  {
526



54
    BOOST_CHECK(geom_data.oMg[i].isApprox(reduded_geom_data.oMg[i]));
527
  }
528
529
  // Test other signature
530
4
  std::vector<GeometryModel> full_geometry_models;
531
2
  full_geometry_models.push_back(humanoid_geometry);
532
2
  full_geometry_models.push_back(humanoid_geometry);
533
2
  full_geometry_models.push_back(humanoid_geometry);
534
535
4
  std::vector<GeometryModel> reduced_geometry_models;
536
537
4
  Model reduced_humanoid_model_other_sig;
538
2
  buildReducedModel(humanoid_model,full_geometry_models,joints_to_lock,
539
                    reference_config_humanoid,reduced_humanoid_model_other_sig,reduced_geometry_models);
540
541



2
  BOOST_CHECK(reduced_geometry_models[0] == reduced_humanoid_geometry);
542



2
  BOOST_CHECK(reduced_geometry_models[1] == reduced_humanoid_geometry);
543



2
  BOOST_CHECK(reduced_geometry_models[2] == reduced_humanoid_geometry);
544
2
}
545
#endif // PINOCCHIO_WITH_HPP_FCL
546
547
















4
BOOST_AUTO_TEST_CASE(test_has_configuration_limit)
548
{
549
  using namespace Eigen;
550
551
  // Test joint specific function hasConfigurationLimit
552
2
  JointModelFreeFlyer test_joint_ff = JointModelFreeFlyer();
553
4
  std::vector<bool> cf_limits_ff = test_joint_ff.hasConfigurationLimit();
554
4
  std::vector<bool> cf_limits_tangent_ff = test_joint_ff.hasConfigurationLimitInTangent();
555
4
  std::vector<bool> expected_cf_limits_ff({true, true, true, false, false, false, false});
556
4
  std::vector<bool> expected_cf_limits_tangent_ff({true, true, true, false, false, false});
557



2
  BOOST_CHECK(cf_limits_ff == expected_cf_limits_ff);
558



2
  BOOST_CHECK(cf_limits_tangent_ff == expected_cf_limits_tangent_ff);
559
560
2
  JointModelPlanar test_joint_planar = JointModelPlanar();
561
4
  std::vector<bool> cf_limits_planar = test_joint_planar.hasConfigurationLimit();
562
4
  std::vector<bool> cf_limits_tangent_planar = test_joint_planar.hasConfigurationLimitInTangent();
563
4
  std::vector<bool> expected_cf_limits_planar({true, true, false, false});
564
4
  std::vector<bool> expected_cf_limits_tangent_planar({true, true, false});
565



2
  BOOST_CHECK(cf_limits_planar == expected_cf_limits_planar);
566



2
  BOOST_CHECK(cf_limits_tangent_planar == expected_cf_limits_tangent_planar);
567
568
2
  JointModelPX test_joint_p = JointModelPX();
569
4
  std::vector<bool> cf_limits_prismatic = test_joint_p.hasConfigurationLimit();
570
4
  std::vector<bool> cf_limits_tangent_prismatic = test_joint_p.hasConfigurationLimitInTangent();
571
4
  std::vector<bool> expected_cf_limits_prismatic({true});
572
4
  std::vector<bool> expected_cf_limits_tangent_prismatic({true});
573



2
  BOOST_CHECK(cf_limits_prismatic == expected_cf_limits_prismatic);
574



2
  BOOST_CHECK(cf_limits_tangent_prismatic == expected_cf_limits_tangent_prismatic);
575
576
  // Test model.hasConfigurationLimit() function
577
4
  Model model;
578
2
  JointIndex jointId = 0;
579
580


2
  jointId = model.addJoint(jointId, JointModelFreeFlyer(), SE3::Identity(), "Joint0");
581


2
  jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
582



2
  jointId = model.addJoint(jointId, JointModelRUBZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");
583
584
  std::vector<bool> expected_cf_limits_model({true, true, true, // translation of FF
585
                                              false, false, false, false,  // rotation of FF
586
                                              true,  // roational joint
587
4
                                              false, false});  // unbounded rotational joint
588
4
  std::vector<bool> model_cf_limits =  model.hasConfigurationLimit();
589



2
  BOOST_CHECK((model_cf_limits == expected_cf_limits_model));
590
591
// Test model.hasConfigurationLimitInTangent() function
592
  std::vector<bool> expected_cf_limits_tangent_model({true, true, true, // translation of FF
593
                                                      false, false, false, // rotation of FF
594
                                                      true,  // roational joint
595
4
                                                      false });  // unbounded rotational joint
596
4
  std::vector<bool> model_cf_limits_tangent =  model.hasConfigurationLimitInTangent();
597



2
  BOOST_CHECK((model_cf_limits_tangent == expected_cf_limits_tangent_model));
598
2
}
599
600
601
BOOST_AUTO_TEST_SUITE_END()