GCC Code Coverage Report

Directory: ./
File: unittest/contact-dynamics.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 214 0.0%
Branches: 0 1322 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #include "pinocchio/spatial/se3.hpp"
6 #include "pinocchio/multibody/model.hpp"
7 #include "pinocchio/multibody/data.hpp"
8 #include "pinocchio/algorithm/jacobian.hpp"
9 #include "pinocchio/algorithm/contact-dynamics.hpp"
10 #include "pinocchio/algorithm/joint-configuration.hpp"
11 #include "pinocchio/multibody/sample-models.hpp"
12 #include "pinocchio/utils/timer.hpp"
13
14 #include <iostream>
15
16 #include <boost/test/unit_test.hpp>
17 #include <boost/utility/binary.hpp>
18
19 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
20
21 ✗ BOOST_AUTO_TEST_CASE(test_FD)
22 {
23 using namespace Eigen;
24 using namespace pinocchio;
25
26 ✗ pinocchio::Model model;
27 ✗ pinocchio::buildModels::humanoidRandom(model, true);
28 ✗ pinocchio::Data data(model);
29
30 ✗ VectorXd q = VectorXd::Ones(model.nq);
31 ✗ q.segment<4>(3).normalize();
32
33 ✗ pinocchio::computeJointJacobians(model, data, q);
34
35 ✗ VectorXd v = VectorXd::Ones(model.nv);
36 ✗ VectorXd tau = VectorXd::Zero(model.nv);
37
38 ✗ const std::string RF = "rleg6_joint";
39 ✗ const std::string LF = "lleg6_joint";
40
41 ✗ Data::Matrix6x J_RF(6, model.nv);
42 ✗ J_RF.setZero();
43 ✗ getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
44 ✗ Data::Matrix6x J_LF(6, model.nv);
45 ✗ J_LF.setZero();
46 ✗ getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
47
48 ✗ Eigen::MatrixXd J(12, model.nv);
49 ✗ J.setZero();
50 ✗ J.topRows<6>() = J_RF;
51 ✗ J.bottomRows<6>() = J_LF;
52
53 ✗ Eigen::VectorXd gamma(VectorXd::Ones(12));
54
55 ✗ Eigen::MatrixXd H(J.transpose());
56
57 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
58 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
59 ✗ pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
60 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
61
62 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
63 ✗ data.M.transpose().triangularView<Eigen::StrictlyLower>();
64
65 ✗ MatrixXd Minv(data.M.inverse());
66 ✗ MatrixXd JMinvJt(J * Minv * J.transpose());
67
68 ✗ Eigen::MatrixXd G_ref(J.transpose());
69 ✗ cholesky::Uiv(model, data, G_ref);
70 ✗ for (int k = 0; k < model.nv; ++k)
71 ✗ G_ref.row(k) /= sqrt(data.D[k]);
72 ✗ Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
73 ✗ BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
74
75 ✗ VectorXd lambda_ref = -JMinvJt.inverse() * (J * Minv * (tau - data.nle) + gamma);
76 ✗ BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12));
77
78 ✗ VectorXd a_ref = Minv * (tau - data.nle + J.transpose() * lambda_ref);
79
80 Eigen::VectorXd dynamics_residual_ref(
81 ✗ data.M * a_ref + data.nle - tau - J.transpose() * lambda_ref);
82 ✗ BOOST_CHECK(
83 dynamics_residual_ref.norm()
84 <= 1e-11); // previously 1e-12, may be due to numerical approximations, i obtain 2.03e-12
85
86 ✗ Eigen::VectorXd constraint_residual(J * data.ddq + gamma);
87 ✗ BOOST_CHECK(constraint_residual.norm() <= 1e-12);
88
89 Eigen::VectorXd dynamics_residual(
90 ✗ data.M * data.ddq + data.nle - tau - J.transpose() * data.lambda_c);
91 ✗ BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
92 }
93
94 ✗ BOOST_AUTO_TEST_CASE(test_computeKKTMatrix)
95 {
96 using namespace Eigen;
97 using namespace pinocchio;
98 ✗ pinocchio::Model model;
99 ✗ pinocchio::buildModels::humanoidRandom(model, true);
100 ✗ pinocchio::Data data(model), data_ref(model);
101
102 ✗ VectorXd q = VectorXd::Ones(model.nq);
103 ✗ q.segment<4>(3).normalize();
104
105 ✗ pinocchio::computeJointJacobians(model, data_ref, q);
106
107 ✗ const std::string RF = "rleg6_joint";
108 ✗ const std::string LF = "lleg6_joint";
109
110 ✗ Data::Matrix6x J_RF(6, model.nv);
111 ✗ J_RF.setZero();
112 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
113 ✗ Data::Matrix6x J_LF(6, model.nv);
114 ✗ J_LF.setZero();
115 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
116
117 ✗ Eigen::MatrixXd J(12, model.nv);
118 ✗ J.setZero();
119 ✗ J.topRows<6>() = J_RF;
120 ✗ J.bottomRows<6>() = J_LF;
121
122 // Check Forward Dynamics
123 ✗ pinocchio::crba(model, data_ref, q, pinocchio::Convention::WORLD);
124 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
125 ✗ data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
126
127 ✗ Eigen::MatrixXd MJtJ(model.nv + 12, model.nv + 12);
128 ✗ MJtJ << data_ref.M, J.transpose(), J, Eigen::MatrixXd::Zero(12, 12);
129
130 ✗ Eigen::MatrixXd KKTMatrix_inv(model.nv + 12, model.nv + 12);
131 ✗ computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv);
132
133 ✗ BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
134 }
135
136 ✗ BOOST_AUTO_TEST_CASE(test_getKKTMatrix)
137 {
138 using namespace Eigen;
139 using namespace pinocchio;
140 ✗ pinocchio::Model model;
141 ✗ pinocchio::buildModels::humanoidRandom(model, true);
142 ✗ pinocchio::Data data(model);
143
144 ✗ VectorXd q = VectorXd::Ones(model.nq);
145 ✗ q.segment<4>(3).normalize();
146
147 ✗ pinocchio::computeJointJacobians(model, data, q);
148
149 ✗ VectorXd v = VectorXd::Ones(model.nv);
150 ✗ VectorXd tau = VectorXd::Zero(model.nv);
151
152 ✗ const std::string RF = "rleg6_joint";
153 ✗ const std::string LF = "lleg6_joint";
154
155 ✗ Data::Matrix6x J_RF(6, model.nv);
156 ✗ J_RF.setZero();
157 ✗ getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
158 ✗ Data::Matrix6x J_LF(6, model.nv);
159 ✗ J_LF.setZero();
160 ✗ getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
161
162 ✗ Eigen::MatrixXd J(12, model.nv);
163 ✗ J.setZero();
164 ✗ J.topRows<6>() = J_RF;
165 ✗ J.bottomRows<6>() = J_LF;
166
167 ✗ Eigen::VectorXd gamma(VectorXd::Ones(12));
168
169 ✗ Eigen::MatrixXd H(J.transpose());
170
171 // Check Forward Dynamics
172 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
173 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
174 ✗ pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
175 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
176
177 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
178 ✗ data.M.transpose().triangularView<Eigen::StrictlyLower>();
179
180 ✗ Eigen::MatrixXd MJtJ(model.nv + 12, model.nv + 12);
181 ✗ MJtJ << data.M, J.transpose(), J, Eigen::MatrixXd::Zero(12, 12);
182
183 ✗ Eigen::MatrixXd KKTMatrix_inv(model.nv + 12, model.nv + 12);
184
185 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
186 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
187 ✗ getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
188 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
189
190 ✗ BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
191
192 // Check Impulse Dynamics
193 ✗ const double r_coeff = 1.;
194 ✗ VectorXd v_before = VectorXd::Ones(model.nv);
195
196 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
197 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
198 ✗ pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
199 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
200
201 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
202 ✗ data.M.transpose().triangularView<Eigen::StrictlyLower>();
203 ✗ MJtJ << data.M, J.transpose(), J, Eigen::MatrixXd::Zero(12, 12);
204
205 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
206 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
207 ✗ getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv);
208 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
209
210 ✗ BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse()));
211 }
212
213 ✗ BOOST_AUTO_TEST_CASE(test_FD_with_damping)
214 {
215 using namespace Eigen;
216 using namespace pinocchio;
217
218 ✗ pinocchio::Model model;
219 ✗ pinocchio::buildModels::humanoidRandom(model, true);
220 ✗ pinocchio::Data data(model);
221
222 ✗ VectorXd q = VectorXd::Ones(model.nq);
223 ✗ q.segment<4>(3).normalize();
224
225 ✗ pinocchio::computeJointJacobians(model, data, q);
226
227 ✗ VectorXd v = VectorXd::Ones(model.nv);
228 ✗ VectorXd tau = VectorXd::Zero(model.nv);
229
230 ✗ const std::string RF = "rleg6_joint";
231
232 ✗ Data::Matrix6x J_RF(6, model.nv);
233 ✗ J_RF.setZero();
234 ✗ getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
235
236 ✗ Eigen::MatrixXd J(12, model.nv);
237 ✗ J.setZero();
238 ✗ J.topRows<6>() = J_RF;
239 ✗ J.bottomRows<6>() = J_RF;
240
241 ✗ Eigen::VectorXd gamma(VectorXd::Ones(12));
242
243 // Forward Dynamics with damping
244 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
245 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
246 ✗ pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12);
247 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
248
249 // Matrix Definitions
250 ✗ Eigen::MatrixXd H(J.transpose());
251 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
252 ✗ data.M.transpose().triangularView<Eigen::StrictlyLower>();
253
254 ✗ MatrixXd Minv(data.M.inverse());
255 ✗ MatrixXd JMinvJt(J * Minv * J.transpose());
256
257 // Check that JMinvJt is correctly formed
258 ✗ Eigen::MatrixXd G_ref(J.transpose());
259 ✗ cholesky::Uiv(model, data, G_ref);
260 ✗ for (int k = 0; k < model.nv; ++k)
261 ✗ G_ref.row(k) /= sqrt(data.D[k]);
262 ✗ Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
263 ✗ BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
264
265 // Actual Residuals
266 ✗ Eigen::VectorXd constraint_residual(J * data.ddq + gamma);
267 Eigen::VectorXd dynamics_residual(
268 ✗ data.M * data.ddq + data.nle - tau - J.transpose() * data.lambda_c);
269 ✗ BOOST_CHECK(constraint_residual.norm() <= 1e-9);
270 ✗ BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
271 }
272
273 ✗ BOOST_AUTO_TEST_CASE(test_ID)
274 {
275 using namespace Eigen;
276 using namespace pinocchio;
277
278 ✗ pinocchio::Model model;
279 ✗ pinocchio::buildModels::humanoidRandom(model, true);
280 ✗ pinocchio::Data data(model);
281
282 ✗ VectorXd q = VectorXd::Ones(model.nq);
283 ✗ q.segment<4>(3).normalize();
284
285 ✗ pinocchio::computeJointJacobians(model, data, q);
286
287 ✗ VectorXd v_before = VectorXd::Ones(model.nv);
288
289 ✗ const std::string RF = "rleg6_joint";
290 ✗ const std::string LF = "lleg6_joint";
291
292 ✗ Data::Matrix6x J_RF(6, model.nv);
293 ✗ J_RF.setZero();
294 ✗ getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
295 ✗ Data::Matrix6x J_LF(6, model.nv);
296 ✗ J_LF.setZero();
297 ✗ getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
298
299 ✗ Eigen::MatrixXd J(12, model.nv);
300 ✗ J.setZero();
301 ✗ J.topRows<6>() = J_RF;
302 ✗ J.bottomRows<6>() = J_LF;
303
304 ✗ const double r_coeff = 1.;
305
306 ✗ Eigen::MatrixXd H(J.transpose());
307
308 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
309 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
310 ✗ pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.);
311 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
312
313 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
314 ✗ data.M.transpose().triangularView<Eigen::StrictlyLower>();
315
316 ✗ MatrixXd Minv(data.M.inverse());
317 ✗ MatrixXd JMinvJt(J * Minv * J.transpose());
318
319 ✗ Eigen::MatrixXd G_ref(J.transpose());
320 ✗ cholesky::Uiv(model, data, G_ref);
321 ✗ for (int k = 0; k < model.nv; ++k)
322 ✗ G_ref.row(k) /= sqrt(data.D[k]);
323 ✗ Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref);
324 ✗ BOOST_CHECK(H_ref.isApprox(JMinvJt, 1e-12));
325
326 ✗ VectorXd lambda_ref = JMinvJt.inverse() * (-r_coeff * J * v_before - J * v_before);
327 ✗ BOOST_CHECK(data.impulse_c.isApprox(lambda_ref, 1e-12));
328
329 ✗ VectorXd v_after_ref = Minv * (data.M * v_before + J.transpose() * lambda_ref);
330
331 ✗ Eigen::VectorXd constraint_residual(J * data.dq_after + r_coeff * J * v_before);
332 ✗ BOOST_CHECK(constraint_residual.norm() <= 1e-12);
333
334 Eigen::VectorXd dynamics_residual(
335 ✗ data.M * data.dq_after - data.M * v_before - J.transpose() * data.impulse_c);
336 ✗ BOOST_CHECK(dynamics_residual.norm() <= 1e-12);
337 }
338
339 ✗ BOOST_AUTO_TEST_CASE(timings_fd_llt)
340 {
341 using namespace Eigen;
342 using namespace pinocchio;
343
344 ✗ pinocchio::Model model;
345 ✗ pinocchio::buildModels::humanoidRandom(model, true);
346 ✗ pinocchio::Data data(model);
347
348 #ifdef NDEBUG
349 #ifdef _INTENSE_TESTING_
350 const size_t NBT = 1000 * 1000;
351 #else
352 const size_t NBT = 100;
353 #endif
354
355 #else
356 ✗ const size_t NBT = 1;
357 ✗ std::cout << "(the time score in debug mode is not relevant) ";
358 #endif // ifndef NDEBUG
359
360 ✗ VectorXd q = VectorXd::Ones(model.nq);
361 ✗ q.segment<4>(3).normalize();
362
363 ✗ pinocchio::computeJointJacobians(model, data, q);
364
365 ✗ VectorXd v = VectorXd::Ones(model.nv);
366 ✗ VectorXd tau = VectorXd::Zero(model.nv);
367
368 ✗ const std::string RF = "rleg6_joint";
369 ✗ const std::string LF = "lleg6_joint";
370
371 ✗ Data::Matrix6x J_RF(6, model.nv);
372 ✗ J_RF.setZero();
373 ✗ getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF);
374 ✗ Data::Matrix6x J_LF(6, model.nv);
375 ✗ J_LF.setZero();
376 ✗ getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF);
377
378 ✗ Eigen::MatrixXd J(12, model.nv);
379 ✗ J.topRows<6>() = J_RF;
380 ✗ J.bottomRows<6>() = J_LF;
381
382 ✗ Eigen::VectorXd gamma(VectorXd::Ones(12));
383
384 ✗ model.lowerPositionLimit.head<7>().fill(-1.);
385 ✗ model.upperPositionLimit.head<7>().fill(1.);
386
387 ✗ q = pinocchio::randomConfiguration(model);
388
389 ✗ PinocchioTicToc timer(PinocchioTicToc::US);
390 ✗ timer.tic();
391 ✗ SMOOTH(NBT)
392 {
393 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
394 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
395 ✗ pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.);
396 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
397 }
398 ✗ timer.toc(std::cout, NBT);
399 }
400
401 BOOST_AUTO_TEST_SUITE_END()
402