GCC Code Coverage Report

Directory: ./
File: unittest/contact-cholesky.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 941 0.0%
Branches: 0 5788 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2022 INRIA
3 //
4
5 #include <iostream>
6
7 #include "pinocchio/algorithm/jacobian.hpp"
8 #include "pinocchio/algorithm/cholesky.hpp"
9 #include "pinocchio/algorithm/contact-info.hpp"
10 #include "pinocchio/algorithm/crba.hpp"
11 #include "pinocchio/algorithm/contact-cholesky.hxx"
12 #include "pinocchio/algorithm/joint-configuration.hpp"
13 #include "pinocchio/multibody/sample-models.hpp"
14
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17
18 namespace pinocchio
19 {
20 namespace cholesky
21 {
22 template<typename Scalar, int Options>
23 struct ContactCholeskyDecompositionAccessorTpl
24 : public ContactCholeskyDecompositionTpl<Scalar, Options>
25 {
26 typedef ContactCholeskyDecompositionTpl<Scalar, Options> Base;
27 typedef typename Base::IndexVector IndexVector;
28 typedef typename Base::BooleanVector BooleanVector;
29
30 ✗ ContactCholeskyDecompositionAccessorTpl(const Base & other)
31 ✗ : Base(other)
32 {
33 }
34
35 ✗ const IndexVector & getParents_fromRow() const
36 {
37 ✗ return this->parents_fromRow;
38 }
39 ✗ const IndexVector & getLastChild() const
40 {
41 ✗ return this->last_child;
42 }
43 ✗ const IndexVector & getNvSubtree_fromRow() const
44 {
45 ✗ return this->nv_subtree_fromRow;
46 }
47 const std::vector<BooleanVector> & getJoint1_indexes() const
48 {
49 return this->joint1_indexes;
50 }
51 const std::vector<BooleanVector> & getJoint2_indexes() const
52 {
53 return this->joint2_indexes;
54 }
55 };
56
57 typedef ContactCholeskyDecompositionAccessorTpl<double, 0> ContactCholeskyDecompositionAccessor;
58 } // namespace cholesky
59 } // namespace pinocchio
60
61 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
62
63 ✗ BOOST_AUTO_TEST_CASE(contact_operator_equal)
64 {
65
66 using namespace Eigen;
67 using namespace pinocchio;
68 using namespace cholesky;
69
70 ✗ pinocchio::Model humanoid_model;
71 ✗ pinocchio::buildModels::humanoidRandom(humanoid_model);
72 ✗ Data humanoid_data(humanoid_model);
73
74 ✗ pinocchio::Model manipulator_model;
75 ✗ pinocchio::buildModels::manipulator(manipulator_model);
76 ✗ Data manipulator_data(manipulator_model);
77
78 ✗ const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
79 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty;
80
81 ✗ humanoid_model.lowerPositionLimit.head<3>().fill(-1.);
82 ✗ humanoid_model.upperPositionLimit.head<3>().fill(1.);
83 ✗ VectorXd humanoid_q = randomConfiguration(humanoid_model);
84 ✗ crba(humanoid_model, humanoid_data, humanoid_q, Convention::WORLD);
85
86 ✗ VectorXd manipulator_q = randomConfiguration(manipulator_model);
87 ✗ crba(manipulator_model, manipulator_data, manipulator_q, Convention::WORLD);
88
89 ✗ ContactCholeskyDecomposition humanoid_chol(humanoid_model), manipulator_chol(manipulator_model);
90 ✗ humanoid_chol.compute(humanoid_model, humanoid_data, contact_models_empty, contact_datas_empty);
91 ✗ manipulator_chol.compute(
92 manipulator_model, manipulator_data, contact_models_empty, contact_datas_empty);
93
94 ✗ BOOST_CHECK(humanoid_chol == humanoid_chol);
95 ✗ BOOST_CHECK(humanoid_chol != manipulator_chol);
96 }
97
98 ✗ BOOST_AUTO_TEST_CASE(contact_cholesky_simple)
99 {
100 using namespace Eigen;
101 using namespace pinocchio;
102 using namespace pinocchio::cholesky;
103
104 ✗ pinocchio::Model model;
105 ✗ pinocchio::buildModels::humanoidRandom(model, true);
106 ✗ pinocchio::Data data_ref(model);
107
108 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
109 ✗ model.upperPositionLimit.head<3>().fill(1.);
110 ✗ VectorXd q = randomConfiguration(model);
111 ✗ crba(model, data_ref, q, Convention::WORLD);
112
113 ✗ pinocchio::cholesky::decompose(model, data_ref);
114 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
115 ✗ data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose();
116
117 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
118 ✗ const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
119 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty;
120 ✗ contact_chol_decomposition.allocate(model, contact_models_empty);
121
122 ✗ BOOST_CHECK(contact_chol_decomposition.D.size() == model.nv);
123 ✗ BOOST_CHECK(contact_chol_decomposition.Dinv.size() == model.nv);
124 ✗ BOOST_CHECK(contact_chol_decomposition.U.rows() == model.nv);
125 ✗ BOOST_CHECK(contact_chol_decomposition.U.cols() == model.nv);
126 ✗ BOOST_CHECK(contact_chol_decomposition.size() == model.nv);
127 ✗ BOOST_CHECK(contact_chol_decomposition.U.diagonal().isOnes());
128
129 ✗ Data data(model);
130 ✗ crba(model, data, q, Convention::WORLD);
131 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
132 ✗ data.M.triangularView<Eigen::StrictlyUpper>().transpose();
133
134 ✗ contact_chol_decomposition.compute(model, data, contact_models_empty, contact_datas_empty);
135
136 ✗ data_ref.Minv = data_ref.M.inverse();
137 ✗ Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
138 ✗ contact_chol_decomposition.getInverseMassMatrix(Minv_test);
139
140 ✗ BOOST_CHECK(data.M.isApprox(data_ref.M));
141 ✗ BOOST_CHECK(Minv_test.isApprox(data_ref.Minv));
142
143 ✗ BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv)));
144 ✗ BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv)));
145 ✗ BOOST_CHECK(
146 data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)));
147
148 ✗ ContactCholeskyDecompositionAccessor access(contact_chol_decomposition);
149 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
150 {
151 ✗ BOOST_CHECK(access.getParents_fromRow()[k] == data.parents_fromRow[(size_t)k]);
152 }
153
154 ✗ for (Eigen::DenseIndex k = 0; k < model.njoints; ++k)
155 {
156 ✗ BOOST_CHECK(access.getLastChild()[k] == data.lastChild[(size_t)k]);
157 }
158
159 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
160 {
161 ✗ BOOST_CHECK(access.getNvSubtree_fromRow()[k] == data.nvSubtree_fromRow[(size_t)k]);
162 }
163
164 // Test basic operation
165 ✗ VectorXd v_in(VectorXd::Random(model.nv));
166 ✗ MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20));
167
168 // Test Uv
169 ✗ VectorXd Uv_op_res(v_in), Uv_op_ref(v_in);
170
171 ✗ contact_chol_decomposition.Uv(Uv_op_res);
172 ✗ pinocchio::cholesky::Uv(model, data_ref, Uv_op_ref);
173
174 ✗ BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref));
175
176 ✗ MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in);
177
178 ✗ contact_chol_decomposition.Uv(Uv_mat_op_res);
179 ✗ pinocchio::cholesky::Uv(model, data_ref, Uv_mat_op_ref);
180
181 ✗ BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref));
182
183 // Test Utv
184 ✗ VectorXd Utv_op_res(v_in), Utv_op_ref(v_in);
185
186 ✗ contact_chol_decomposition.Utv(Utv_op_res);
187 ✗ pinocchio::cholesky::Utv(model, data_ref, Utv_op_ref);
188
189 ✗ BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref));
190
191 ✗ MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in);
192
193 ✗ contact_chol_decomposition.Utv(Utv_mat_op_res);
194 ✗ pinocchio::cholesky::Utv(model, data_ref, Utv_mat_op_ref);
195
196 ✗ BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref));
197
198 // Test Uiv
199 ✗ VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in);
200
201 ✗ contact_chol_decomposition.Uiv(Uiv_op_res);
202 ✗ pinocchio::cholesky::Uiv(model, data_ref, Uiv_op_ref);
203
204 ✗ BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref));
205
206 ✗ MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in);
207
208 ✗ contact_chol_decomposition.Uiv(Uiv_mat_op_res);
209 ✗ pinocchio::cholesky::Uiv(model, data_ref, Uiv_mat_op_ref);
210
211 ✗ BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref));
212
213 // Test Utiv
214 ✗ VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in);
215
216 ✗ contact_chol_decomposition.Utiv(Utiv_op_res);
217 ✗ pinocchio::cholesky::Utiv(model, data_ref, Utiv_op_ref);
218
219 ✗ BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref));
220
221 ✗ MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in);
222
223 ✗ contact_chol_decomposition.Utiv(Utiv_mat_op_res);
224 ✗ pinocchio::cholesky::Utiv(model, data_ref, Utiv_mat_op_ref);
225
226 ✗ BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref));
227
228 // SolveInPlace
229 ✗ VectorXd sol(v_in);
230 ✗ contact_chol_decomposition.solveInPlace(sol);
231
232 ✗ VectorXd sol_ref(data.M.inverse() * v_in);
233
234 ✗ BOOST_CHECK(sol.isApprox(sol_ref));
235
236 ✗ MatrixXd sol_mat(mat_in);
237 ✗ contact_chol_decomposition.solveInPlace(sol_mat);
238
239 ✗ MatrixXd sol_mat_ref(data.M.inverse() * mat_in);
240
241 ✗ BOOST_CHECK(sol_mat.isApprox(sol_mat_ref));
242
243 // solve
244 ✗ MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in);
245 ✗ BOOST_CHECK(sol_copy_mat.isApprox(sol_mat));
246
247 // inverse
248 ✗ MatrixXd M_inv(model.nv, model.nv);
249 ✗ contact_chol_decomposition.inverse(M_inv);
250
251 ✗ MatrixXd M_inv_ref = data.M.inverse();
252 ✗ BOOST_CHECK(M_inv.isApprox(M_inv_ref));
253
254 // test retrieve Mass Matrix Cholesky Decomposition
255 ContactCholeskyDecomposition mass_matrix_chol =
256 ✗ contact_chol_decomposition.getMassMatrixChoeslkyDecomposition(model);
257
258 // test Operational Space Inertia Matrix
259 ✗ MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
260 ✗ MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
261 ✗ BOOST_CHECK(iosim.size() == 0);
262 ✗ BOOST_CHECK(osim.size() == 0);
263
264 ✗ BOOST_CHECK(mass_matrix_chol == contact_chol_decomposition);
265 ✗ BOOST_CHECK(mass_matrix_chol.U.isApprox(data_ref.U));
266 ✗ BOOST_CHECK(mass_matrix_chol.D.isApprox(data_ref.D));
267 ✗ BOOST_CHECK(mass_matrix_chol.Dinv.isApprox(data_ref.Dinv));
268 }
269
270 ✗ BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_LOCAL)
271 {
272 using namespace Eigen;
273 using namespace pinocchio;
274 using namespace pinocchio::cholesky;
275
276 ✗ pinocchio::Model model;
277 ✗ pinocchio::buildModels::humanoidRandom(model, true);
278 ✗ pinocchio::Data data_ref(model);
279
280 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
281 ✗ model.upperPositionLimit.head<3>().fill(1.);
282 ✗ VectorXd q = randomConfiguration(model);
283
284 ✗ const std::string RF = "rleg6_joint";
285 ✗ const std::string LF = "lleg6_joint";
286
287 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
288 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
289 ✗ RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL);
290 ✗ contact_models.push_back(ci_RF);
291 ✗ contact_datas.push_back(RigidConstraintData(ci_RF));
292 ✗ RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL);
293 ✗ contact_models.push_back(ci_LF);
294 ✗ contact_datas.push_back(RigidConstraintData(ci_LF));
295
296 // Compute Mass Matrix
297 ✗ crba(model, data_ref, q, Convention::WORLD);
298 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
299 ✗ data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose();
300
301 // Compute Cholesky decomposition
302 ✗ pinocchio::cholesky::decompose(model, data_ref);
303
304 // Compute Jacobians
305 ✗ Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv);
306 ✗ J_RF.setZero();
307 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
308 ✗ J_LF.setZero();
309 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
310
311 ✗ const int constraint_dim = 12;
312 ✗ const int total_dim = model.nv + constraint_dim;
313
314 ✗ Data::MatrixXs H(total_dim, total_dim);
315 ✗ H.setZero();
316 ✗ H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
317 ✗ H.middleRows<6>(0).rightCols(model.nv) = J_RF;
318 ✗ H.middleRows<6>(6).rightCols(model.nv) = J_LF;
319
320 ✗ H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
321
322 ✗ Data data(model);
323 ✗ crba(model, data, q, Convention::WORLD);
324 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
325 ✗ contact_chol_decomposition.allocate(model, contact_models);
326
327 ✗ ContactCholeskyDecompositionAccessor access(contact_chol_decomposition);
328 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
329 {
330 ✗ if (data.parents_fromRow[(size_t)k] == -1)
331 ✗ BOOST_CHECK(access.getParents_fromRow()[k + constraint_dim] == -1);
332 else
333 ✗ BOOST_CHECK(
334 access.getParents_fromRow()[k + constraint_dim]
335 == data.parents_fromRow[(size_t)k] + constraint_dim);
336 }
337
338 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas);
339
340 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
341 ✗ data.M.triangularView<Eigen::StrictlyUpper>().transpose();
342
343 ✗ BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv)));
344 ✗ BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv)));
345 ✗ BOOST_CHECK(
346 data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)));
347
348 Data::MatrixXs M_recomposed =
349 ✗ contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)
350 ✗ * contact_chol_decomposition.D.tail(model.nv).asDiagonal()
351 ✗ * contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose();
352 ✗ BOOST_CHECK(M_recomposed.isApprox(data.M));
353
354 Data::MatrixXs H_recomposed = contact_chol_decomposition.U
355 ✗ * contact_chol_decomposition.D.asDiagonal()
356 ✗ * contact_chol_decomposition.U.transpose();
357
358 ✗ BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
359 ✗ BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv)
360 .isApprox(H.topRightCorner(constraint_dim, model.nv)));
361 ✗ BOOST_CHECK(H_recomposed.isApprox(H));
362
363 // test Operational Space Inertia Matrix
364 {
365 ✗ MatrixXd JMinvJt = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse()
366 ✗ * H.middleRows<12>(0).rightCols(model.nv).transpose();
367 ✗ MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
368 ✗ MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
369 ✗ Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(constraint_dim, model.nv));
370 ✗ contact_chol_decomposition.getJMinv(JMinv_test);
371 ✗ MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse();
372 ✗ BOOST_CHECK(JMinv_ref.isApprox(JMinv_test));
373
374 ✗ BOOST_CHECK(iosim.isApprox(JMinvJt));
375 ✗ BOOST_CHECK(osim.isApprox(JMinvJt.inverse()));
376
377 ✗ const MatrixXd rhs = MatrixXd::Random(12, 12);
378 ✗ const MatrixXd res_delassus = contact_chol_decomposition.getDelassusCholeskyExpression() * rhs;
379 ✗ const MatrixXd res_delassus_ref = iosim * rhs;
380
381 ✗ BOOST_CHECK(res_delassus_ref.isApprox(res_delassus));
382
383 const MatrixXd res_delassus_inverse =
384 ✗ contact_chol_decomposition.getDelassusCholeskyExpression().solve(rhs);
385 ✗ const MatrixXd res_delassus_inverse_ref = osim * rhs;
386
387 ✗ BOOST_CHECK(res_delassus_inverse_ref.isApprox(res_delassus_inverse));
388 }
389
390 // test Mass matrix cholesky
391 ✗ data_ref.Minv = data_ref.M.inverse();
392 ✗ Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
393 ✗ contact_chol_decomposition.getInverseMassMatrix(Minv_test);
394
395 ✗ BOOST_CHECK(Minv_test.isApprox(data_ref.Minv));
396
397 ✗ ContactCholeskyDecomposition contact_chol_decomposition_mu;
398 ✗ contact_chol_decomposition_mu.allocate(model, contact_models);
399 ✗ contact_chol_decomposition_mu.compute(model, data, contact_models, contact_datas, 0.);
400
401 ✗ BOOST_CHECK(contact_chol_decomposition_mu.D.isApprox(contact_chol_decomposition.D));
402 ✗ BOOST_CHECK(contact_chol_decomposition_mu.Dinv.isApprox(contact_chol_decomposition.Dinv));
403 ✗ BOOST_CHECK(contact_chol_decomposition_mu.U.isApprox(contact_chol_decomposition.U));
404
405 ✗ const double mu = 0.1;
406 ✗ contact_chol_decomposition_mu.compute(model, data, contact_models, contact_datas, mu);
407 ✗ Data::MatrixXs H_mu(H);
408 ✗ H_mu.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu);
409
410 // test damped Operational Space Inertia Matrix
411 {
412 ✗ MatrixXd JMinvJt_mu = H_mu.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse()
413 ✗ * H_mu.middleRows<12>(0).rightCols(model.nv).transpose()
414 ✗ + mu * MatrixXd::Identity(12, 12);
415 ✗ MatrixXd iosim_mu = contact_chol_decomposition_mu.getInverseOperationalSpaceInertiaMatrix();
416 ✗ MatrixXd osim_mu = contact_chol_decomposition_mu.getOperationalSpaceInertiaMatrix();
417
418 ✗ BOOST_CHECK(iosim_mu.isApprox(JMinvJt_mu));
419 ✗ BOOST_CHECK(osim_mu.isApprox(JMinvJt_mu.inverse()));
420
421 ✗ const MatrixXd rhs = MatrixXd::Random(12, 1);
422 ✗ const MatrixXd res = contact_chol_decomposition_mu.getDelassusCholeskyExpression() * rhs;
423 ✗ const MatrixXd res_ref = iosim_mu * rhs;
424
425 ✗ BOOST_CHECK(res_ref.isApprox(res));
426
427 const MatrixXd res_no_mu =
428 ✗ contact_chol_decomposition_mu.getDelassusCholeskyExpression() * rhs - mu * rhs;
429 ✗ const MatrixXd res_no_mu_ref = contact_chol_decomposition.getDelassusCholeskyExpression() * rhs;
430
431 ✗ BOOST_CHECK(res_no_mu.isApprox(res_no_mu_ref));
432 }
433
434 Data::MatrixXs H_recomposed_mu = contact_chol_decomposition_mu.U
435 ✗ * contact_chol_decomposition_mu.D.asDiagonal()
436 ✗ * contact_chol_decomposition_mu.U.transpose();
437
438 ✗ BOOST_CHECK(H_recomposed_mu.isApprox(H_mu));
439
440 // Test basic operation
441 ✗ VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size()));
442 ✗ MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20));
443
444 // Test Uv
445 ✗ VectorXd Uv_op_res(v_in), Uv_op_ref(v_in);
446
447 ✗ contact_chol_decomposition.Uv(Uv_op_res);
448 ✗ Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in;
449
450 ✗ BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref));
451
452 ✗ MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in);
453
454 ✗ contact_chol_decomposition.Uv(Uv_mat_op_res);
455 ✗ Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in;
456
457 ✗ BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref));
458
459 // Test Utv
460 ✗ VectorXd Utv_op_res(v_in), Utv_op_ref(v_in);
461
462 ✗ contact_chol_decomposition.Utv(Utv_op_res);
463 ✗ Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in;
464
465 ✗ BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref));
466
467 ✗ MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in);
468
469 ✗ contact_chol_decomposition.Utv(Utv_mat_op_res);
470 ✗ Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in;
471
472 ✗ BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref));
473
474 // Test Uiv
475 ✗ VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in);
476
477 ✗ contact_chol_decomposition.Uiv(Uiv_op_res);
478 ✗ Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in;
479
480 ✗ BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref));
481
482 ✗ MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in);
483
484 ✗ contact_chol_decomposition.Uiv(Uiv_mat_op_res);
485 ✗ Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in;
486
487 ✗ BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref));
488
489 // Test Utiv
490 ✗ VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in);
491
492 ✗ contact_chol_decomposition.Utiv(Utiv_op_res);
493 ✗ Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in;
494
495 ✗ BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref));
496
497 ✗ MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in);
498
499 ✗ contact_chol_decomposition.Utiv(Utiv_mat_op_res);
500 ✗ Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in;
501
502 ✗ BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref));
503
504 // SolveInPlace
505 ✗ VectorXd sol(v_in);
506 ✗ contact_chol_decomposition.solveInPlace(sol);
507
508 ✗ VectorXd sol_ref(H.inverse() * v_in);
509
510 ✗ BOOST_CHECK(sol.isApprox(sol_ref));
511
512 ✗ MatrixXd sol_mat(mat_in), sol_mat_ref(mat_in);
513
514 ✗ contact_chol_decomposition.solveInPlace(sol_mat);
515 ✗ sol_mat_ref.noalias() = H.inverse() * mat_in;
516
517 ✗ BOOST_CHECK(sol_mat.isApprox(sol_mat_ref));
518
519 // solve
520 ✗ MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in);
521 ✗ BOOST_CHECK(sol_copy_mat.isApprox(sol_mat));
522
523 // inverse
524 ✗ MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
525 ✗ contact_chol_decomposition.inverse(H_inv);
526
527 ✗ MatrixXd H_inv_ref = H.inverse();
528 ✗ BOOST_CHECK(H_inv.isApprox(H_inv_ref));
529
530 // Check matrix
531 ✗ MatrixXd mat1;
532 ✗ contact_chol_decomposition.matrix(mat1);
533 ✗ BOOST_CHECK(mat1.isApprox(H));
534
535 ✗ MatrixXd mat2(constraint_dim + model.nv, constraint_dim + model.nv);
536 ✗ contact_chol_decomposition.matrix(mat2.middleCols(0, constraint_dim + model.nv));
537 ✗ BOOST_CHECK(mat2.isApprox(H));
538
539 ✗ MatrixXd mat3 = contact_chol_decomposition.matrix();
540 ✗ BOOST_CHECK(mat3.isApprox(H));
541 }
542
543 ✗ BOOST_AUTO_TEST_CASE(contact_cholesky_contact3D_6D_LOCAL)
544 {
545 using namespace Eigen;
546 using namespace pinocchio;
547 using namespace pinocchio::cholesky;
548
549 ✗ pinocchio::Model model;
550 ✗ pinocchio::buildModels::humanoidRandom(model, true);
551 ✗ pinocchio::Data data_ref(model);
552
553 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
554 ✗ model.upperPositionLimit.head<3>().fill(1.);
555 ✗ VectorXd q = randomConfiguration(model);
556
557 ✗ const std::string RF = "rleg6_joint";
558 ✗ const std::string LF = "lleg6_joint";
559 ✗ const std::string RA = "rarm6_joint";
560 ✗ const std::string LA = "larm6_joint";
561
562 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
563 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
564 ✗ RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL);
565 ✗ contact_models.push_back(ci_RF);
566 ✗ contact_datas.push_back(RigidConstraintData(ci_RF));
567 ✗ RigidConstraintModel ci_LF(CONTACT_3D, model, model.getJointId(LF), LOCAL);
568 ✗ contact_models.push_back(ci_LF);
569 ✗ contact_datas.push_back(RigidConstraintData(ci_LF));
570 // RigidConstraintModel ci_RA(CONTACT_3D,model.getJointId(RA),LOCAL);
571 // contact_models.push_back(ci_RA);
572 // contact_datas.push_back(RigidConstraintData(ci_RA));
573 // RigidConstraintModel ci_LA(CONTACT_3D,model.getJointId(LA),LOCAL_WORLD_ALIGNED);
574 // contact_models.push_back(ci_LA);
575 // contact_datas.push_back(RigidConstraintData(ci_LA));
576
577 // Compute Mass Matrix
578 ✗ crba(model, data_ref, q, Convention::WORLD);
579 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
580 ✗ data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose();
581
582 // Compute Cholesky decomposition
583 ✗ pinocchio::cholesky::decompose(model, data_ref);
584
585 // Compute Jacobians
586 ✗ Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv);
587 ✗ J_RF.setZero();
588 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
589 ✗ J_LF.setZero();
590 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
591 ✗ J_RA.setZero();
592 ✗ getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA);
593 ✗ J_LA.setZero();
594 ✗ getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL_WORLD_ALIGNED, J_LA);
595
596 ✗ const int constraint_dim = 9;
597 ✗ const int total_dim = model.nv + constraint_dim;
598
599 ✗ Data::MatrixXs H(total_dim, total_dim);
600 ✗ H.setZero();
601 ✗ H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
602 ✗ H.middleRows<6>(0).rightCols(model.nv) = J_RF;
603 ✗ H.middleRows<3>(6).rightCols(model.nv) = J_LF.middleRows<3>(Motion::LINEAR);
604 // H.middleRows<3>(9).rightCols(model.nv) = J_RA.middleRows<3>(Motion::LINEAR);
605 // H.middleRows<3>(12).rightCols(model.nv) = J_LA.middleRows<3>(Motion::LINEAR);
606
607 ✗ H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
608
609 ✗ Data data(model);
610 ✗ crba(model, data, q, Convention::WORLD);
611 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
612 ✗ contact_chol_decomposition.allocate(model, contact_models);
613
614 ✗ ContactCholeskyDecompositionAccessor access(contact_chol_decomposition);
615 ✗ for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
616 {
617 ✗ if (data.parents_fromRow[(size_t)k] == -1)
618 ✗ BOOST_CHECK(access.getParents_fromRow()[k + constraint_dim] == -1);
619 else
620 ✗ BOOST_CHECK(
621 access.getParents_fromRow()[k + constraint_dim]
622 == data.parents_fromRow[(size_t)k] + constraint_dim);
623 }
624
625 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas);
626
627 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
628 ✗ data.M.triangularView<Eigen::StrictlyUpper>().transpose();
629
630 ✗ BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv)));
631 ✗ BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv)));
632 ✗ BOOST_CHECK(
633 data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)));
634
635 Data::MatrixXs M_recomposed =
636 ✗ contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)
637 ✗ * contact_chol_decomposition.D.tail(model.nv).asDiagonal()
638 ✗ * contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose();
639 ✗ BOOST_CHECK(M_recomposed.isApprox(data.M));
640
641 Data::MatrixXs H_recomposed = contact_chol_decomposition.U
642 ✗ * contact_chol_decomposition.D.asDiagonal()
643 ✗ * contact_chol_decomposition.U.transpose();
644
645 ✗ BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
646 ✗ BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv)
647 .isApprox(H.topRightCorner(constraint_dim, model.nv)));
648 ✗ BOOST_CHECK(H_recomposed.isApprox(H));
649
650 // Test basic operation
651 ✗ VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size()));
652 ✗ MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20));
653
654 // Test Uv
655 ✗ VectorXd Uv_op_res(v_in), Uv_op_ref(v_in);
656
657 ✗ contact_chol_decomposition.Uv(Uv_op_res);
658 ✗ Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in;
659
660 ✗ BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref));
661
662 ✗ MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in);
663
664 ✗ contact_chol_decomposition.Uv(Uv_mat_op_res);
665 ✗ Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in;
666
667 ✗ BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref));
668
669 // Test Utv
670 ✗ VectorXd Utv_op_res(v_in), Utv_op_ref(v_in);
671
672 ✗ contact_chol_decomposition.Utv(Utv_op_res);
673 ✗ Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in;
674
675 ✗ BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref));
676
677 ✗ MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in);
678
679 ✗ contact_chol_decomposition.Utv(Utv_mat_op_res);
680 ✗ Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in;
681
682 ✗ BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref));
683
684 // Test Uiv
685 ✗ VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in);
686
687 ✗ contact_chol_decomposition.Uiv(Uiv_op_res);
688 ✗ Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in;
689
690 ✗ BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref));
691
692 ✗ MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in);
693
694 ✗ contact_chol_decomposition.Uiv(Uiv_mat_op_res);
695 ✗ Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in;
696
697 ✗ BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref));
698
699 // Test Utiv
700 ✗ VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in);
701
702 ✗ contact_chol_decomposition.Utiv(Utiv_op_res);
703 ✗ Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in;
704
705 ✗ BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref));
706
707 ✗ MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in);
708
709 ✗ contact_chol_decomposition.Utiv(Utiv_mat_op_res);
710 ✗ Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in;
711
712 ✗ BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref));
713
714 // SolveInPlace
715 ✗ VectorXd sol(v_in);
716 ✗ contact_chol_decomposition.solveInPlace(sol);
717
718 ✗ VectorXd sol_ref(H.inverse() * v_in);
719
720 ✗ BOOST_CHECK(sol.isApprox(sol_ref));
721
722 ✗ MatrixXd sol_mat(mat_in), sol_mat_ref(mat_in);
723
724 ✗ contact_chol_decomposition.solveInPlace(sol_mat);
725 ✗ sol_mat_ref.noalias() = H.inverse() * mat_in;
726
727 ✗ BOOST_CHECK(sol_mat.isApprox(sol_mat_ref));
728
729 // solve
730 ✗ MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in);
731 ✗ BOOST_CHECK(sol_copy_mat.isApprox(sol_mat));
732
733 // inverse
734 ✗ MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
735 ✗ contact_chol_decomposition.inverse(H_inv);
736
737 ✗ MatrixXd H_inv_ref = H.inverse();
738 ✗ BOOST_CHECK(H_inv.isApprox(H_inv_ref));
739
740 // Check matrix
741 ✗ MatrixXd mat1;
742 ✗ contact_chol_decomposition.matrix(mat1);
743 ✗ BOOST_CHECK(mat1.isApprox(H));
744
745 ✗ MatrixXd mat2(constraint_dim + model.nv, constraint_dim + model.nv);
746 ✗ contact_chol_decomposition.matrix(mat2.middleCols(0, constraint_dim + model.nv));
747 ✗ BOOST_CHECK(mat2.isApprox(H));
748
749 ✗ MatrixXd mat3 = contact_chol_decomposition.matrix();
750 ✗ BOOST_CHECK(mat3.isApprox(H));
751
752 // test Operational Space Inertia Matrix
753 ✗ MatrixXd JMinvJt = H.middleRows<9>(0).rightCols(model.nv) * data_ref.M.inverse()
754 ✗ * H.middleRows<9>(0).rightCols(model.nv).transpose();
755 ✗ MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
756 ✗ MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
757
758 ✗ BOOST_CHECK(iosim.isApprox(JMinvJt));
759 ✗ BOOST_CHECK(osim.isApprox(JMinvJt.inverse()));
760
761 // test Mass matrix cholesky
762 ✗ data_ref.Minv = data_ref.M.inverse();
763 ✗ Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
764 ✗ contact_chol_decomposition.getInverseMassMatrix(Minv_test);
765
766 ✗ Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(9, model.nv));
767 ✗ contact_chol_decomposition.getJMinv(JMinv_test);
768 ✗ MatrixXd JMinv_ref = H.middleRows<9>(0).rightCols(model.nv) * data_ref.M.inverse();
769 ✗ BOOST_CHECK(JMinv_ref.isApprox(JMinv_test));
770
771 ✗ BOOST_CHECK(Minv_test.isApprox(data_ref.Minv));
772 }
773
774 ✗ BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_LOCAL_WORLD_ALIGNED)
775 {
776 using namespace Eigen;
777 using namespace pinocchio;
778 using namespace pinocchio::cholesky;
779
780 ✗ pinocchio::Model model;
781 ✗ pinocchio::buildModels::humanoidRandom(model, true);
782 ✗ pinocchio::Data data_ref(model);
783
784 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
785 ✗ model.upperPositionLimit.head<3>().fill(1.);
786 ✗ VectorXd q = randomConfiguration(model);
787
788 ✗ const std::string RF = "rleg6_joint";
789 ✗ const std::string LF = "lleg6_joint";
790
791 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
792 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
793 ✗ RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL_WORLD_ALIGNED);
794 ✗ contact_models.push_back(ci_RF);
795 ✗ contact_datas.push_back(RigidConstraintData(ci_RF));
796 ✗ RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL);
797 ✗ contact_models.push_back(ci_LF);
798 ✗ contact_datas.push_back(RigidConstraintData(ci_LF));
799
800 // Compute Mass Matrix
801 ✗ crba(model, data_ref, q, Convention::WORLD);
802 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
803 ✗ data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose();
804
805 // Compute Cholesky decomposition
806 ✗ pinocchio::cholesky::decompose(model, data_ref);
807
808 // Compute Jacobians
809 ✗ Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv);
810 ✗ J_RF.setZero();
811 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), ci_RF.reference_frame, J_RF);
812 ✗ J_LF.setZero();
813 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), ci_LF.reference_frame, J_LF);
814
815 ✗ const int constraint_dim = 12;
816 ✗ const int total_dim = model.nv + constraint_dim;
817
818 ✗ Data::MatrixXs H(total_dim, total_dim);
819 ✗ H.setZero();
820 ✗ H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
821 ✗ H.middleRows<6>(0).rightCols(model.nv) = J_RF;
822 ✗ H.middleRows<6>(6).rightCols(model.nv) = J_LF;
823
824 ✗ H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
825
826 ✗ Data data(model);
827 ✗ crba(model, data, q, Convention::WORLD);
828 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
829 ✗ contact_chol_decomposition.allocate(model, contact_models);
830 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas);
831
832 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
833 ✗ data.M.triangularView<Eigen::StrictlyUpper>().transpose();
834
835 ✗ Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix();
836
837 ✗ BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
838 ✗ BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv)
839 .isApprox(H.topRightCorner(constraint_dim, model.nv)));
840 ✗ BOOST_CHECK(H_recomposed.isApprox(H));
841
842 // inverse
843 ✗ MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
844 ✗ contact_chol_decomposition.inverse(H_inv);
845
846 ✗ MatrixXd H_inv_ref = H_recomposed.inverse();
847 ✗ BOOST_CHECK(H_inv.isApprox(H_inv_ref));
848
849 // test Operational Space Inertia Matrix
850 ✗ MatrixXd JMinvJt = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse()
851 ✗ * H.middleRows<12>(0).rightCols(model.nv).transpose();
852 ✗ MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
853 ✗ MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
854
855 ✗ BOOST_CHECK(iosim.isApprox(JMinvJt));
856 ✗ BOOST_CHECK(osim.isApprox(JMinvJt.inverse()));
857
858 // test Mass matrix cholesky
859 ✗ data_ref.Minv = data_ref.M.inverse();
860 ✗ Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
861 ✗ contact_chol_decomposition.getInverseMassMatrix(Minv_test);
862
863 ✗ Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(12, model.nv));
864 ✗ contact_chol_decomposition.getJMinv(JMinv_test);
865 ✗ MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse();
866 ✗ BOOST_CHECK(JMinv_ref.isApprox(JMinv_test));
867
868 ✗ BOOST_CHECK(Minv_test.isApprox(data_ref.Minv));
869 }
870
871 ✗ BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_by_joint_2)
872 {
873 using namespace Eigen;
874 using namespace pinocchio;
875 using namespace pinocchio::cholesky;
876
877 ✗ pinocchio::Model model;
878 ✗ pinocchio::buildModels::humanoidRandom(model, true);
879 ✗ pinocchio::Data data_ref(model);
880
881 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
882 ✗ model.upperPositionLimit.head<3>().fill(1.);
883 ✗ VectorXd q = randomConfiguration(model);
884
885 ✗ const std::string RF = "rleg6_joint";
886 ✗ const std::string LF = "lleg6_joint";
887 ✗ const std::string RA = "rarm6_joint";
888 ✗ const std::string LA = "larm6_joint";
889
890 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
891 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
892 ✗ RigidConstraintModel ci_RF(CONTACT_6D, model, 0, model.getJointId(RF), LOCAL);
893 ✗ contact_models.push_back(ci_RF);
894 ✗ contact_datas.push_back(RigidConstraintData(ci_RF));
895 ✗ RigidConstraintModel ci_LF(CONTACT_6D, model, 0, model.getJointId(LF), LOCAL);
896 ✗ contact_models.push_back(ci_LF);
897 ✗ contact_datas.push_back(RigidConstraintData(ci_LF));
898 ✗ RigidConstraintModel ci_RA(CONTACT_6D, model, 0, model.getJointId(RA), LOCAL_WORLD_ALIGNED);
899 ✗ contact_models.push_back(ci_RA);
900 ✗ contact_datas.push_back(RigidConstraintData(ci_RA));
901 ✗ RigidConstraintModel ci_LA(CONTACT_6D, model, 0, model.getJointId(LA), LOCAL);
902 ✗ contact_models.push_back(ci_LA);
903 ✗ contact_datas.push_back(RigidConstraintData(ci_LA));
904
905 // Compute Mass Matrix
906 ✗ crba(model, data_ref, q, Convention::WORLD);
907 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
908 ✗ data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose();
909
910 // Compute Cholesky decomposition
911 ✗ pinocchio::cholesky::decompose(model, data_ref);
912
913 // Compute Jacobians
914 ✗ Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv);
915 ✗ J_RF.setZero();
916 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF);
917 ✗ J_LF.setZero();
918 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF);
919 ✗ J_RA.setZero();
920 ✗ getJointJacobian(model, data_ref, model.getJointId(RA), WORLD, J_RA);
921 ✗ J_LA.setZero();
922 ✗ getJointJacobian(model, data_ref, model.getJointId(LA), WORLD, J_LA);
923
924 ✗ const int constraint_dim = 24;
925 ✗ const int total_dim = model.nv + constraint_dim;
926
927 ✗ const SE3 oMRA_wla = SE3(SE3::Matrix3::Identity(), ci_RA.joint1_placement.translation());
928
929 ✗ const double mu = 0.1;
930 ✗ Data::MatrixXs H(total_dim, total_dim);
931 ✗ H.setZero();
932 ✗ H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu);
933 ✗ H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
934 ✗ H.middleRows<6>(0).rightCols(model.nv) = -ci_RF.joint1_placement.toActionMatrixInverse() * J_RF;
935 ✗ H.middleRows<6>(6).rightCols(model.nv) = -ci_LF.joint1_placement.toActionMatrixInverse() * J_LF;
936 ✗ H.middleRows<6>(12).rightCols(model.nv) = -oMRA_wla.toActionMatrixInverse() * J_RA;
937 ✗ H.middleRows<6>(18).rightCols(model.nv) = -ci_LA.joint1_placement.toActionMatrixInverse() * J_LA;
938
939 ✗ H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
940
941 ✗ Data data(model);
942 ✗ crba(model, data, q, Convention::WORLD);
943 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
944 ✗ contact_chol_decomposition.allocate(model, contact_models);
945 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu);
946
947 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
948 ✗ data.M.triangularView<Eigen::StrictlyUpper>().transpose();
949
950 ✗ Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix();
951
952 ✗ BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
953 ✗ BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv)
954 .isApprox(H.topRightCorner(constraint_dim, model.nv)));
955 ✗ BOOST_CHECK(H_recomposed.isApprox(H));
956
957 ✗ VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size()));
958 ✗ MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20));
959
960 // Test Uv
961 ✗ VectorXd Uv_op_res(v_in), Uv_op_ref(v_in);
962
963 ✗ contact_chol_decomposition.Uv(Uv_op_res);
964 ✗ Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in;
965
966 ✗ BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref));
967
968 ✗ MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in);
969
970 ✗ contact_chol_decomposition.Uv(Uv_mat_op_res);
971 ✗ Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in;
972
973 ✗ BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref));
974
975 // Test Utv
976 ✗ VectorXd Utv_op_res(v_in), Utv_op_ref(v_in);
977
978 ✗ contact_chol_decomposition.Utv(Utv_op_res);
979 ✗ Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in;
980
981 ✗ BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref));
982
983 ✗ MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in);
984
985 ✗ contact_chol_decomposition.Utv(Utv_mat_op_res);
986 ✗ Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in;
987
988 ✗ BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref));
989
990 // Test Uiv
991 ✗ VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in);
992
993 ✗ contact_chol_decomposition.Uiv(Uiv_op_res);
994 ✗ Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in;
995
996 ✗ BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref));
997
998 ✗ MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in);
999
1000 ✗ contact_chol_decomposition.Uiv(Uiv_mat_op_res);
1001 ✗ Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in;
1002
1003 ✗ BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref));
1004
1005 // Test Utiv
1006 ✗ VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in);
1007
1008 ✗ contact_chol_decomposition.Utiv(Utiv_op_res);
1009 ✗ Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in;
1010
1011 ✗ BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref));
1012
1013 ✗ MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in);
1014
1015 ✗ contact_chol_decomposition.Utiv(Utiv_mat_op_res);
1016 ✗ Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in;
1017
1018 ✗ BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref));
1019
1020 // inverse
1021 ✗ MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
1022 ✗ contact_chol_decomposition.inverse(H_inv);
1023
1024 ✗ MatrixXd H_inv2 = contact_chol_decomposition.U.transpose().inverse()
1025 ✗ * contact_chol_decomposition.Dinv.asDiagonal()
1026 ✗ * contact_chol_decomposition.U.inverse();
1027
1028 MatrixXd H_inv3 =
1029 ✗ MatrixXd::Identity(contact_chol_decomposition.size(), contact_chol_decomposition.size());
1030 ✗ contact_chol_decomposition.solveInPlace(H_inv3);
1031
1032 ✗ MatrixXd H_inv_ref = H_recomposed.inverse();
1033 ✗ BOOST_CHECK(H_inv.topLeftCorner(constraint_dim, constraint_dim)
1034 .isApprox(H_inv_ref.topLeftCorner(constraint_dim, constraint_dim)));
1035 ✗ BOOST_CHECK(H_inv.bottomRightCorner(model.nv, model.nv)
1036 .isApprox(H_inv_ref.bottomRightCorner(model.nv, model.nv)));
1037 ✗ BOOST_CHECK(H_inv.topRightCorner(constraint_dim, model.nv)
1038 .isApprox(H_inv_ref.topRightCorner(constraint_dim, model.nv)));
1039
1040 ✗ BOOST_CHECK(H_inv_ref.isApprox(H_inv));
1041 ✗ BOOST_CHECK(H_inv_ref.isApprox(H_inv2));
1042 ✗ BOOST_CHECK(H_inv_ref.isApprox(H_inv3));
1043 ✗ const VectorXd ei = VectorXd::Unit(contact_chol_decomposition.size(), constraint_dim);
1044 ✗ VectorXd ei_inv = ei;
1045 ✗ contact_chol_decomposition.solveInPlace(ei_inv);
1046 ✗ VectorXd ei_inv2 = ei;
1047 ✗ contact_chol_decomposition.Uiv(ei_inv2);
1048 ✗ ei_inv2 = contact_chol_decomposition.Dinv.asDiagonal() * ei_inv2;
1049 ✗ contact_chol_decomposition.Utiv(ei_inv2);
1050
1051 ✗ BOOST_CHECK(ei_inv.isApprox(H_inv_ref * ei));
1052 ✗ BOOST_CHECK(ei_inv2.isApprox(H_inv_ref * ei));
1053 ✗ BOOST_CHECK(ei_inv.isApprox(H_inv * ei));
1054 ✗ BOOST_CHECK(ei_inv2.isApprox(H_inv * ei));
1055
1056 // test Operational Space Inertia Matrix
1057 ✗ MatrixXd JMinvJt = H.middleRows<24>(0).rightCols(model.nv) * data_ref.M.inverse()
1058 ✗ * H.middleRows<24>(0).rightCols(model.nv).transpose()
1059 ✗ + mu * Eigen::MatrixXd::Identity(24, 24);
1060 ✗ MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
1061 ✗ MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
1062
1063 ✗ BOOST_CHECK(iosim.isApprox(JMinvJt));
1064 ✗ BOOST_CHECK(osim.isApprox(JMinvJt.inverse()));
1065
1066 // test Mass matrix cholesky
1067 ✗ data_ref.Minv = data_ref.M.inverse();
1068 ✗ Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
1069 ✗ contact_chol_decomposition.getInverseMassMatrix(Minv_test);
1070
1071 ✗ BOOST_CHECK(Minv_test.isApprox(data_ref.Minv));
1072 ✗ Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(24, model.nv));
1073 ✗ contact_chol_decomposition.getJMinv(JMinv_test);
1074 ✗ MatrixXd JMinv_ref = H.middleRows<24>(0).rightCols(model.nv) * data_ref.M.inverse();
1075 ✗ BOOST_CHECK(JMinv_ref.isApprox(JMinv_test));
1076 }
1077
1078 ✗ BOOST_AUTO_TEST_CASE(contact_cholesky_contact3D_6D_WORLD_by_joint_2)
1079 {
1080 using namespace Eigen;
1081 using namespace pinocchio;
1082 using namespace pinocchio::cholesky;
1083
1084 ✗ pinocchio::Model model;
1085 ✗ pinocchio::buildModels::humanoidRandom(model, true);
1086 ✗ pinocchio::Data data_ref(model);
1087
1088 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
1089 ✗ model.upperPositionLimit.head<3>().fill(1.);
1090 ✗ VectorXd q = randomConfiguration(model);
1091
1092 ✗ const std::string RF = "rleg6_joint";
1093 ✗ const std::string LF = "lleg6_joint";
1094 ✗ const std::string RA = "rarm6_joint";
1095 ✗ const std::string LA = "larm6_joint";
1096
1097 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
1098 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
1099 ✗ RigidConstraintModel ci_RF(CONTACT_6D, model, 0, model.getJointId(RF), LOCAL);
1100 ✗ contact_models.push_back(ci_RF);
1101 ✗ contact_datas.push_back(RigidConstraintData(ci_RF));
1102 ✗ RigidConstraintModel ci_LF(CONTACT_3D, model, 0, model.getJointId(LF), LOCAL);
1103 ✗ contact_models.push_back(ci_LF);
1104 ✗ contact_datas.push_back(RigidConstraintData(ci_LF));
1105 ✗ RigidConstraintModel ci_RA(CONTACT_3D, model, 0, model.getJointId(RA), LOCAL);
1106 ✗ contact_models.push_back(ci_RA);
1107 ✗ contact_datas.push_back(RigidConstraintData(ci_RA));
1108 ✗ RigidConstraintModel ci_LA(CONTACT_3D, model, 0, model.getJointId(LA), LOCAL_WORLD_ALIGNED);
1109 ✗ contact_models.push_back(ci_LA);
1110 ✗ contact_datas.push_back(RigidConstraintData(ci_LA));
1111
1112 // Compute Mass Matrix
1113 ✗ crba(model, data_ref, q, Convention::WORLD);
1114 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
1115 ✗ data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose();
1116
1117 // Compute Cholesky decomposition
1118 ✗ pinocchio::cholesky::decompose(model, data_ref);
1119
1120 // Compute Jacobians
1121 ✗ Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv);
1122 ✗ J_RF.setZero();
1123 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF);
1124 ✗ J_LF.setZero();
1125 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
1126 ✗ J_RA.setZero();
1127 ✗ getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA);
1128 ✗ J_LA.setZero();
1129 ✗ getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL_WORLD_ALIGNED, J_LA);
1130
1131 ✗ const int constraint_dim = 15;
1132 ✗ const int total_dim = model.nv + constraint_dim;
1133
1134 ✗ Data::MatrixXs H(total_dim, total_dim);
1135 ✗ H.setZero();
1136 ✗ H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
1137 ✗ H.middleRows<6>(0).rightCols(model.nv) = -ci_RF.joint1_placement.toActionMatrix() * J_RF;
1138 ✗ H.middleRows<3>(6).rightCols(model.nv) =
1139 ✗ -data_ref.oMi[model.getJointId(LF)].rotation() * J_LF.middleRows<3>(Motion::LINEAR);
1140 ✗ H.middleRows<3>(9).rightCols(model.nv) =
1141 ✗ -data_ref.oMi[model.getJointId(RA)].rotation() * J_RA.middleRows<3>(Motion::LINEAR);
1142 ✗ H.middleRows<3>(12).rightCols(model.nv) = -J_LA.middleRows<3>(Motion::LINEAR);
1143
1144 ✗ H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
1145
1146 ✗ Data data(model);
1147 ✗ crba(model, data, q, Convention::WORLD);
1148 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
1149 ✗ contact_chol_decomposition.allocate(model, contact_models);
1150
1151 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas);
1152
1153 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
1154 ✗ data.M.triangularView<Eigen::StrictlyUpper>().transpose();
1155
1156 ✗ BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv)));
1157 ✗ BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv)));
1158 ✗ BOOST_CHECK(
1159 data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)));
1160
1161 Data::MatrixXs M_recomposed =
1162 ✗ contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv)
1163 ✗ * contact_chol_decomposition.D.tail(model.nv).asDiagonal()
1164 ✗ * contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose();
1165 ✗ BOOST_CHECK(M_recomposed.isApprox(data.M));
1166
1167 Data::MatrixXs H_recomposed = contact_chol_decomposition.U
1168 ✗ * contact_chol_decomposition.D.asDiagonal()
1169 ✗ * contact_chol_decomposition.U.transpose();
1170
1171 ✗ BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
1172 ✗ BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv)
1173 .isApprox(H.topRightCorner(constraint_dim, model.nv)));
1174 ✗ BOOST_CHECK(H_recomposed.isApprox(H));
1175
1176 // test Operational Space Inertia Matrix
1177 ✗ MatrixXd JMinvJt = H.middleRows<15>(0).rightCols(model.nv) * data_ref.M.inverse()
1178 ✗ * H.middleRows<15>(0).rightCols(model.nv).transpose();
1179 ✗ MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
1180 ✗ MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
1181
1182 ✗ BOOST_CHECK(iosim.isApprox(JMinvJt));
1183 ✗ BOOST_CHECK(osim.isApprox(JMinvJt.inverse()));
1184
1185 // test Mass matrix cholesky
1186 ✗ data_ref.Minv = data_ref.M.inverse();
1187 ✗ Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
1188 ✗ contact_chol_decomposition.getInverseMassMatrix(Minv_test);
1189
1190 ✗ BOOST_CHECK(Minv_test.isApprox(data_ref.Minv));
1191 ✗ Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(15, model.nv));
1192 ✗ contact_chol_decomposition.getJMinv(JMinv_test);
1193 ✗ MatrixXd JMinv_ref = H.middleRows<15>(0).rightCols(model.nv) * data_ref.M.inverse();
1194 ✗ BOOST_CHECK(JMinv_ref.isApprox(JMinv_test));
1195 }
1196
1197 ✗ BOOST_AUTO_TEST_CASE(loop_contact_cholesky_contact6D)
1198 {
1199 using namespace Eigen;
1200 using namespace pinocchio;
1201 using namespace pinocchio::cholesky;
1202
1203 ✗ pinocchio::Model model;
1204 ✗ pinocchio::buildModels::humanoidRandom(model, true);
1205 ✗ pinocchio::Data data_ref(model);
1206
1207 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
1208 ✗ model.upperPositionLimit.head<3>().fill(1.);
1209 ✗ VectorXd q = randomConfiguration(model);
1210
1211 ✗ const std::string RF = "rleg6_joint";
1212 ✗ const std::string LF = "lleg6_joint";
1213 ✗ const std::string RA = "rarm6_joint";
1214 ✗ const std::string LA = "larm6_joint";
1215
1216 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
1217 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
1218 RigidConstraintModel loop_RF_LF_local(
1219 ✗ CONTACT_6D, model, model.getJointId(RF), model.getJointId(LF), LOCAL);
1220 RigidConstraintModel loop_RA_LA_lwa(
1221 ✗ CONTACT_6D, model, model.getJointId(RA), model.getJointId(LA), LOCAL_WORLD_ALIGNED);
1222
1223 ✗ loop_RF_LF_local.joint1_placement.setRandom();
1224 ✗ loop_RF_LF_local.joint2_placement.setRandom();
1225
1226 ✗ loop_RA_LA_lwa.joint1_placement.setRandom();
1227 ✗ loop_RA_LA_lwa.joint2_placement.setRandom();
1228
1229 ✗ contact_models.push_back(loop_RF_LF_local);
1230 ✗ contact_datas.push_back(RigidConstraintData(loop_RF_LF_local));
1231
1232 ✗ contact_models.push_back(loop_RA_LA_lwa);
1233 ✗ contact_datas.push_back(RigidConstraintData(loop_RA_LA_lwa));
1234
1235 // Compute Mass Matrix
1236 ✗ crba(model, data_ref, q, Convention::WORLD);
1237 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
1238 ✗ data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose();
1239
1240 // Compute Cholesky decomposition
1241 ✗ const double mu = 0.1;
1242 ✗ pinocchio::cholesky::decompose(model, data_ref);
1243
1244 // Compute Jacobians
1245 ✗ Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA_local(6, model.nv),
1246 ✗ J_LA_local(6, model.nv);
1247 ✗ J_RF.setZero();
1248 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
1249 ✗ Data::Matrix6x J_RF_local = loop_RF_LF_local.joint1_placement.toActionMatrixInverse() * J_RF;
1250 ✗ J_LF.setZero();
1251 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
1252 ✗ Data::Matrix6x J_LF_local = loop_RF_LF_local.joint2_placement.toActionMatrixInverse() * J_LF;
1253 ✗ J_RA_local.setZero();
1254 ✗ getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA_local);
1255 ✗ J_RA_local = loop_RA_LA_lwa.joint1_placement.toActionMatrixInverse() * J_RA_local;
1256 ✗ J_LA_local.setZero();
1257 ✗ getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL, J_LA_local);
1258 ✗ J_LA_local = loop_RA_LA_lwa.joint2_placement.toActionMatrixInverse() * J_LA_local;
1259
1260 ✗ Data::Matrix6x J_RF_world(6, model.nv), J_LF_world(6, model.nv);
1261 ✗ J_RF_world.setZero();
1262 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF_world);
1263 ✗ J_LF_world.setZero();
1264 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF_world);
1265
1266 ✗ forwardKinematics(model, data_ref, q);
1267 const SE3 oM1_loop1 =
1268 ✗ data_ref.oMi[loop_RF_LF_local.joint1_id] * loop_RF_LF_local.joint1_placement;
1269 const SE3 oM2_loop1 =
1270 ✗ data_ref.oMi[loop_RF_LF_local.joint2_id] * loop_RF_LF_local.joint2_placement;
1271 ✗ const SE3 _1M2_loop1 = oM1_loop1.inverse() * oM2_loop1;
1272 ✗ const SE3 oM1_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint1_id] * loop_RA_LA_lwa.joint1_placement;
1273 ✗ const SE3 oM2_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint2_id] * loop_RA_LA_lwa.joint2_placement;
1274 ✗ const SE3 _1M2_loop2 = oM1_loop2.inverse() * oM2_loop2;
1275
1276 ✗ const int constraint_dim = 12;
1277 ✗ const int total_dim = model.nv + constraint_dim;
1278
1279 ✗ Data::MatrixXs H(total_dim, total_dim);
1280 ✗ H.setZero();
1281 ✗ H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu);
1282 ✗ H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
1283 ✗ H.middleRows<6>(0).rightCols(model.nv) = J_RF_local - _1M2_loop1.toActionMatrix() * J_LF_local;
1284 ✗ const SE3 oM1_loop2_lwa = SE3(oM1_loop2.rotation(), SE3::Vector3::Zero());
1285 ✗ H.middleRows<6>(6).rightCols(model.nv) =
1286 ✗ oM1_loop2_lwa.toActionMatrix() * J_RA_local
1287 ✗ - (oM1_loop2_lwa.toActionMatrix() * _1M2_loop2.toActionMatrix()) * J_LA_local;
1288
1289 ✗ H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
1290
1291 ✗ Data data(model);
1292 ✗ crba(model, data, q, Convention::WORLD);
1293 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
1294 ✗ contact_chol_decomposition.allocate(model, contact_models);
1295 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu);
1296
1297 ✗ Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix();
1298
1299 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
1300 ✗ data.M.triangularView<Eigen::StrictlyUpper>().transpose();
1301 ✗ BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
1302 ✗ BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv)
1303 .isApprox(H.topRightCorner(constraint_dim, model.nv)));
1304 ✗ BOOST_CHECK(H_recomposed.isApprox(H));
1305
1306 // inverse
1307 ✗ MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
1308 ✗ contact_chol_decomposition.inverse(H_inv);
1309
1310 ✗ MatrixXd H_inv_ref = H_recomposed.inverse();
1311
1312 ✗ BOOST_CHECK(H_inv_ref.isApprox(H_inv));
1313
1314 // test Operational Space Inertia Matrix
1315 ✗ MatrixXd JMinvJt = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse()
1316 ✗ * H.middleRows<12>(0).rightCols(model.nv).transpose()
1317 ✗ + mu * Eigen::MatrixXd::Identity(12, 12);
1318 ✗ MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
1319 ✗ MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
1320
1321 ✗ BOOST_CHECK(iosim.isApprox(JMinvJt));
1322 ✗ BOOST_CHECK(osim.isApprox(JMinvJt.inverse()));
1323
1324 // test Mass matrix cholesky
1325 ✗ data_ref.Minv = data_ref.M.inverse();
1326 ✗ Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
1327 ✗ contact_chol_decomposition.getInverseMassMatrix(Minv_test);
1328
1329 ✗ BOOST_CHECK(Minv_test.isApprox(data_ref.Minv));
1330 ✗ Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(12, model.nv));
1331 ✗ contact_chol_decomposition.getJMinv(JMinv_test);
1332 ✗ MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse();
1333 ✗ BOOST_CHECK(JMinv_ref.isApprox(JMinv_test));
1334 }
1335
1336 ✗ BOOST_AUTO_TEST_CASE(loop_contact_cholesky_contact_3d)
1337 {
1338 using namespace Eigen;
1339 using namespace pinocchio;
1340 using namespace pinocchio::cholesky;
1341
1342 ✗ pinocchio::Model model;
1343 ✗ pinocchio::buildModels::humanoidRandom(model, true);
1344 ✗ pinocchio::Data data_ref(model);
1345
1346 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
1347 ✗ model.upperPositionLimit.head<3>().fill(1.);
1348 ✗ VectorXd q = randomConfiguration(model);
1349
1350 ✗ const std::string RF = "rleg6_joint";
1351 ✗ const std::string LF = "lleg6_joint";
1352 ✗ const std::string RA = "rarm6_joint";
1353 ✗ const std::string LA = "larm6_joint";
1354
1355 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
1356 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
1357 RigidConstraintModel loop_RF_LF_local(
1358 ✗ CONTACT_3D, model, model.getJointId(RF), model.getJointId(LF), LOCAL);
1359 RigidConstraintModel loop_RA_LA_lwa(
1360 ✗ CONTACT_3D, model, model.getJointId(RA), model.getJointId(LA), LOCAL_WORLD_ALIGNED);
1361
1362 ✗ loop_RF_LF_local.joint1_placement.setRandom();
1363 ✗ loop_RF_LF_local.joint2_placement.setRandom();
1364
1365 ✗ loop_RA_LA_lwa.joint1_placement.setRandom();
1366 ✗ loop_RA_LA_lwa.joint2_placement.setRandom();
1367
1368 ✗ contact_models.push_back(loop_RF_LF_local);
1369 ✗ contact_datas.push_back(RigidConstraintData(loop_RF_LF_local));
1370
1371 ✗ contact_models.push_back(loop_RA_LA_lwa);
1372 ✗ contact_datas.push_back(RigidConstraintData(loop_RA_LA_lwa));
1373
1374 // Compute Mass Matrix
1375 ✗ crba(model, data_ref, q, Convention::WORLD);
1376 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
1377 ✗ data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose();
1378
1379 // Compute Cholesky decomposition
1380 ✗ const double mu = 0.1;
1381 ✗ pinocchio::cholesky::decompose(model, data_ref);
1382
1383 // Compute Jacobians
1384 ✗ Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA_local(6, model.nv),
1385 ✗ J_LA_local(6, model.nv);
1386 ✗ J_RF.setZero();
1387 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF);
1388 const Data::Matrix6x J_RF_local =
1389 ✗ loop_RF_LF_local.joint1_placement.toActionMatrixInverse() * J_RF;
1390 ✗ J_LF.setZero();
1391 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF);
1392 const Data::Matrix6x J_LF_local =
1393 ✗ loop_RF_LF_local.joint2_placement.toActionMatrixInverse() * J_LF;
1394 ✗ J_RA_local.setZero();
1395 ✗ getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA_local);
1396 ✗ J_RA_local = loop_RA_LA_lwa.joint1_placement.toActionMatrixInverse() * J_RA_local;
1397 ✗ J_LA_local.setZero();
1398 ✗ getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL, J_LA_local);
1399 ✗ J_LA_local = loop_RA_LA_lwa.joint2_placement.toActionMatrixInverse() * J_LA_local;
1400
1401 ✗ Data::Matrix6x J_RF_world(6, model.nv), J_LF_world(6, model.nv);
1402 ✗ J_RF_world.setZero();
1403 ✗ getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF_world);
1404 ✗ J_LF_world.setZero();
1405 ✗ getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF_world);
1406
1407 ✗ forwardKinematics(model, data_ref, q);
1408 const SE3 oM1_loop1 =
1409 ✗ data_ref.oMi[loop_RF_LF_local.joint1_id] * loop_RF_LF_local.joint1_placement;
1410 const SE3 oM2_loop1 =
1411 ✗ data_ref.oMi[loop_RF_LF_local.joint2_id] * loop_RF_LF_local.joint2_placement;
1412 ✗ const SE3 _1M2_loop1 = oM1_loop1.inverse() * oM2_loop1;
1413 ✗ const SE3 oM1_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint1_id] * loop_RA_LA_lwa.joint1_placement;
1414 ✗ const SE3 oM2_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint2_id] * loop_RA_LA_lwa.joint2_placement;
1415 ✗ const SE3 _1M2_loop2 = oM1_loop2.inverse() * oM2_loop2;
1416
1417 ✗ const int constraint_dim = 6;
1418 ✗ const int total_dim = model.nv + constraint_dim;
1419
1420 ✗ Data::MatrixXs H(total_dim, total_dim);
1421 ✗ H.setZero();
1422 ✗ H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu);
1423 ✗ H.bottomRightCorner(model.nv, model.nv) = data_ref.M;
1424 ✗ H.middleRows<3>(0).rightCols(model.nv) =
1425 ✗ J_RF_local.middleRows<3>(Motion::LINEAR)
1426 ✗ - _1M2_loop1.rotation() * J_LF_local.middleRows<3>(Motion::LINEAR);
1427 ✗ const SE3 oM1_loop2_lwa = SE3(oM1_loop2.rotation(), SE3::Vector3::Zero());
1428 ✗ const SE3 oM2_loop2_lwa = SE3(oM2_loop2.rotation(), SE3::Vector3::Zero());
1429 ✗ H.middleRows<3>(3).rightCols(model.nv) =
1430 ✗ (oM1_loop2_lwa.toActionMatrix() * J_RA_local - (oM2_loop2_lwa.toActionMatrix()) * J_LA_local)
1431 ✗ .middleRows<3>(Motion::LINEAR);
1432
1433 ✗ H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose();
1434
1435 ✗ Data data(model);
1436 ✗ crba(model, data, q, Convention::WORLD);
1437 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
1438 ✗ contact_chol_decomposition.allocate(model, contact_models);
1439 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu);
1440 ✗ BOOST_CHECK(contact_datas[0].c1Mc2.isApprox(_1M2_loop1));
1441 ✗ BOOST_CHECK(contact_datas[1].c1Mc2.isApprox(_1M2_loop2));
1442
1443 ✗ Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix();
1444
1445 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
1446 ✗ data.M.triangularView<Eigen::StrictlyUpper>().transpose();
1447 ✗ BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M));
1448 ✗ BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv)
1449 .isApprox(H.topRightCorner(constraint_dim, model.nv)));
1450 ✗ BOOST_CHECK(H_recomposed.isApprox(H));
1451
1452 // std::cout << "H_recomposed.topRightCorner(constraint_dim,model.nv):\n" <<
1453 // H_recomposed.topRightCorner(constraint_dim,model.nv) << std::endl; std::cout <<
1454 // "H.topRightCorner(constraint_dim,model.nv):\n" << H .topRightCorner(constraint_dim,model.nv)
1455 // << std::endl;
1456
1457 // inverse
1458 ✗ MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size());
1459 ✗ contact_chol_decomposition.inverse(H_inv);
1460
1461 ✗ MatrixXd H_inv_ref = H_recomposed.inverse();
1462
1463 ✗ BOOST_CHECK(H_inv_ref.isApprox(H_inv));
1464
1465 // test Operational Space Inertia Matrix
1466 ✗ MatrixXd JMinvJt = H.middleRows<6>(0).rightCols(model.nv) * data_ref.M.inverse()
1467 ✗ * H.middleRows<6>(0).rightCols(model.nv).transpose()
1468 ✗ + mu * Eigen::MatrixXd::Identity(6, 6);
1469 ✗ MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix();
1470 ✗ MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix();
1471
1472 ✗ BOOST_CHECK(iosim.isApprox(JMinvJt));
1473 ✗ BOOST_CHECK(osim.isApprox(JMinvJt.inverse()));
1474
1475 // test Mass matrix cholesky
1476 ✗ data_ref.Minv = data_ref.M.inverse();
1477 ✗ Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv));
1478 ✗ contact_chol_decomposition.getInverseMassMatrix(Minv_test);
1479
1480 ✗ Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(6, model.nv));
1481 ✗ contact_chol_decomposition.getJMinv(JMinv_test);
1482 ✗ MatrixXd JMinv_ref = H.middleRows<6>(0).rightCols(model.nv) * data_ref.M.inverse();
1483 ✗ BOOST_CHECK(JMinv_ref.isApprox(JMinv_test));
1484 ✗ BOOST_CHECK(Minv_test.isApprox(data_ref.Minv));
1485 }
1486
1487 ✗ BOOST_AUTO_TEST_CASE(contact_cholesky_updateDamping)
1488 {
1489 using namespace Eigen;
1490 using namespace pinocchio;
1491 using namespace pinocchio::cholesky;
1492
1493 ✗ pinocchio::Model model;
1494 ✗ pinocchio::buildModels::humanoidRandom(model, true);
1495 ✗ pinocchio::Data data_ref(model);
1496
1497 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
1498 ✗ model.upperPositionLimit.head<3>().fill(1.);
1499 ✗ VectorXd q = randomConfiguration(model);
1500
1501 ✗ const std::string RF = "rleg6_joint";
1502 ✗ const std::string LF = "lleg6_joint";
1503
1504 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
1505 ✗ PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
1506 ✗ RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL);
1507 ✗ contact_models.push_back(ci_RF);
1508 ✗ contact_datas.push_back(RigidConstraintData(ci_RF));
1509 ✗ RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL);
1510 ✗ contact_models.push_back(ci_LF);
1511 ✗ contact_datas.push_back(RigidConstraintData(ci_LF));
1512
1513 ✗ Data data(model);
1514 ✗ crba(model, data, q, Convention::WORLD);
1515
1516 ✗ const double mu1 = 1e-2, mu2 = 1e-10;
1517
1518 {
1519 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
1520 ✗ contact_chol_decomposition.allocate(model, contact_models);
1521 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu1);
1522 ✗ contact_chol_decomposition.updateDamping(mu2);
1523
1524 ✗ ContactCholeskyDecomposition contact_chol_decomposition_ref;
1525 ✗ contact_chol_decomposition_ref.allocate(model, contact_models);
1526 ✗ contact_chol_decomposition_ref.compute(model, data, contact_models, contact_datas, mu2);
1527
1528 ✗ BOOST_CHECK(contact_chol_decomposition.D.isApprox(contact_chol_decomposition_ref.D));
1529 ✗ BOOST_CHECK(contact_chol_decomposition.Dinv.isApprox(contact_chol_decomposition_ref.Dinv));
1530 ✗ BOOST_CHECK(contact_chol_decomposition.U.isApprox(contact_chol_decomposition_ref.U));
1531 }
1532
1533 {
1534 ✗ ContactCholeskyDecomposition contact_chol_decomposition;
1535 ✗ contact_chol_decomposition.allocate(model, contact_models);
1536 ✗ contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu1);
1537 ✗ contact_chol_decomposition.getDelassusCholeskyExpression().updateDamping(mu2);
1538
1539 ✗ ContactCholeskyDecomposition contact_chol_decomposition_ref;
1540 ✗ contact_chol_decomposition_ref.allocate(model, contact_models);
1541 ✗ contact_chol_decomposition_ref.compute(model, data, contact_models, contact_datas, mu2);
1542
1543 ✗ BOOST_CHECK(contact_chol_decomposition.D.isApprox(contact_chol_decomposition_ref.D));
1544 ✗ BOOST_CHECK(contact_chol_decomposition.Dinv.isApprox(contact_chol_decomposition_ref.Dinv));
1545 ✗ BOOST_CHECK(contact_chol_decomposition.U.isApprox(contact_chol_decomposition_ref.U));
1546 }
1547 }
1548
1549 BOOST_AUTO_TEST_SUITE_END()
1550