GCC Code Coverage Report


Directory: ./
File: unittest/serialization.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 357 357 100.0%
Branches: 1013 2074 48.8%

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