GCC Code Coverage Report

Directory: ./
File: unittest/joint-configurations.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 270 0.0%
Branches: 0 2268 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #include "utils/model-generator.hpp"
6 #include "pinocchio/multibody/sample-models.hpp"
7 #include "pinocchio/algorithm/joint-configuration.hpp"
8 #include "pinocchio/math/quaternion.hpp"
9
10 #include <boost/test/unit_test.hpp>
11 #include <boost/utility/binary.hpp>
12
13 using namespace pinocchio;
14
15 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
16
17 ✗ BOOST_AUTO_TEST_CASE(integration_test)
18 {
19 ✗ Model model;
20 ✗ buildAllJointsModel(model);
21
22 ✗ std::vector<Eigen::VectorXd> qs(2);
23 ✗ std::vector<Eigen::VectorXd> qdots(2);
24 ✗ std::vector<Eigen::VectorXd> results(2);
25
26 //
27 // Test Case 0 : Integration of a config with zero velocity
28 //
29 ✗ qs[0] = Eigen::VectorXd::Ones(model.nq);
30 ✗ normalize(model, qs[0]);
31
32 ✗ qdots[0] = Eigen::VectorXd::Zero(model.nv);
33 ✗ results[0] = integrate(model, qs[0], qdots[0]);
34
35 ✗ BOOST_CHECK_MESSAGE(
36 results[0].isApprox(qs[0], 1e-12),
37 "integration of full body with zero velocity - wrong results");
38 }
39
40 ✗ BOOST_AUTO_TEST_CASE(interpolate_test)
41 {
42 ✗ Model model;
43 ✗ buildAllJointsModel(model);
44
45 Eigen::VectorXd q0(randomConfiguration(
46 ✗ model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
47 Eigen::VectorXd q1(randomConfiguration(
48 ✗ model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
49
50 ✗ Eigen::VectorXd q01_0 = interpolate(model, q0, q1, 0.0);
51 ✗ BOOST_CHECK_MESSAGE(isSameConfiguration(model, q01_0, q0), "interpolation: q01_0 != q0");
52
53 ✗ Eigen::VectorXd q01_1 = interpolate(model, q0, q1, 1.0);
54 ✗ BOOST_CHECK_MESSAGE(isSameConfiguration(model, q01_1, q1), "interpolation: q01_1 != q1");
55 }
56
57 ✗ BOOST_AUTO_TEST_CASE(diff_integration_test)
58 {
59 ✗ Model model;
60 ✗ buildAllJointsModel(model);
61
62 ✗ std::vector<Eigen::VectorXd> qs(2);
63 ✗ std::vector<Eigen::VectorXd> vs(2);
64 ✗ std::vector<Eigen::MatrixXd> results(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
65 ✗ std::vector<Eigen::MatrixXd> results_fd(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
66
67 ✗ qs[0] = Eigen::VectorXd::Ones(model.nq);
68 ✗ normalize(model, qs[0]);
69
70 ✗ vs[0] = Eigen::VectorXd::Zero(model.nv);
71 ✗ vs[1] = Eigen::VectorXd::Ones(model.nv);
72 ✗ dIntegrate(model, qs[0], vs[0], results[0], ARG0);
73
74 ✗ Eigen::VectorXd q_fd(model.nq), v_fd(model.nv);
75 ✗ v_fd.setZero();
76 ✗ const double eps = 1e-8;
77 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
78 {
79 ✗ v_fd[k] = eps;
80 ✗ q_fd = integrate(model, qs[0], v_fd);
81 ✗ results_fd[0].col(k) = difference(model, qs[0], q_fd) / eps;
82 ✗ v_fd[k] = 0.;
83 }
84 ✗ BOOST_CHECK(results[0].isIdentity(sqrt(eps)));
85 ✗ BOOST_CHECK(results[0].isApprox(results_fd[0], sqrt(eps)));
86
87 ✗ dIntegrate(model, qs[0], vs[0], results[1], ARG1);
88 ✗ BOOST_CHECK(results[1].isApprox(results[0]));
89
90 ✗ dIntegrate(model, qs[0], vs[1], results[0], ARG0);
91 ✗ Eigen::VectorXd q_fd_intermediate(model.nq);
92 ✗ Eigen::VectorXd q0_plus_v = integrate(model, qs[0], vs[1]);
93 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
94 {
95 ✗ v_fd[k] = eps;
96 ✗ q_fd_intermediate = integrate(model, qs[0], v_fd);
97 ✗ q_fd = integrate(model, q_fd_intermediate, vs[1]);
98 ✗ results_fd[0].col(k) = difference(model, q0_plus_v, q_fd) / eps;
99 ✗ v_fd[k] = 0.;
100 }
101
102 ✗ BOOST_CHECK(results[0].isApprox(results_fd[0], sqrt(eps)));
103
104 ✗ dIntegrate(model, qs[0], vs[1], results[1], ARG1);
105 ✗ v_fd = vs[1];
106 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
107 {
108 ✗ v_fd[k] += eps;
109 ✗ q_fd = integrate(model, qs[0], v_fd);
110 ✗ results_fd[1].col(k) = difference(model, q0_plus_v, q_fd) / eps;
111 ✗ v_fd[k] -= eps;
112 }
113
114 ✗ BOOST_CHECK(results[1].isApprox(results_fd[1], sqrt(eps)));
115 }
116
117 ✗ BOOST_AUTO_TEST_CASE(diff_difference_test)
118 {
119 ✗ Model model;
120 ✗ buildAllJointsModel(model);
121
122 ✗ std::vector<Eigen::VectorXd> qs(2);
123 ✗ std::vector<Eigen::VectorXd> vs(2);
124 ✗ std::vector<Eigen::MatrixXd> results(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
125 ✗ std::vector<Eigen::MatrixXd> results_fd(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
126
127 ✗ qs[0] = Eigen::VectorXd::Random(model.nq);
128 ✗ normalize(model, qs[0]);
129 ✗ const Eigen::VectorXd & q0 = qs[0];
130 ✗ qs[1] = Eigen::VectorXd::Random(model.nq);
131 ✗ normalize(model, qs[1]);
132 ✗ const Eigen::VectorXd & q1 = qs[1];
133
134 ✗ vs[0] = Eigen::VectorXd::Zero(model.nv);
135 ✗ vs[1] = Eigen::VectorXd::Ones(model.nv);
136 ✗ dDifference(model, q0, q1, results[0], ARG0);
137
138 ✗ Eigen::VectorXd q_fd(model.nq), v_fd(model.nv);
139 ✗ v_fd.setZero();
140 ✗ const double eps = 1e-8;
141 ✗ const Eigen::VectorXd v_ref = difference(model, q0, q1);
142 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
143 {
144 ✗ v_fd[k] = eps;
145 ✗ q_fd = integrate(model, q0, v_fd);
146 ✗ results_fd[0].col(k) = (difference(model, q_fd, q1) - v_ref) / eps;
147 ✗ v_fd[k] = 0.;
148 }
149 ✗ BOOST_CHECK(results[0].isApprox(results_fd[0], sqrt(eps)));
150
151 ✗ dDifference(model, q0, q0, results[0], ARG0);
152 ✗ BOOST_CHECK((-results[0]).isIdentity());
153
154 ✗ dDifference(model, q0, q0, results[1], ARG1);
155 ✗ BOOST_CHECK(results[1].isIdentity());
156
157 ✗ dDifference(model, q0, q1, results[1], ARG1);
158 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
159 {
160 ✗ v_fd[k] = eps;
161 ✗ q_fd = integrate(model, q1, v_fd);
162 ✗ results_fd[1].col(k) = (difference(model, q0, q_fd) - v_ref) / eps;
163 ✗ v_fd[k] = 0.;
164 }
165 ✗ BOOST_CHECK(results[1].isApprox(results_fd[1], sqrt(eps)));
166 }
167
168 ✗ BOOST_AUTO_TEST_CASE(diff_difference_vs_diff_integrate)
169 {
170 ✗ Model model;
171 ✗ buildAllJointsModel(model);
172
173 ✗ std::vector<Eigen::VectorXd> qs(2);
174 ✗ std::vector<Eigen::VectorXd> vs(2);
175 ✗ std::vector<Eigen::MatrixXd> results(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
176 ✗ std::vector<Eigen::MatrixXd> results_fd(2, Eigen::MatrixXd::Zero(model.nv, model.nv));
177
178 ✗ Eigen::VectorXd q0 = Eigen::VectorXd::Random(model.nq);
179 ✗ normalize(model, q0);
180
181 ✗ Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
182 ✗ Eigen::VectorXd q1 = integrate(model, q0, v);
183
184 ✗ Eigen::VectorXd v_diff = difference(model, q0, q1);
185 ✗ BOOST_CHECK(v_diff.isApprox(v));
186
187 ✗ Eigen::MatrixXd J_int_dq = Eigen::MatrixXd::Zero(model.nv, model.nv);
188 ✗ Eigen::MatrixXd J_int_dv = Eigen::MatrixXd::Zero(model.nv, model.nv);
189 ✗ dIntegrate(model, q0, v, J_int_dq, ARG0);
190 ✗ dIntegrate(model, q0, v, J_int_dv, ARG1);
191
192 ✗ Eigen::MatrixXd J_diff_dq0 = Eigen::MatrixXd::Zero(model.nv, model.nv);
193 ✗ Eigen::MatrixXd J_diff_dq1 = Eigen::MatrixXd::Zero(model.nv, model.nv);
194 ✗ dDifference(model, q0, q1, J_diff_dq0, ARG0);
195 ✗ dDifference(model, q0, q1, J_diff_dq1, ARG1);
196
197 ✗ BOOST_CHECK(J_int_dq.isApprox(Eigen::MatrixXd(-(J_int_dv * J_diff_dq0))));
198 ✗ BOOST_CHECK(Eigen::MatrixXd(J_int_dv * J_diff_dq1).isIdentity());
199 }
200
201 ✗ BOOST_AUTO_TEST_CASE(dIntegrate_assignementop_test)
202 {
203 ✗ Model model;
204 ✗ buildAllJointsModel(model);
205
206 ✗ std::vector<Eigen::MatrixXd> results(3, Eigen::MatrixXd::Zero(model.nv, model.nv));
207
208 ✗ Eigen::VectorXd qs = Eigen::VectorXd::Ones(model.nq);
209 ✗ normalize(model, qs);
210
211 ✗ Eigen::VectorXd vs = Eigen::VectorXd::Random(model.nv);
212
213 // SETTO
214 ✗ dIntegrate(model, qs, vs, results[0], ARG0);
215 ✗ dIntegrate(model, qs, vs, results[1], ARG0, SETTO);
216 ✗ BOOST_CHECK(results[0].isApprox(results[1]));
217
218 // ADDTO
219 ✗ results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
220 ✗ results[2] = results[1];
221 ✗ results[0].setZero();
222 ✗ dIntegrate(model, qs, vs, results[0], ARG0, SETTO);
223 ✗ dIntegrate(model, qs, vs, results[1], ARG0, ADDTO);
224 ✗ BOOST_CHECK(results[1].isApprox(results[2] + results[0]));
225
226 // RMTO
227 ✗ results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
228 ✗ results[2] = results[1];
229 ✗ results[0].setZero();
230 ✗ dIntegrate(model, qs, vs, results[0], ARG0, SETTO);
231 ✗ dIntegrate(model, qs, vs, results[1], ARG0, RMTO);
232 ✗ BOOST_CHECK(results[1].isApprox(results[2] - results[0]));
233
234 // SETTO
235 ✗ results[0].setZero();
236 ✗ results[1].setZero();
237 ✗ dIntegrate(model, qs, vs, results[0], ARG1);
238 ✗ dIntegrate(model, qs, vs, results[1], ARG1, SETTO);
239 ✗ BOOST_CHECK(results[0].isApprox(results[1]));
240
241 // ADDTO
242 ✗ results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
243 ✗ results[2] = results[1];
244 ✗ results[0].setZero();
245 ✗ dIntegrate(model, qs, vs, results[0], ARG1, SETTO);
246 ✗ dIntegrate(model, qs, vs, results[1], ARG1, ADDTO);
247 ✗ BOOST_CHECK(results[1].isApprox(results[2] + results[0]));
248
249 // RMTO
250 ✗ results[1] = Eigen::MatrixXd::Random(model.nv, model.nv);
251 ✗ results[2] = results[1];
252 ✗ results[0].setZero();
253 ✗ dIntegrate(model, qs, vs, results[0], ARG1, SETTO);
254 ✗ dIntegrate(model, qs, vs, results[1], ARG1, RMTO);
255 ✗ BOOST_CHECK(results[1].isApprox(results[2] - results[0]));
256
257 // Transport
258 ✗ std::vector<Eigen::MatrixXd> J(2, Eigen::MatrixXd::Zero(model.nv, 2 * model.nv));
259 ✗ J[0] = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
260 ✗ results[0].setZero();
261 ✗ dIntegrate(model, qs, vs, results[0], ARG0, SETTO);
262 ✗ dIntegrateTransport(model, qs, vs, J[0], J[1], ARG0);
263 ✗ BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
264
265 ✗ J[0] = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
266 ✗ results[0].setZero();
267 ✗ dIntegrate(model, qs, vs, results[0], ARG1, SETTO);
268 ✗ dIntegrateTransport(model, qs, vs, J[0], J[1], ARG1);
269 ✗ BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
270
271 // TransportInPlace
272 ✗ J[1] = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
273 ✗ J[0] = J[1];
274 ✗ results[0].setZero();
275 ✗ dIntegrate(model, qs, vs, results[0], ARG0, SETTO);
276 ✗ dIntegrateTransport(model, qs, vs, J[1], ARG0);
277 ✗ BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
278
279 ✗ J[1] = Eigen::MatrixXd::Random(model.nv, 2 * model.nv);
280 ✗ J[0] = J[1];
281 ✗ results[0].setZero();
282 ✗ dIntegrate(model, qs, vs, results[0], ARG1, SETTO);
283 ✗ dIntegrateTransport(model, qs, vs, J[1], ARG1);
284 ✗ BOOST_CHECK(J[1].isApprox(results[0] * J[0]));
285 }
286
287 ✗ BOOST_AUTO_TEST_CASE(integrate_difference_test)
288 {
289 ✗ Model model;
290 ✗ buildAllJointsModel(model);
291
292 Eigen::VectorXd q0(randomConfiguration(
293 ✗ model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
294 Eigen::VectorXd q1(randomConfiguration(
295 ✗ model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
296 ✗ Eigen::VectorXd qdot(Eigen::VectorXd::Random(model.nv));
297
298 ✗ BOOST_CHECK_MESSAGE(
299 isSameConfiguration(model, integrate(model, q0, difference(model, q0, q1)), q1),
300 "Integrate (difference) - wrong results");
301
302 ✗ BOOST_CHECK_MESSAGE(
303 difference(model, q0, integrate(model, q0, qdot)).isApprox(qdot),
304 "difference (integrate) - wrong results");
305 }
306
307 ✗ BOOST_AUTO_TEST_CASE(neutral_configuration_test)
308 {
309 ✗ Model model;
310 ✗ buildAllJointsModel(model);
311
312 ✗ Eigen::VectorXd expected(model.nq);
313 ✗ expected << 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
314
315 ✗ Eigen::VectorXd neutral_config = neutral(model);
316 ✗ BOOST_CHECK_MESSAGE(
317 neutral_config.isApprox(expected, 1e-12), "neutral configuration - wrong results");
318 }
319
320 ✗ BOOST_AUTO_TEST_CASE(distance_configuration_test)
321 {
322 ✗ Model model;
323 ✗ buildAllJointsModel(model);
324
325 ✗ Model::ConfigVectorType q0 = neutral(model);
326 ✗ Model::ConfigVectorType q1(integrate(model, q0, Model::TangentVectorType::Ones(model.nv)));
327
328 ✗ double dist = distance(model, q0, q1);
329
330 ✗ BOOST_CHECK_MESSAGE(dist > 0., "distance - wrong results");
331 ✗ BOOST_CHECK_SMALL(dist - difference(model, q0, q1).norm(), 1e-12);
332 }
333
334 ✗ BOOST_AUTO_TEST_CASE(squared_distance_test)
335 {
336 ✗ Model model;
337 ✗ buildAllJointsModel(model);
338
339 Eigen::VectorXd q0(randomConfiguration(
340 ✗ model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
341 Eigen::VectorXd q1(randomConfiguration(
342 ✗ model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq)));
343
344 ✗ double dist = distance(model, q0, q1);
345 ✗ Eigen::VectorXd squaredDistance_ = squaredDistance(model, q0, q1);
346
347 ✗ BOOST_CHECK_SMALL(dist - math::sqrt(squaredDistance_.sum()), 1e-12);
348 }
349
350 ✗ BOOST_AUTO_TEST_CASE(uniform_sampling_test)
351 {
352 ✗ Model model;
353 ✗ buildAllJointsModel(model);
354
355 ✗ model.lowerPositionLimit = -1 * Eigen::VectorXd::Ones(model.nq);
356 ✗ model.upperPositionLimit = Eigen::VectorXd::Ones(model.nq);
357 ✗ Eigen::VectorXd q1(randomConfiguration(model));
358
359 ✗ for (int i = 0; i < model.nq; ++i)
360 {
361 ✗ BOOST_CHECK_MESSAGE(
362 q1[i] >= model.lowerPositionLimit[i] && q1[i] <= model.upperPositionLimit[i],
363 " UniformlySample : Generated config not in bounds");
364 }
365 }
366
367 ✗ BOOST_AUTO_TEST_CASE(normalize_test)
368 {
369 ✗ Model model;
370 ✗ buildAllJointsModel(model);
371
372 ✗ Eigen::VectorXd q(Eigen::VectorXd::Ones(model.nq));
373 ✗ pinocchio::normalize(model, q);
374
375 ✗ BOOST_CHECK(q.head<3>().isApprox(Eigen::VectorXd::Ones(3)));
376 ✗ BOOST_CHECK(
377 fabs(q.segment<4>(3).norm() - 1)
378 < Eigen::NumTraits<double>::epsilon()); // quaternion of freeflyer
379 ✗ BOOST_CHECK(
380 fabs(q.segment<4>(7).norm() - 1)
381 < Eigen::NumTraits<double>::epsilon()); // quaternion of spherical joint
382 ✗ const int n = model.nq - 7 - 4 - 4; // free flyer + spherical + planar
383 ✗ BOOST_CHECK(q.tail(n).isApprox(Eigen::VectorXd::Ones(n)));
384 }
385
386 ✗ BOOST_AUTO_TEST_CASE(is_normalized_test)
387 {
388 ✗ Model model;
389 ✗ buildAllJointsModel(model);
390
391 ✗ Eigen::VectorXd q;
392 ✗ q = Eigen::VectorXd::Ones(model.nq);
393 ✗ BOOST_CHECK(!pinocchio::isNormalized(model, q));
394 ✗ BOOST_CHECK(!pinocchio::isNormalized(model, q, 1e-8));
395 ✗ BOOST_CHECK(pinocchio::isNormalized(model, q, 1e2));
396
397 ✗ pinocchio::normalize(model, q);
398 ✗ BOOST_CHECK(pinocchio::isNormalized(model, q));
399 ✗ BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
400
401 ✗ q = pinocchio::neutral(model);
402 ✗ BOOST_CHECK(pinocchio::isNormalized(model, q));
403 ✗ BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
404
405 ✗ model.lowerPositionLimit = -1 * Eigen::VectorXd::Ones(model.nq);
406 ✗ model.upperPositionLimit = Eigen::VectorXd::Ones(model.nq);
407 ✗ q = pinocchio::randomConfiguration(model);
408 ✗ BOOST_CHECK(pinocchio::isNormalized(model, q));
409 ✗ BOOST_CHECK(pinocchio::isNormalized(model, q, 1e-8));
410 }
411
412 ✗ BOOST_AUTO_TEST_CASE(integrateCoeffWiseJacobian_test)
413 {
414 ✗ Model model;
415 ✗ buildModels::humanoidRandom(model);
416
417 ✗ Eigen::VectorXd q(Eigen::VectorXd::Ones(model.nq));
418 ✗ pinocchio::normalize(model, q);
419
420 ✗ Eigen::MatrixXd jac(model.nq, model.nv);
421 ✗ jac.setZero();
422
423 ✗ integrateCoeffWiseJacobian(model, q, jac);
424
425 ✗ Eigen::MatrixXd jac_fd(model.nq, model.nv);
426 ✗ Eigen::VectorXd q_plus;
427 ✗ const double eps = 1e-8;
428
429 ✗ Eigen::VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
430 ✗ for (int k = 0; k < model.nv; ++k)
431 {
432 ✗ v_eps[k] = eps;
433 ✗ q_plus = integrate(model, q, v_eps);
434 ✗ jac_fd.col(k) = (q_plus - q) / eps;
435
436 ✗ v_eps[k] = 0.;
437 }
438 ✗ BOOST_CHECK(jac.isApprox(jac_fd, sqrt(eps)));
439 }
440
441 BOOST_AUTO_TEST_SUITE_END()
442