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