GCC Code Coverage Report

Directory: ./
File: unittest/serialization.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 294 0.0%
Branches: 0 1964 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2023 INRIA
3 //
4
5 #include "pinocchio/multibody/data.hpp"
6 #include "pinocchio/algorithm/joint-configuration.hpp"
7 #include "pinocchio/algorithm/kinematics.hpp"
8 #include "pinocchio/algorithm/geometry.hpp"
9
10 #include "pinocchio/serialization/fwd.hpp"
11 #include "pinocchio/serialization/archive.hpp"
12
13 #include "pinocchio/serialization/spatial.hpp"
14
15 #include "pinocchio/serialization/frame.hpp"
16
17 #include "pinocchio/serialization/joints.hpp"
18 #include "pinocchio/serialization/model.hpp"
19 #include "pinocchio/serialization/data.hpp"
20
21 #include "pinocchio/serialization/geometry.hpp"
22
23 #include "pinocchio/multibody/sample-models.hpp"
24
25 #include <iostream>
26
27 #include <boost/test/unit_test.hpp>
28 #include <boost/utility/binary.hpp>
29
30 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
31
32 template<typename T1, typename T2 = T1>
33 struct call_equality_op
34 {
35 ✗ static bool run(const T1 & v1, const T2 & v2)
36 {
37 ✗ return v1 == v2;
38 }
39 };
40
41 template<typename T>
42 ✗ bool run_call_equality_op(const T & v1, const T & v2)
43 {
44 ✗ return call_equality_op<T, T>::run(v1, v2);
45 }
46
47 // Bug fix in Eigen::Tensor
48 #ifdef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
49 template<typename Scalar, int NumIndices, int Options, typename IndexType>
50 struct call_equality_op<pinocchio::Tensor<Scalar, NumIndices, Options, IndexType>>
51 {
52 typedef pinocchio::Tensor<Scalar, NumIndices, Options, IndexType> T;
53
54 ✗ static bool run(const T & v1, const T & v2)
55 {
56 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXd;
57 ✗ Eigen::Map<const VectorXd> map1(v1.data(), v1.size(), 1);
58 ✗ Eigen::Map<const VectorXd> map2(v2.data(), v2.size(), 1);
59 ✗ return map1 == map2;
60 }
61 };
62 #endif
63
64 template<typename T>
65 struct empty_contructor_algo
66 {
67 ✗ static T * run()
68 {
69 ✗ return new T();
70 }
71 };
72
73 template<>
74 struct empty_contructor_algo<pinocchio::GeometryObject>
75 {
76 ✗ static pinocchio::GeometryObject * run()
77 {
78 ✗ return new pinocchio::GeometryObject("", 0, 0, pinocchio::SE3::Identity(), nullptr);
79 }
80 };
81
82 template<typename T>
83 ✗ T * empty_contructor()
84 {
85 ✗ return empty_contructor_algo<T>::run();
86 }
87
88 template<typename T>
89 ✗ void generic_test(const T & object, const std::string & filename, const std::string & tag_name)
90 {
91 using namespace pinocchio::serialization;
92
93 // Load and save as TXT
94 ✗ const std::string txt_filename = filename + ".txt";
95 ✗ saveToText(object, txt_filename);
96
97 {
98 ✗ T & object_loaded = *empty_contructor<T>();
99 ✗ loadFromText(object_loaded, txt_filename);
100
101 // Check
102 ✗ BOOST_CHECK(run_call_equality_op(object_loaded, object));
103
104 ✗ delete &object_loaded;
105 }
106
107 // Load and save as string stream (TXT format)
108 ✗ std::stringstream ss_out;
109 ✗ saveToStringStream(object, ss_out);
110
111 {
112 ✗ T & object_loaded = *empty_contructor<T>();
113 ✗ std::istringstream is(ss_out.str());
114 ✗ loadFromStringStream(object_loaded, is);
115
116 // Check
117 ✗ BOOST_CHECK(run_call_equality_op(object_loaded, object));
118
119 ✗ delete &object_loaded;
120 }
121
122 // Load and save as string
123 ✗ std::string str_out = saveToString(object);
124
125 {
126 ✗ T & object_loaded = *empty_contructor<T>();
127 ✗ std::string str_in(str_out);
128 ✗ loadFromString(object_loaded, str_in);
129
130 // Check
131 ✗ BOOST_CHECK(run_call_equality_op(object_loaded, object));
132
133 ✗ delete &object_loaded;
134 }
135
136 // Load and save as XML
137 ✗ const std::string xml_filename = filename + ".xml";
138 ✗ saveToXML(object, xml_filename, tag_name);
139
140 {
141 ✗ T & object_loaded = *empty_contructor<T>();
142 ✗ loadFromXML(object_loaded, xml_filename, tag_name);
143
144 // Check
145 ✗ BOOST_CHECK(run_call_equality_op(object_loaded, object));
146
147 ✗ delete &object_loaded;
148 }
149
150 // Load and save as binary
151 ✗ const std::string bin_filename = filename + ".bin";
152 ✗ saveToBinary(object, bin_filename);
153
154 {
155 ✗ T & object_loaded = *empty_contructor<T>();
156 ✗ loadFromBinary(object_loaded, bin_filename);
157
158 // Check
159 ✗ BOOST_CHECK(run_call_equality_op(object_loaded, object));
160
161 ✗ delete &object_loaded;
162 }
163
164 // Load and save as binary stream
165 ✗ boost::asio::streambuf buffer;
166 ✗ saveToBinary(object, buffer);
167
168 {
169 ✗ T & object_loaded = *empty_contructor<T>();
170 ✗ loadFromBinary(object_loaded, buffer);
171
172 // Check
173 ✗ BOOST_CHECK(run_call_equality_op(object_loaded, object));
174
175 ✗ delete &object_loaded;
176 }
177
178 // Load and save as static binary stream
179 ✗ pinocchio::serialization::StaticBuffer static_buffer(100000000);
180 ✗ saveToBinary(object, static_buffer);
181
182 {
183 ✗ T & object_loaded = *empty_contructor<T>();
184 ✗ loadFromBinary(object_loaded, static_buffer);
185
186 // Check
187 ✗ BOOST_CHECK(run_call_equality_op(object_loaded, object));
188
189 ✗ delete &object_loaded;
190 }
191 }
192
193 ✗ BOOST_AUTO_TEST_CASE(test_static_buffer)
194 {
195 using namespace pinocchio::serialization;
196 ✗ const size_t size = 10000000;
197 ✗ StaticBuffer static_buffer(size);
198 ✗ BOOST_CHECK(size == static_buffer.size());
199
200 ✗ const size_t new_size = 2 * size;
201 ✗ static_buffer.resize(new_size);
202 ✗ BOOST_CHECK(new_size == static_buffer.size());
203
204 ✗ BOOST_CHECK(static_buffer.data() != NULL);
205 ✗ BOOST_CHECK(reinterpret_cast<const StaticBuffer &>(static_buffer).data() != NULL);
206 ✗ BOOST_CHECK(reinterpret_cast<const StaticBuffer &>(static_buffer).data() == static_buffer.data());
207 }
208
209 ✗ BOOST_AUTO_TEST_CASE(test_eigen_serialization)
210 {
211 using namespace pinocchio;
212
213 ✗ const Eigen::DenseIndex num_cols = 10;
214 ✗ const Eigen::DenseIndex num_rows = 20;
215
216 ✗ const Eigen::DenseIndex array_size = 3;
217
218 ✗ Eigen::MatrixXd Mat = Eigen::MatrixXd::Random(num_rows, num_cols);
219 ✗ generic_test(Mat, TEST_SERIALIZATION_FOLDER "/eigen_matrix", "matrix");
220
221 ✗ Eigen::VectorXd Vec = Eigen::VectorXd::Random(num_rows * num_cols);
222 ✗ generic_test(Vec, TEST_SERIALIZATION_FOLDER "/eigen_vector", "vector");
223
224 ✗ Eigen::array<Eigen::DenseIndex, array_size> array = {1, 2, 3};
225 ✗ generic_test(array, TEST_SERIALIZATION_FOLDER "/eigen_array", "array");
226
227 ✗ const Eigen::DenseIndex tensor_size = 3;
228 ✗ const Eigen::DenseIndex x_dim = 10, y_dim = 20, z_dim = 30;
229
230 typedef pinocchio::Tensor<double, tensor_size> Tensor3x;
231 ✗ Tensor3x tensor(x_dim, y_dim, z_dim);
232
233 ✗ Eigen::Map<Eigen::VectorXd>(tensor.data(), tensor.size(), 1).setRandom();
234
235 ✗ generic_test(tensor, TEST_SERIALIZATION_FOLDER "/eigen_tensor", "tensor");
236 }
237
238 ✗ BOOST_AUTO_TEST_CASE(test_spatial_serialization)
239 {
240 using namespace pinocchio;
241
242 ✗ SE3 M(SE3::Random());
243 ✗ generic_test(M, TEST_SERIALIZATION_FOLDER "/SE3", "SE3");
244
245 ✗ Motion m(Motion::Random());
246 ✗ generic_test(m, TEST_SERIALIZATION_FOLDER "/Motion", "Motion");
247
248 ✗ Force f(Force::Random());
249 ✗ generic_test(f, TEST_SERIALIZATION_FOLDER "/Force", "Force");
250
251 ✗ Symmetric3 S(Symmetric3::Random());
252 ✗ generic_test(S, TEST_SERIALIZATION_FOLDER "/Symmetric3", "Symmetric3");
253
254 ✗ Inertia I(Inertia::Random());
255 ✗ generic_test(I, TEST_SERIALIZATION_FOLDER "/Inertia", "Inertia");
256 }
257
258 ✗ BOOST_AUTO_TEST_CASE(test_multibody_serialization)
259 {
260 using namespace pinocchio;
261
262 ✗ Frame frame("frame", 0, 0, SE3::Random(), SENSOR);
263 ✗ generic_test(frame, TEST_SERIALIZATION_FOLDER "/Frame", "Frame");
264 }
265
266 template<typename JointModel_>
267 struct init;
268
269 template<typename JointModel_>
270 struct init
271 {
272 ✗ static JointModel_ run()
273 {
274 ✗ JointModel_ jmodel;
275 ✗ jmodel.setIndexes(0, 0, 0);
276 ✗ 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 ✗ static JointModel run()
286 {
287 typedef typename JointModel::Vector3 Vector3;
288 ✗ JointModel jmodel(Vector3::Random().normalized());
289
290 ✗ jmodel.setIndexes(0, 0, 0);
291 ✗ 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 ✗ static JointModel run()
301 {
302 typedef typename JointModel::Vector3 Vector3;
303 ✗ JointModel jmodel(Vector3::Random().normalized());
304
305 ✗ jmodel.setIndexes(0, 0, 0);
306 ✗ 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 ✗ static JointModel run()
316 {
317 typedef typename JointModel::Vector3 Vector3;
318 ✗ JointModel jmodel(Vector3::Random().normalized());
319
320 ✗ jmodel.setIndexes(0, 0, 0);
321 ✗ return jmodel;
322 }
323 };
324
325 template<typename Scalar, int Options>
326 struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
327 {
328 typedef pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options> JointModel;
329
330 ✗ static JointModel run()
331 {
332 typedef typename JointModel::Vector3 Vector3;
333 ✗ JointModel jmodel(Vector3::Random().normalized());
334
335 ✗ jmodel.setIndexes(0, 0, 0);
336 ✗ return jmodel;
337 }
338 };
339
340 template<typename Scalar, int Options, int axis>
341 struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
342 {
343 typedef pinocchio::JointModelHelicalTpl<Scalar, Options, axis> JointModel;
344
345 ✗ static JointModel run()
346 {
347 ✗ JointModel jmodel(static_cast<Scalar>(0.0));
348
349 ✗ jmodel.setIndexes(0, 0, 0);
350 ✗ return jmodel;
351 }
352 };
353
354 template<typename Scalar, int Options, template<typename, int> class JointCollection>
355 struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
356 {
357 typedef pinocchio::JointModelTpl<Scalar, Options, JointCollection> JointModel;
358
359 static JointModel run()
360 {
361 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
362 JointModel jmodel((JointModelRX()));
363
364 jmodel.setIndexes(0, 0, 0);
365 return jmodel;
366 }
367 };
368
369 template<typename Scalar, int Options>
370 struct init<pinocchio::JointModelUniversalTpl<Scalar, Options>>
371 {
372 typedef pinocchio::JointModelUniversalTpl<Scalar, Options> JointModel;
373
374 ✗ static JointModel run()
375 {
376 ✗ JointModel jmodel(pinocchio::XAxis::vector(), pinocchio::YAxis::vector());
377
378 ✗ jmodel.setIndexes(0, 0, 0);
379 ✗ return jmodel;
380 }
381 };
382
383 template<typename Scalar, int Options, template<typename, int> class JointCollection>
384 struct init<pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection>>
385 {
386 typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection> JointModel;
387
388 ✗ static JointModel run()
389 {
390 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
391 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 1> JointModelRY;
392 ✗ JointModel jmodel((JointModelRX()));
393 ✗ jmodel.addJoint(JointModelRY());
394
395 ✗ jmodel.setIndexes(0, 0, 0);
396 ✗ return jmodel;
397 }
398 };
399
400 template<typename JointModel_>
401 struct init<pinocchio::JointModelMimic<JointModel_>>
402 {
403 typedef pinocchio::JointModelMimic<JointModel_> JointModel;
404
405 ✗ static JointModel run()
406 {
407 ✗ JointModel_ jmodel_ref = init<JointModel_>::run();
408
409 ✗ JointModel jmodel(jmodel_ref, 1., 0.);
410
411 ✗ return jmodel;
412 }
413 };
414
415 struct TestJointModel
416 {
417 template<typename JointModel>
418 ✗ void operator()(const pinocchio::JointModelBase<JointModel> &) const
419 {
420 ✗ JointModel jmodel = init<JointModel>::run();
421 ✗ test(jmodel);
422 }
423
424 template<typename JointType>
425 ✗ static void test(JointType & jmodel)
426 {
427 ✗ generic_test(jmodel, TEST_SERIALIZATION_FOLDER "/Joint", "jmodel");
428 }
429 };
430
431 ✗ BOOST_AUTO_TEST_CASE(test_multibody_joints_model_serialization)
432 {
433 using namespace pinocchio;
434 ✗ boost::mpl::for_each<JointModelVariant::types>(TestJointModel());
435 }
436
437 struct TestJointTransform
438 {
439 template<typename JointModel>
440 ✗ void operator()(const pinocchio::JointModelBase<JointModel> &) const
441 {
442 typedef typename JointModel::JointDerived JointDerived;
443 typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform;
444 typedef typename pinocchio::traits<JointDerived>::Constraint_t Constraint;
445 typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData;
446 typedef pinocchio::JointDataBase<JointData> JointDataBase;
447 ✗ JointModel jmodel = init<JointModel>::run();
448
449 ✗ JointData jdata = jmodel.createData();
450 ✗ JointDataBase & jdata_base = static_cast<JointDataBase &>(jdata);
451
452 typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
453 ✗ LieGroupType lg;
454
455 ✗ Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.));
456 ✗ Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
457
458 ✗ Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub);
459
460 ✗ jmodel.calc(jdata, q_random);
461 ✗ Transform & m = jdata_base.M();
462 ✗ test(m);
463
464 ✗ Constraint & S = jdata_base.S();
465 ✗ test(S);
466 }
467
468 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
469 ✗ void operator()(const pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> &)
470 {
471 // Do nothing
472 }
473
474 template<typename JointModel>
475 ✗ void operator()(const pinocchio::JointModelMimic<JointModel> &)
476 {
477 typedef pinocchio::JointModelMimic<JointModel> JointModelMimic;
478 typedef typename JointModelMimic::JointDerived JointDerived;
479 typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform;
480 typedef typename pinocchio::traits<JointDerived>::Constraint_t Constraint;
481 typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointDataMimic;
482 typedef pinocchio::JointDataBase<JointDataMimic> JointDataBase;
483 ✗ JointModelMimic jmodel_mimic = init<JointModelMimic>::run();
484 ✗ JointModel jmodel = init<JointModel>::run();
485
486 ✗ JointDataMimic jdata_mimic = jmodel_mimic.createData();
487 ✗ JointDataBase & jdata_mimic_base = static_cast<JointDataBase &>(jdata_mimic);
488
489 typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
490 ✗ LieGroupType lg;
491
492 ✗ Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.));
493 ✗ Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
494
495 ✗ Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub);
496
497 ✗ jmodel_mimic.calc(jdata_mimic, q_random);
498 ✗ Transform & m = jdata_mimic_base.M();
499 ✗ test(m);
500
501 ✗ Constraint & S = jdata_mimic_base.S();
502 ✗ test(S);
503 }
504
505 template<typename Transform>
506 ✗ static void test(Transform & m)
507 {
508 ✗ generic_test(m, TEST_SERIALIZATION_FOLDER "/JointTransform", "transform");
509 }
510 };
511
512 ✗ BOOST_AUTO_TEST_CASE(test_multibody_joints_transform_serialization)
513 {
514 using namespace pinocchio;
515 ✗ boost::mpl::for_each<JointModelVariant::types>(TestJointTransform());
516 }
517
518 struct TestJointMotion
519 {
520 template<typename JointModel>
521 ✗ void operator()(const pinocchio::JointModelBase<JointModel> &) const
522 {
523 typedef typename JointModel::JointDerived JointDerived;
524 typedef typename pinocchio::traits<JointDerived>::Motion_t Motion;
525 typedef typename pinocchio::traits<JointDerived>::Bias_t Bias;
526 typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData;
527 typedef pinocchio::JointDataBase<JointData> JointDataBase;
528 ✗ JointModel jmodel = init<JointModel>::run();
529
530 ✗ JointData jdata = jmodel.createData();
531 ✗ JointDataBase & jdata_base = static_cast<JointDataBase &>(jdata);
532
533 typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
534 ✗ LieGroupType lg;
535
536 ✗ Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.));
537 ✗ Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
538
539 ✗ Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub);
540 ✗ Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv());
541
542 ✗ jmodel.calc(jdata, q_random, v_random);
543 ✗ Motion & m = jdata_base.v();
544
545 ✗ test(m);
546
547 ✗ Bias & b = jdata_base.c();
548 ✗ test(b);
549 }
550
551 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
552 ✗ void operator()(const pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> &)
553 {
554 // Do nothing
555 }
556
557 template<typename JointModel>
558 ✗ void operator()(const pinocchio::JointModelMimic<JointModel> &)
559 {
560 typedef pinocchio::JointModelMimic<JointModel> JointModelMimic;
561 typedef typename JointModelMimic::JointDerived JointDerived;
562 typedef typename pinocchio::traits<JointDerived>::Motion_t Motion;
563 typedef typename pinocchio::traits<JointDerived>::Bias_t Bias;
564 typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointDataMimic;
565 typedef pinocchio::JointDataBase<JointDataMimic> JointDataBase;
566 ✗ JointModelMimic jmodel_mimic = init<JointModelMimic>::run();
567 ✗ JointModel jmodel = init<JointModel>::run();
568
569 ✗ JointDataMimic jdata_mimic = jmodel_mimic.createData();
570 ✗ JointDataBase & jdata_mimic_base = static_cast<JointDataBase &>(jdata_mimic);
571
572 typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
573 ✗ LieGroupType lg;
574
575 ✗ Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.));
576 ✗ Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
577
578 ✗ Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub);
579 ✗ Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv());
580
581 ✗ jmodel_mimic.calc(jdata_mimic, q_random, v_random);
582 ✗ Motion & m = jdata_mimic_base.v();
583
584 ✗ test(m);
585
586 ✗ Bias & b = jdata_mimic_base.c();
587 ✗ test(b);
588 }
589
590 template<typename Motion>
591 ✗ static void test(Motion & m)
592 {
593 ✗ generic_test(m, TEST_SERIALIZATION_FOLDER "/JointMotion", "motion");
594 }
595 };
596
597 ✗ BOOST_AUTO_TEST_CASE(test_multibody_joints_motion_serialization)
598 {
599 using namespace pinocchio;
600 ✗ boost::mpl::for_each<JointModelVariant::types>(TestJointMotion());
601 }
602
603 struct TestJointData
604 {
605 template<typename JointModel>
606 ✗ void operator()(const pinocchio::JointModelBase<JointModel> &) const
607 {
608 typedef typename JointModel::JointDerived JointDerived;
609 typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData;
610 ✗ JointModel jmodel = init<JointModel>::run();
611
612 ✗ JointData jdata = jmodel.createData();
613
614 typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
615 ✗ LieGroupType lg;
616
617 ✗ Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.));
618 ✗ Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
619
620 ✗ Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub);
621 ✗ Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv());
622
623 ✗ jmodel.calc(jdata, q_random, v_random);
624 ✗ pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
625 ✗ jmodel.calc_aba(jdata, Eigen::VectorXd::Zero(jmodel.nv()), I, false);
626 ✗ test(jdata);
627 }
628
629 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
630 ✗ void operator()(const pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> &)
631 {
632 typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> JointModel;
633 typedef typename JointModel::JointDerived JointDerived;
634 typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData;
635
636 ✗ JointModel jmodel_build = init<JointModel>::run();
637
638 ✗ pinocchio::Model model;
639 ✗ model.addJoint(0, jmodel_build, pinocchio::SE3::Random(), "model");
640 ✗ model.lowerPositionLimit.fill(-1.);
641 ✗ model.upperPositionLimit.fill(1.);
642
643 ✗ JointModel & jmodel = boost::get<JointModel>(model.joints[1]);
644 ✗ Eigen::VectorXd q_random = pinocchio::randomConfiguration(model);
645 ✗ Eigen::VectorXd v_random = Eigen::VectorXd::Random(model.nv);
646
647 ✗ pinocchio::Data data(model);
648 ✗ JointData & jdata = boost::get<JointData>(data.joints[1]);
649
650 ✗ jmodel.calc(jdata, q_random, v_random);
651 ✗ pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
652 ✗ jmodel.calc_aba(jdata, Eigen::VectorXd::Zero(jmodel.nv()), I, false);
653
654 ✗ test(jdata);
655 }
656
657 template<typename JointModel>
658 ✗ void operator()(const pinocchio::JointModelMimic<JointModel> &)
659 {
660 typedef pinocchio::JointModelMimic<JointModel> JointModelMimic;
661 typedef typename JointModelMimic::JointDerived JointDerived;
662 typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointDataMimic;
663 ✗ JointModelMimic jmodel_mimic = init<JointModelMimic>::run();
664 ✗ JointModel jmodel = init<JointModel>::run();
665
666 ✗ JointDataMimic jdata_mimic = jmodel_mimic.createData();
667
668 typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
669 ✗ LieGroupType lg;
670
671 ✗ Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), -1.));
672 ✗ Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.));
673
674 ✗ Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub);
675 ✗ Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv());
676
677 ✗ jmodel_mimic.calc(jdata_mimic, q_random, v_random);
678 ✗ pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity());
679 ✗ jmodel_mimic.calc_aba(jdata_mimic, Eigen::VectorXd::Zero(jmodel.nv()), I, false);
680 ✗ test(jdata_mimic);
681 }
682
683 template<typename JointData>
684 ✗ static void test(JointData & joint_data)
685 {
686 ✗ generic_test(joint_data, TEST_SERIALIZATION_FOLDER "/JointData", "data");
687 }
688 };
689
690 ✗ BOOST_AUTO_TEST_CASE(test_multibody_joints_data_serialization)
691 {
692 using namespace pinocchio;
693 ✗ boost::mpl::for_each<JointModelVariant::types>(TestJointData());
694 }
695
696 ✗ BOOST_AUTO_TEST_CASE(test_model_serialization)
697 {
698 using namespace pinocchio;
699
700 ✗ Model model;
701 ✗ buildModels::humanoidRandom(model);
702
703 ✗ generic_test(model, TEST_SERIALIZATION_FOLDER "/Model", "Model");
704 }
705
706 ✗ BOOST_AUTO_TEST_CASE(test_throw_extension)
707 {
708 using namespace pinocchio;
709
710 ✗ Model model;
711 ✗ buildModels::humanoidRandom(model);
712
713 ✗ const std::string & fake_filename = "this_is_a_fake_filename";
714
715 {
716 ✗ const std::string complete_filename = fake_filename + ".txt";
717 ✗ BOOST_REQUIRE_THROW(loadFromText(model, complete_filename), std::invalid_argument);
718 }
719
720 ✗ saveToText(model, TEST_SERIALIZATION_FOLDER "/model.txt");
721 ✗ saveToXML(model, TEST_SERIALIZATION_FOLDER "/model.xml", "model");
722 ✗ saveToBinary(model, TEST_SERIALIZATION_FOLDER "/model.bin");
723
724 {
725 ✗ const std::string complete_filename = fake_filename + ".txte";
726
727 ✗ BOOST_REQUIRE_THROW(loadFromText(model, complete_filename), std::invalid_argument);
728 }
729
730 {
731 ✗ const std::string complete_filename = fake_filename + ".xmle";
732 ✗ BOOST_REQUIRE_THROW(loadFromXML(model, complete_filename, "model"), std::invalid_argument);
733 }
734
735 {
736 ✗ const std::string complete_filename = fake_filename + ".bine";
737 ✗ BOOST_REQUIRE_THROW(loadFromBinary(model, complete_filename), std::invalid_argument);
738 }
739 }
740
741 ✗ BOOST_AUTO_TEST_CASE(test_data_serialization)
742 {
743 using namespace pinocchio;
744
745 ✗ Model model;
746 ✗ buildModels::humanoidRandom(model);
747
748 ✗ Data data(model);
749
750 ✗ generic_test(data, TEST_SERIALIZATION_FOLDER "/Data", "Data");
751 }
752
753 ✗ BOOST_AUTO_TEST_CASE(test_collision_pair)
754 {
755 using namespace pinocchio;
756
757 ✗ CollisionPair collision_pair(1, 2);
758 ✗ generic_test(collision_pair, TEST_SERIALIZATION_FOLDER "/CollisionPair", "CollisionPair");
759 }
760
761 ✗ BOOST_AUTO_TEST_CASE(test_model_item)
762 {
763 using namespace pinocchio;
764
765 typedef GeometryObject::Base GeometryObject_ModelItem;
766 ✗ GeometryObject_ModelItem model_item("pinocchio", 1, 2, SE3::Random());
767 ✗ generic_test(model_item, TEST_SERIALIZATION_FOLDER "/ModelItem", "ModelItem");
768 }
769
770 ✗ BOOST_AUTO_TEST_CASE(test_geometry_object)
771 {
772 using namespace pinocchio;
773
774 {
775 ✗ GeometryObject geometry_object("nullptr", 1, 2, SE3::Random(), nullptr);
776 ✗ generic_test(geometry_object, TEST_SERIALIZATION_FOLDER "/GeometryObject", "GeometryObject");
777 }
778
779 #ifdef PINOCCHIO_WITH_HPP_FCL
780 {
781 ✗ hpp::fcl::Box box(1., 2., 3.);
782 ✗ generic_test(box, TEST_SERIALIZATION_FOLDER "/Box", "Box");
783 }
784
785 #if HPP_FCL_VERSION_AT_LEAST(3, 0, 0)
786 {
787 typedef GeometryObject::CollisionGeometryPtr CollisionGeometryPtr;
788 CollisionGeometryPtr box_ptr = CollisionGeometryPtr(new hpp::fcl::Box(1., 2., 3.));
789 GeometryObject geometry_object("box", 1, 2, SE3::Random(), box_ptr);
790 generic_test(geometry_object, TEST_SERIALIZATION_FOLDER "/GeometryObject", "GeometryObject");
791 }
792 #endif // hpp-fcl >= 3.0.0
793 #endif
794 }
795
796 ✗ BOOST_AUTO_TEST_CASE(test_geometry_model_and_data_serialization)
797 {
798 using namespace pinocchio;
799
800 ✗ Model model;
801 ✗ buildModels::humanoid(model);
802 ✗ Data data(model);
803
804 // Empty structures
805 {
806 ✗ GeometryModel geom_model;
807 ✗ generic_test(geom_model, TEST_SERIALIZATION_FOLDER "/GeometryModel", "GeometryModel");
808
809 ✗ GeometryData geom_data(geom_model);
810 ✗ generic_test(geom_data, TEST_SERIALIZATION_FOLDER "/GeometryData", "GeometryData");
811 }
812
813 #ifdef PINOCCHIO_WITH_HPP_FCL
814 #if HPP_FCL_VERSION_AT_LEAST(3, 0, 0)
815 {
816 pinocchio::GeometryModel geom_model;
817 pinocchio::buildModels::humanoidGeometries(model, geom_model);
818 // Append new objects
819 {
820 using namespace hpp::fcl;
821 BVHModel<OBBRSS> * bvh_ptr = new BVHModel<OBBRSS>();
822 // bvh_ptr->beginModel();
823 // bvh_ptr->addSubModel(p1, t1);
824 // bvh_ptr->endModel();
825
826 GeometryObject obj_bvh(
827 "bvh", 0, 0, SE3::Identity(), GeometryObject::CollisionGeometryPtr(bvh_ptr));
828 geom_model.addGeometryObject(obj_bvh);
829
830 const double min_altitude = -1.;
831 const double x_dim = 1., y_dim = 2.;
832 const Eigen::DenseIndex nx = 100, ny = 200;
833 const Eigen::MatrixXd heights = Eigen::MatrixXd::Random(ny, nx);
834
835 HeightField<OBBRSS> * hfield_ptr =
836 new HeightField<OBBRSS>(x_dim, y_dim, heights, min_altitude);
837
838 GeometryObject obj_hfield(
839 "hfield", 0, 0, SE3::Identity(), GeometryObject::CollisionGeometryPtr(hfield_ptr));
840 geom_model.addGeometryObject(obj_hfield);
841 }
842 generic_test(geom_model, TEST_SERIALIZATION_FOLDER "/GeometryModel", "GeometryModel");
843
844 pinocchio::GeometryData geom_data(geom_model);
845 const Eigen::VectorXd q = pinocchio::neutral(model);
846 pinocchio::forwardKinematics(model, data, q);
847 pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
848
849 generic_test(geom_data, TEST_SERIALIZATION_FOLDER "/GeometryData", "GeometryData");
850 }
851 #endif // hpp-fcl >= 3.0.0
852 #endif // PINOCCHIO_WITH_HPP_FCL
853 }
854
855 BOOST_AUTO_TEST_SUITE_END()
856