| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2019-2021 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/parsers/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 |
1876 |
static bool run(const T1 & v1, const T2 & v2) |
|
36 |
{ |
||
37 |
1876 |
return v1 == v2; |
|
38 |
} |
||
39 |
}; |
||
40 |
|||
41 |
template<typename T> |
||
42 |
1890 |
bool run_call_equality_op(const T & v1, const T & v2) |
|
43 |
{ |
||
44 |
1890 |
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 |
✓✗✓✗ ✓✗✓✗ |
7 |
Eigen::Map<const VectorXd> map1(v1.data(),v1.size(),1); |
58 |
✓✗✓✗ ✓✗✓✗ |
7 |
Eigen::Map<const VectorXd> map2(v2.data(),v2.size(),1); |
59 |
✓✗ | 14 |
return map1 == map2; |
60 |
} |
||
61 |
}; |
||
62 |
#endif |
||
63 |
|||
64 |
template<typename T> |
||
65 |
270 |
void generic_test(const T & object, |
|
66 |
const std::string & filename, |
||
67 |
const std::string & tag_name) |
||
68 |
{ |
||
69 |
using namespace pinocchio::serialization; |
||
70 |
|||
71 |
// Load and save as TXT |
||
72 |
✓✗ | 540 |
const std::string txt_filename = filename + ".txt"; |
73 |
✓✗ | 270 |
saveToText(object,txt_filename); |
74 |
|||
75 |
{ |
||
76 |
✓✗ | 246 |
T object_loaded; |
77 |
✓✗ | 270 |
loadFromText(object_loaded,txt_filename); |
78 |
|||
79 |
// Check |
||
80 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
270 |
BOOST_CHECK(run_call_equality_op(object_loaded,object)); |
81 |
} |
||
82 |
|||
83 |
// Load and save as string stream (TXT format) |
||
84 |
✓✗ | 540 |
std::stringstream ss_out; |
85 |
✓✗ | 270 |
saveToStringStream(object,ss_out); |
86 |
|||
87 |
{ |
||
88 |
✓✗ | 246 |
T object_loaded; |
89 |
✓✗✓✗ |
540 |
std::istringstream is(ss_out.str()); |
90 |
✓✗ | 270 |
loadFromStringStream(object_loaded,is); |
91 |
|||
92 |
// Check |
||
93 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
270 |
BOOST_CHECK(run_call_equality_op(object_loaded,object)); |
94 |
} |
||
95 |
|||
96 |
// Load and save as string |
||
97 |
✓✗ | 540 |
std::string str_out = saveToString(object); |
98 |
|||
99 |
{ |
||
100 |
✓✗ | 246 |
T object_loaded; |
101 |
✓✗ | 540 |
std::string str_in(str_out); |
102 |
✓✗ | 270 |
loadFromString(object_loaded,str_in); |
103 |
|||
104 |
// Check |
||
105 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
270 |
BOOST_CHECK(run_call_equality_op(object_loaded,object)); |
106 |
} |
||
107 |
|||
108 |
// Load and save as XML |
||
109 |
✓✗ | 540 |
const std::string xml_filename = filename + ".xml"; |
110 |
✓✗ | 270 |
saveToXML(object,xml_filename,tag_name); |
111 |
|||
112 |
{ |
||
113 |
✓✗ | 246 |
T object_loaded; |
114 |
✓✗ | 270 |
loadFromXML(object_loaded,xml_filename,tag_name); |
115 |
|||
116 |
// Check |
||
117 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
270 |
BOOST_CHECK(run_call_equality_op(object_loaded,object)); |
118 |
} |
||
119 |
|||
120 |
// Load and save as binary |
||
121 |
✓✗ | 540 |
const std::string bin_filename = filename + ".bin"; |
122 |
✓✗ | 270 |
saveToBinary(object,bin_filename); |
123 |
|||
124 |
{ |
||
125 |
✓✗ | 246 |
T object_loaded; |
126 |
✓✗ | 270 |
loadFromBinary(object_loaded,bin_filename); |
127 |
|||
128 |
// Check |
||
129 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
270 |
BOOST_CHECK(run_call_equality_op(object_loaded,object)); |
130 |
} |
||
131 |
|||
132 |
// Load and save as binary stream |
||
133 |
✓✗ | 540 |
boost::asio::streambuf buffer; |
134 |
✓✗ | 270 |
saveToBinary(object,buffer); |
135 |
|||
136 |
{ |
||
137 |
✓✗ | 246 |
T object_loaded; |
138 |
✓✗ | 270 |
loadFromBinary(object_loaded,buffer); |
139 |
|||
140 |
// Check |
||
141 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
270 |
BOOST_CHECK(run_call_equality_op(object_loaded,object)); |
142 |
} |
||
143 |
|||
144 |
// Load and save as static binary stream |
||
145 |
✓✗ | 540 |
pinocchio::serialization::StaticBuffer static_buffer(10000000); |
146 |
✓✗ | 270 |
saveToBinary(object,static_buffer); |
147 |
|||
148 |
{ |
||
149 |
✓✗ | 246 |
T object_loaded; |
150 |
✓✗ | 270 |
loadFromBinary(object_loaded,static_buffer); |
151 |
|||
152 |
// Check |
||
153 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
270 |
BOOST_CHECK(run_call_equality_op(object_loaded,object)); |
154 |
} |
||
155 |
270 |
} |
|
156 |
|||
157 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_static_buffer) |
158 |
{ |
||
159 |
using namespace pinocchio::serialization; |
||
160 |
2 |
const size_t size = 10000000; |
|
161 |
✓✗ | 4 |
StaticBuffer static_buffer(size); |
162 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(size == static_buffer.size()); |
163 |
|||
164 |
2 |
const size_t new_size = 2*size; |
|
165 |
✓✗ | 2 |
static_buffer.resize(new_size); |
166 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(new_size == static_buffer.size()); |
167 |
|||
168 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(static_buffer.data() != NULL); |
169 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reinterpret_cast<const StaticBuffer &>(static_buffer).data() != NULL); |
170 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(reinterpret_cast<const StaticBuffer &>(static_buffer).data() == static_buffer.data()); |
171 |
2 |
} |
|
172 |
|||
173 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_eigen_serialization) |
174 |
{ |
||
175 |
using namespace pinocchio; |
||
176 |
|||
177 |
2 |
const Eigen::DenseIndex num_cols = 10; |
|
178 |
2 |
const Eigen::DenseIndex num_rows = 20; |
|
179 |
|||
180 |
2 |
const Eigen::DenseIndex array_size = 3; |
|
181 |
|||
182 |
✓✗✓✗ |
4 |
Eigen::MatrixXd Mat = Eigen::MatrixXd::Random(num_rows,num_cols); |
183 |
✓✗✓✗ ✓✗ |
2 |
generic_test(Mat,TEST_SERIALIZATION_FOLDER"/eigen_matrix","matrix"); |
184 |
|||
185 |
✓✗✓✗ |
4 |
Eigen::VectorXd Vec = Eigen::VectorXd::Random(num_rows*num_cols); |
186 |
✓✗✓✗ ✓✗ |
2 |
generic_test(Vec,TEST_SERIALIZATION_FOLDER"/eigen_vector","vector"); |
187 |
|||
188 |
2 |
Eigen::array<Eigen::DenseIndex,array_size> array = { 1, 2, 3 }; |
|
189 |
✓✗✓✗ ✓✗ |
2 |
generic_test(array,TEST_SERIALIZATION_FOLDER"/eigen_array","array"); |
190 |
|||
191 |
2 |
const Eigen::DenseIndex tensor_size = 3; |
|
192 |
2 |
const Eigen::DenseIndex x_dim = 10, y_dim = 20, z_dim = 30; |
|
193 |
|||
194 |
typedef pinocchio::Tensor<double,tensor_size> Tensor3x; |
||
195 |
✓✗ | 2 |
Tensor3x tensor(x_dim,y_dim,z_dim); |
196 |
|||
197 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
Eigen::Map<Eigen::VectorXd>(tensor.data(),tensor.size(),1).setRandom(); |
198 |
|||
199 |
✓✗✓✗ ✓✗ |
2 |
generic_test(tensor,TEST_SERIALIZATION_FOLDER"/eigen_tensor","tensor"); |
200 |
2 |
} |
|
201 |
|||
202 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_spatial_serialization) |
203 |
{ |
||
204 |
using namespace pinocchio; |
||
205 |
|||
206 |
✓✗ | 2 |
SE3 M(SE3::Random()); |
207 |
✓✗✓✗ ✓✗ |
2 |
generic_test(M,TEST_SERIALIZATION_FOLDER"/SE3","SE3"); |
208 |
|||
209 |
✓✗ | 2 |
Motion m(Motion::Random()); |
210 |
✓✗✓✗ ✓✗ |
2 |
generic_test(m,TEST_SERIALIZATION_FOLDER"/Motion","Motion"); |
211 |
|||
212 |
✓✗ | 2 |
Force f(Force::Random()); |
213 |
✓✗✓✗ ✓✗ |
2 |
generic_test(f,TEST_SERIALIZATION_FOLDER"/Force","Force"); |
214 |
|||
215 |
✓✗ | 2 |
Symmetric3 S(Symmetric3::Random()); |
216 |
✓✗✓✗ ✓✗ |
2 |
generic_test(S,TEST_SERIALIZATION_FOLDER"/Symmetric3","Symmetric3"); |
217 |
|||
218 |
✓✗ | 2 |
Inertia I(Inertia::Random()); |
219 |
✓✗✓✗ ✓✗ |
2 |
generic_test(I,TEST_SERIALIZATION_FOLDER"/Inertia","Inertia"); |
220 |
2 |
} |
|
221 |
|||
222 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_multibody_serialization) |
223 |
{ |
||
224 |
using namespace pinocchio; |
||
225 |
|||
226 |
✓✗✓✗ ✓✗✓✗ |
4 |
Frame frame("frame",0,0,SE3::Random(),SENSOR); |
227 |
✓✗✓✗ ✓✗ |
2 |
generic_test(frame,TEST_SERIALIZATION_FOLDER"/Frame","Frame"); |
228 |
2 |
} |
|
229 |
|||
230 |
template<typename JointModel_> struct init; |
||
231 |
|||
232 |
template<typename JointModel_> |
||
233 |
struct init |
||
234 |
{ |
||
235 |
154 |
static JointModel_ run() |
|
236 |
{ |
||
237 |
154 |
JointModel_ jmodel; |
|
238 |
154 |
jmodel.setIndexes(0,0,0); |
|
239 |
154 |
return jmodel; |
|
240 |
} |
||
241 |
}; |
||
242 |
|||
243 |
template<typename Scalar, int Options> |
||
244 |
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> > |
||
245 |
{ |
||
246 |
typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel; |
||
247 |
|||
248 |
4 |
static JointModel run() |
|
249 |
{ |
||
250 |
typedef typename JointModel::Vector3 Vector3; |
||
251 |
✓✗✓✗ |
4 |
JointModel jmodel(Vector3::Random().normalized()); |
252 |
|||
253 |
4 |
jmodel.setIndexes(0,0,0); |
|
254 |
4 |
return jmodel; |
|
255 |
} |
||
256 |
}; |
||
257 |
|||
258 |
template<typename Scalar, int Options> |
||
259 |
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> > |
||
260 |
{ |
||
261 |
typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel; |
||
262 |
|||
263 |
4 |
static JointModel run() |
|
264 |
{ |
||
265 |
typedef typename JointModel::Vector3 Vector3; |
||
266 |
✓✗✓✗ |
4 |
JointModel jmodel(Vector3::Random().normalized()); |
267 |
|||
268 |
4 |
jmodel.setIndexes(0,0,0); |
|
269 |
4 |
return jmodel; |
|
270 |
} |
||
271 |
}; |
||
272 |
|||
273 |
template<typename Scalar, int Options> |
||
274 |
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> > |
||
275 |
{ |
||
276 |
typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel; |
||
277 |
|||
278 |
4 |
static JointModel run() |
|
279 |
{ |
||
280 |
typedef typename JointModel::Vector3 Vector3; |
||
281 |
✓✗✓✗ |
4 |
JointModel jmodel(Vector3::Random().normalized()); |
282 |
|||
283 |
4 |
jmodel.setIndexes(0,0,0); |
|
284 |
4 |
return jmodel; |
|
285 |
} |
||
286 |
}; |
||
287 |
|||
288 |
template<typename Scalar, int Options, template<typename,int> class JointCollection> |
||
289 |
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> > |
||
290 |
{ |
||
291 |
typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel; |
||
292 |
|||
293 |
static JointModel run() |
||
294 |
{ |
||
295 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX; |
||
296 |
JointModel jmodel((JointModelRX())); |
||
297 |
|||
298 |
jmodel.setIndexes(0,0,0); |
||
299 |
return jmodel; |
||
300 |
} |
||
301 |
}; |
||
302 |
|||
303 |
template<typename Scalar, int Options, template<typename,int> class JointCollection> |
||
304 |
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> > |
||
305 |
{ |
||
306 |
typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel; |
||
307 |
|||
308 |
2 |
static JointModel run() |
|
309 |
{ |
||
310 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX; |
||
311 |
typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY; |
||
312 |
✓✗✓✗ |
2 |
JointModel jmodel((JointModelRX())); |
313 |
✓✗✓✗ ✓✗ |
2 |
jmodel.addJoint(JointModelRY()); |
314 |
|||
315 |
✓✗ | 2 |
jmodel.setIndexes(0,0,0); |
316 |
2 |
return jmodel; |
|
317 |
} |
||
318 |
}; |
||
319 |
|||
320 |
template<typename JointModel_> |
||
321 |
struct init<pinocchio::JointModelMimic<JointModel_> > |
||
322 |
{ |
||
323 |
typedef pinocchio::JointModelMimic<JointModel_> JointModel; |
||
324 |
|||
325 |
24 |
static JointModel run() |
|
326 |
{ |
||
327 |
✓✗ | 24 |
JointModel_ jmodel_ref = init<JointModel_>::run(); |
328 |
|||
329 |
✓✗ | 24 |
JointModel jmodel(jmodel_ref,1.,0.); |
330 |
|||
331 |
48 |
return jmodel; |
|
332 |
} |
||
333 |
}; |
||
334 |
|||
335 |
struct TestJointModel |
||
336 |
{ |
||
337 |
template <typename JointModel> |
||
338 |
42 |
void operator()(const pinocchio::JointModelBase<JointModel> &) const |
|
339 |
{ |
||
340 |
✓✗ | 44 |
JointModel jmodel = init<JointModel>::run(); |
341 |
✓✗ | 42 |
test(jmodel); |
342 |
42 |
} |
|
343 |
|||
344 |
template<typename JointType> |
||
345 |
42 |
static void test(JointType & jmodel) |
|
346 |
{ |
||
347 |
✓✗✓✗ ✓✗ |
42 |
generic_test(jmodel,TEST_SERIALIZATION_FOLDER"/Joint","jmodel"); |
348 |
42 |
} |
|
349 |
|||
350 |
}; |
||
351 |
|||
352 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_multibody_joints_model_serialization) |
353 |
{ |
||
354 |
using namespace pinocchio; |
||
355 |
✓✗ | 2 |
boost::mpl::for_each<JointModelVariant::types>(TestJointModel()); |
356 |
2 |
} |
|
357 |
|||
358 |
struct TestJointTransform |
||
359 |
{ |
||
360 |
template <typename JointModel> |
||
361 |
34 |
void operator()(const pinocchio::JointModelBase<JointModel> &) const |
|
362 |
{ |
||
363 |
typedef typename JointModel::JointDerived JointDerived; |
||
364 |
typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform; |
||
365 |
typedef typename pinocchio::traits<JointDerived>::Constraint_t Constraint; |
||
366 |
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData; |
||
367 |
typedef pinocchio::JointDataBase<JointData> JointDataBase; |
||
368 |
✓✗ | 34 |
JointModel jmodel = init<JointModel>::run(); |
369 |
|||
370 |
✓✗ | 34 |
JointData jdata = jmodel.createData(); |
371 |
34 |
JointDataBase & jdata_base = static_cast<JointDataBase &>(jdata); |
|
372 |
|||
373 |
typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType; |
||
374 |
✓✗ | 34 |
LieGroupType lg; |
375 |
|||
376 |
✓✗✓✗ ✓✗ |
68 |
Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); |
377 |
✓✗✓✗ ✓✗ |
68 |
Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); |
378 |
|||
379 |
✓✗✓✗ |
68 |
Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); |
380 |
|||
381 |
✓✗ | 34 |
jmodel.calc(jdata,q_random); |
382 |
✓✗ | 34 |
Transform & m = jdata_base.M(); |
383 |
✓✗ | 34 |
test(m); |
384 |
|||
385 |
✓✗ | 34 |
Constraint & S = jdata_base.S(); |
386 |
✓✗ | 34 |
test(S); |
387 |
34 |
} |
|
388 |
|||
389 |
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
||
390 |
1 |
void operator()(const pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> &) |
|
391 |
{ |
||
392 |
// Do nothing |
||
393 |
1 |
} |
|
394 |
|||
395 |
template<typename JointModel> |
||
396 |
6 |
void operator()(const pinocchio::JointModelMimic<JointModel> &) |
|
397 |
{ |
||
398 |
typedef pinocchio::JointModelMimic<JointModel> JointModelMimic; |
||
399 |
typedef typename JointModelMimic::JointDerived JointDerived; |
||
400 |
typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform; |
||
401 |
typedef typename pinocchio::traits<JointDerived>::Constraint_t Constraint; |
||
402 |
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointDataMimic; |
||
403 |
typedef pinocchio::JointDataBase<JointDataMimic> JointDataBase; |
||
404 |
✓✗ | 6 |
JointModelMimic jmodel_mimic = init<JointModelMimic>::run(); |
405 |
✓✗ | 6 |
JointModel jmodel = init<JointModel>::run(); |
406 |
|||
407 |
✓✗ | 6 |
JointDataMimic jdata_mimic = jmodel_mimic.createData(); |
408 |
6 |
JointDataBase & jdata_mimic_base = static_cast<JointDataBase &>(jdata_mimic); |
|
409 |
|||
410 |
typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType; |
||
411 |
✓✗ | 6 |
LieGroupType lg; |
412 |
|||
413 |
✓✗✓✗ ✓✗ |
12 |
Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); |
414 |
✓✗✓✗ ✓✗ |
12 |
Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); |
415 |
|||
416 |
✓✗✓✗ |
12 |
Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); |
417 |
|||
418 |
✓✗ | 6 |
jmodel_mimic.calc(jdata_mimic,q_random); |
419 |
✓✗ | 6 |
Transform & m = jdata_mimic_base.M(); |
420 |
✓✗ | 6 |
test(m); |
421 |
|||
422 |
✓✗ | 6 |
Constraint & S = jdata_mimic_base.S(); |
423 |
✓✗ | 6 |
test(S); |
424 |
6 |
} |
|
425 |
|||
426 |
template<typename Transform> |
||
427 |
80 |
static void test(Transform & m) |
|
428 |
{ |
||
429 |
✓✗✓✗ ✓✗ |
80 |
generic_test(m,TEST_SERIALIZATION_FOLDER"/JointTransform","transform"); |
430 |
80 |
} |
|
431 |
|||
432 |
}; |
||
433 |
|||
434 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_multibody_joints_transform_serialization) |
435 |
{ |
||
436 |
using namespace pinocchio; |
||
437 |
✓✗ | 2 |
boost::mpl::for_each<JointModelVariant::types>(TestJointTransform()); |
438 |
2 |
} |
|
439 |
|||
440 |
struct TestJointMotion |
||
441 |
{ |
||
442 |
template <typename JointModel> |
||
443 |
34 |
void operator()(const pinocchio::JointModelBase<JointModel> &) const |
|
444 |
{ |
||
445 |
typedef typename JointModel::JointDerived JointDerived; |
||
446 |
typedef typename pinocchio::traits<JointDerived>::Motion_t Motion; |
||
447 |
typedef typename pinocchio::traits<JointDerived>::Bias_t Bias; |
||
448 |
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData; |
||
449 |
typedef pinocchio::JointDataBase<JointData> JointDataBase; |
||
450 |
✓✗ | 34 |
JointModel jmodel = init<JointModel>::run(); |
451 |
|||
452 |
✓✗ | 34 |
JointData jdata = jmodel.createData(); |
453 |
34 |
JointDataBase & jdata_base = static_cast<JointDataBase &>(jdata); |
|
454 |
|||
455 |
typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType; |
||
456 |
✓✗ | 34 |
LieGroupType lg; |
457 |
|||
458 |
✓✗✓✗ ✓✗ |
68 |
Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); |
459 |
✓✗✓✗ ✓✗ |
68 |
Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); |
460 |
|||
461 |
✓✗✓✗ |
68 |
Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); |
462 |
✓✗✓✗ ✓✗ |
68 |
Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); |
463 |
|||
464 |
✓✗ | 34 |
jmodel.calc(jdata,q_random,v_random); |
465 |
✓✗ | 34 |
Motion & m = jdata_base.v(); |
466 |
|||
467 |
✓✗ | 34 |
test(m); |
468 |
|||
469 |
✓✗ | 34 |
Bias & b = jdata_base.c(); |
470 |
✓✗ | 34 |
test(b); |
471 |
34 |
} |
|
472 |
|||
473 |
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
||
474 |
1 |
void operator()(const pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> &) |
|
475 |
{ |
||
476 |
// Do nothing |
||
477 |
1 |
} |
|
478 |
|||
479 |
template<typename JointModel> |
||
480 |
6 |
void operator()(const pinocchio::JointModelMimic<JointModel> &) |
|
481 |
{ |
||
482 |
typedef pinocchio::JointModelMimic<JointModel> JointModelMimic; |
||
483 |
typedef typename JointModelMimic::JointDerived JointDerived; |
||
484 |
typedef typename pinocchio::traits<JointDerived>::Motion_t Motion; |
||
485 |
typedef typename pinocchio::traits<JointDerived>::Bias_t Bias; |
||
486 |
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointDataMimic; |
||
487 |
typedef pinocchio::JointDataBase<JointDataMimic> JointDataBase; |
||
488 |
✓✗ | 6 |
JointModelMimic jmodel_mimic = init<JointModelMimic>::run(); |
489 |
✓✗ | 6 |
JointModel jmodel = init<JointModel>::run(); |
490 |
|||
491 |
✓✗ | 6 |
JointDataMimic jdata_mimic = jmodel_mimic.createData(); |
492 |
6 |
JointDataBase & jdata_mimic_base = static_cast<JointDataBase &>(jdata_mimic); |
|
493 |
|||
494 |
typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType; |
||
495 |
✓✗ | 6 |
LieGroupType lg; |
496 |
|||
497 |
✓✗✓✗ ✓✗ |
12 |
Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); |
498 |
✓✗✓✗ ✓✗ |
12 |
Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); |
499 |
|||
500 |
✓✗✓✗ |
12 |
Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); |
501 |
✓✗✓✗ ✓✗ |
12 |
Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); |
502 |
|||
503 |
✓✗ | 6 |
jmodel_mimic.calc(jdata_mimic,q_random,v_random); |
504 |
✓✗ | 6 |
Motion & m = jdata_mimic_base.v(); |
505 |
|||
506 |
✓✗ | 6 |
test(m); |
507 |
|||
508 |
✓✗ | 6 |
Bias & b = jdata_mimic_base.c(); |
509 |
✓✗ | 6 |
test(b); |
510 |
6 |
} |
|
511 |
|||
512 |
template<typename Motion> |
||
513 |
80 |
static void test(Motion & m) |
|
514 |
{ |
||
515 |
✓✗✓✗ ✓✗ |
80 |
generic_test(m,TEST_SERIALIZATION_FOLDER"/JointMotion","motion"); |
516 |
80 |
} |
|
517 |
|||
518 |
}; |
||
519 |
|||
520 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_multibody_joints_motion_serialization) |
521 |
{ |
||
522 |
using namespace pinocchio; |
||
523 |
✓✗ | 2 |
boost::mpl::for_each<JointModelVariant::types>(TestJointMotion()); |
524 |
2 |
} |
|
525 |
|||
526 |
struct TestJointData |
||
527 |
{ |
||
528 |
template <typename JointModel> |
||
529 |
34 |
void operator()(const pinocchio::JointModelBase<JointModel> &) const |
|
530 |
{ |
||
531 |
typedef typename JointModel::JointDerived JointDerived; |
||
532 |
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData; |
||
533 |
✓✗ | 34 |
JointModel jmodel = init<JointModel>::run(); |
534 |
|||
535 |
✓✗ | 34 |
JointData jdata = jmodel.createData(); |
536 |
|||
537 |
typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType; |
||
538 |
✓✗ | 34 |
LieGroupType lg; |
539 |
|||
540 |
✓✗✓✗ ✓✗ |
68 |
Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); |
541 |
✓✗✓✗ ✓✗ |
68 |
Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); |
542 |
|||
543 |
✓✗✓✗ |
68 |
Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); |
544 |
✓✗✓✗ ✓✗ |
68 |
Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); |
545 |
|||
546 |
✓✗ | 34 |
jmodel.calc(jdata,q_random,v_random); |
547 |
✓✗✓✗ |
34 |
pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); |
548 |
✓✗ | 34 |
jmodel.calc_aba(jdata,I,false); |
549 |
✓✗ | 34 |
test(jdata); |
550 |
34 |
} |
|
551 |
|||
552 |
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> |
||
553 |
1 |
void operator()(const pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> &) |
|
554 |
{ |
||
555 |
typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> JointModel; |
||
556 |
typedef typename JointModel::JointDerived JointDerived; |
||
557 |
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData; |
||
558 |
|||
559 |
✓✗ | 2 |
JointModel jmodel_build = init<JointModel>::run(); |
560 |
|||
561 |
✓✗ | 2 |
pinocchio::Model model; |
562 |
✓✗✓✗ ✓✗✓✗ |
1 |
model.addJoint(0, jmodel_build, pinocchio::SE3::Random(), "model"); |
563 |
✓✗ | 1 |
model.lowerPositionLimit.fill(-1.); |
564 |
✓✗ | 1 |
model.upperPositionLimit.fill( 1.); |
565 |
|||
566 |
✓✗ | 1 |
JointModel & jmodel = boost::get<JointModel>(model.joints[1]); |
567 |
✓✗ | 2 |
Eigen::VectorXd q_random = pinocchio::randomConfiguration(model); |
568 |
✓✗✓✗ |
2 |
Eigen::VectorXd v_random = Eigen::VectorXd::Random(model.nv); |
569 |
|||
570 |
✓✗ | 2 |
pinocchio::Data data(model); |
571 |
✓✗ | 1 |
JointData & jdata = boost::get<JointData>(data.joints[1]); |
572 |
|||
573 |
✓✗ | 1 |
jmodel.calc(jdata,q_random,v_random); |
574 |
✓✗✓✗ |
1 |
pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); |
575 |
✓✗ | 1 |
jmodel.calc_aba(jdata,I,false); |
576 |
|||
577 |
✓✗ | 1 |
test(jdata); |
578 |
1 |
} |
|
579 |
|||
580 |
template<typename JointModel> |
||
581 |
6 |
void operator()(const pinocchio::JointModelMimic<JointModel> &) |
|
582 |
{ |
||
583 |
typedef pinocchio::JointModelMimic<JointModel> JointModelMimic; |
||
584 |
typedef typename JointModelMimic::JointDerived JointDerived; |
||
585 |
typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointDataMimic; |
||
586 |
✓✗ | 6 |
JointModelMimic jmodel_mimic = init<JointModelMimic>::run(); |
587 |
✓✗ | 6 |
JointModel jmodel = init<JointModel>::run(); |
588 |
|||
589 |
✓✗ | 6 |
JointDataMimic jdata_mimic = jmodel_mimic.createData(); |
590 |
|||
591 |
typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType; |
||
592 |
✓✗ | 6 |
LieGroupType lg; |
593 |
|||
594 |
✓✗✓✗ ✓✗ |
12 |
Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(),-1.)); |
595 |
✓✗✓✗ ✓✗ |
12 |
Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); |
596 |
|||
597 |
✓✗✓✗ |
12 |
Eigen::VectorXd q_random = lg.randomConfiguration(lb,ub); |
598 |
✓✗✓✗ ✓✗ |
12 |
Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); |
599 |
|||
600 |
✓✗ | 6 |
jmodel_mimic.calc(jdata_mimic,q_random,v_random); |
601 |
✓✗✓✗ |
6 |
pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); |
602 |
✓✗ | 6 |
jmodel_mimic.calc_aba(jdata_mimic,I,false); |
603 |
✓✗ | 6 |
test(jdata_mimic); |
604 |
6 |
} |
|
605 |
|||
606 |
template<typename JointData> |
||
607 |
42 |
static void test(JointData & joint_data) |
|
608 |
{ |
||
609 |
✓✗✓✗ ✓✗ |
42 |
generic_test(joint_data,TEST_SERIALIZATION_FOLDER"/JointData","data"); |
610 |
42 |
} |
|
611 |
|||
612 |
}; |
||
613 |
|||
614 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_multibody_joints_data_serialization) |
615 |
{ |
||
616 |
using namespace pinocchio; |
||
617 |
✓✗ | 2 |
boost::mpl::for_each<JointModelVariant::types>(TestJointData()); |
618 |
2 |
} |
|
619 |
|||
620 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_model_serialization) |
621 |
{ |
||
622 |
using namespace pinocchio; |
||
623 |
|||
624 |
✓✗ | 2 |
Model model; |
625 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
626 |
|||
627 |
✓✗✓✗ ✓✗ |
2 |
generic_test(model,TEST_SERIALIZATION_FOLDER"/Model","Model"); |
628 |
2 |
} |
|
629 |
|||
630 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_throw_extension) |
631 |
{ |
||
632 |
using namespace pinocchio; |
||
633 |
|||
634 |
✓✗ | 4 |
Model model; |
635 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
636 |
|||
637 |
✓✗ | 4 |
const std::string & fake_filename = "this_is_a_fake_filename"; |
638 |
|||
639 |
{ |
||
640 |
✓✗ | 4 |
const std::string complete_filename = fake_filename + ".txt"; |
641 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
6 |
BOOST_REQUIRE_THROW(loadFromText(model,complete_filename), |
642 |
std::invalid_argument); |
||
643 |
} |
||
644 |
|||
645 |
✓✗✓✗ |
2 |
saveToText(model,TEST_SERIALIZATION_FOLDER"/model.txt"); |
646 |
✓✗✓✗ ✓✗ |
2 |
saveToXML(model,TEST_SERIALIZATION_FOLDER"/model.xml","model"); |
647 |
✓✗✓✗ |
2 |
saveToBinary(model,TEST_SERIALIZATION_FOLDER"/model.bin"); |
648 |
|||
649 |
{ |
||
650 |
✓✗ | 4 |
const std::string complete_filename = fake_filename + ".txte"; |
651 |
|||
652 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
6 |
BOOST_REQUIRE_THROW(loadFromText(model,complete_filename), |
653 |
std::invalid_argument); |
||
654 |
} |
||
655 |
|||
656 |
{ |
||
657 |
✓✗ | 4 |
const std::string complete_filename = fake_filename + ".xmle"; |
658 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✓✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
10 |
BOOST_REQUIRE_THROW(loadFromXML(model,complete_filename,"model"), |
659 |
std::invalid_argument); |
||
660 |
} |
||
661 |
|||
662 |
{ |
||
663 |
✓✗ | 4 |
const std::string complete_filename = fake_filename + ".bine"; |
664 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✓ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
6 |
BOOST_REQUIRE_THROW(loadFromBinary(model,complete_filename), |
665 |
std::invalid_argument); |
||
666 |
} |
||
667 |
|||
668 |
2 |
} |
|
669 |
|||
670 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_data_serialization) |
671 |
{ |
||
672 |
using namespace pinocchio; |
||
673 |
|||
674 |
✓✗ | 4 |
Model model; |
675 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
676 |
|||
677 |
✓✗ | 2 |
Data data(model); |
678 |
|||
679 |
✓✗✓✗ ✓✗ |
2 |
generic_test(data,TEST_SERIALIZATION_FOLDER"/Data","Data"); |
680 |
2 |
} |
|
681 |
|||
682 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_geometry_data_serialization) |
683 |
{ |
||
684 |
using namespace pinocchio; |
||
685 |
|||
686 |
✓✗ | 4 |
pinocchio::Model model; |
687 |
✓✗ | 2 |
pinocchio::buildModels::humanoid(model); |
688 |
✓✗ | 4 |
pinocchio::Data data(model); |
689 |
|||
690 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
691 |
✓✗ | 4 |
pinocchio::GeometryModel geom_model; |
692 |
✓✗ | 2 |
pinocchio::buildModels::humanoidGeometries(model,geom_model); |
693 |
✓✗ | 4 |
pinocchio::GeometryData geom_data(geom_model); |
694 |
|||
695 |
✓✗ | 2 |
const Eigen::VectorXd q = pinocchio::neutral(model); |
696 |
✓✗ | 2 |
pinocchio::forwardKinematics(model,data,q); |
697 |
✓✗ | 2 |
pinocchio::updateGeometryPlacements(model,data,geom_model,geom_data,q); |
698 |
|||
699 |
✓✗✓✗ ✓✗ |
2 |
generic_test(geom_data,TEST_SERIALIZATION_FOLDER"/GeomData","GeomData"); |
700 |
#endif |
||
701 |
|||
702 |
|||
703 |
2 |
} |
|
704 |
|||
705 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |