unittest/contact-cholesky.cpp
Go to the documentation of this file.
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()
pinocchio::RigidConstraintData
RigidConstraintDataTpl< context::Scalar, context::Options > RigidConstraintData
Definition: algorithm/fwd.hpp:27
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:199
pinocchio::DataTpl::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: multibody/data.hpp:74
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
simulation-contact-dynamics.sol
sol
Definition: simulation-contact-dynamics.py:113
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(contact_operator_equal)
Definition: unittest/contact-cholesky.cpp:63
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::cholesky::Uv
Mat & Uv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the sparse multiplication using the Cholesky decomposition stored in data and acting in plac...
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:149
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: multibody/model.hpp:170
pinocchio::SE3Tpl< context::Scalar, context::Options >
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl
Definition: unittest/contact-cholesky.cpp:23
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::buildModels::manipulator
void manipulator(ModelTpl< Scalar, Options, JointCollectionTpl > &model)
Create a 6-DOF kinematic chain shoulder-elbow-wrist.
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::Base
ContactCholeskyDecompositionTpl< Scalar, Options > Base
Definition: unittest/contact-cholesky.cpp:26
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
pinocchio::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition: algorithm/contact-info.hpp:22
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::getJoint2_indexes
const std::vector< BooleanVector > & getJoint2_indexes() const
Definition: unittest/contact-cholesky.cpp:51
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::BooleanVector
Base::BooleanVector BooleanVector
Definition: unittest/contact-cholesky.cpp:28
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
simulation-contact-dynamics.contact_datas
list contact_datas
Definition: simulation-contact-dynamics.py:60
res
res
joint-configuration.hpp
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::IndexVector
Base::IndexVector IndexVector
Definition: unittest/contact-cholesky.cpp:27
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::getLastChild
const IndexVector & getLastChild() const
Definition: unittest/contact-cholesky.cpp:39
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
pinocchio::DataTpl::Minv
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:202
pinocchio::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
pinocchio::cholesky::Uiv
Mat & Uiv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the pivot inversion using the Cholesky decomposition stored in data and acting in place.
pinocchio::cholesky
Cholesky decompositions.
q
q
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
pinocchio::DataTpl::Dinv
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: multibody/data.hpp:336
pinocchio::cholesky::ContactCholeskyDecompositionAccessor
ContactCholeskyDecompositionAccessorTpl< double, 0 > ContactCholeskyDecompositionAccessor
Definition: unittest/contact-cholesky.cpp:57
cholesky.hpp
pinocchio::cholesky::decompose
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & decompose(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the Cholesky decomposition of the joint space inertia matrix M contained in data.
dcrba.H
def H
Definition: dcrba.py:425
mu
double mu
Definition: delassus.cpp:58
fill
fill
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::ContactCholeskyDecompositionAccessorTpl
ContactCholeskyDecompositionAccessorTpl(const Base &other)
Definition: unittest/contact-cholesky.cpp:30
pinocchio::DataTpl::U
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
Definition: multibody/data.hpp:329
pinocchio::DataTpl::D
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: multibody/data.hpp:332
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:22
contact-info.hpp
pinocchio::ContactCholeskyDecompositionTpl
Definition: algorithm/fwd.hpp:17
pinocchio::python::context::SE3
SE3Tpl< Scalar, Options > SE3
Definition: bindings/python/context/generic.hpp:53
anymal-simulation.constraint_dim
constraint_dim
Definition: anymal-simulation.py:52
sample-models.hpp
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::getJoint1_indexes
const std::vector< BooleanVector > & getJoint1_indexes() const
Definition: unittest/contact-cholesky.cpp:47
pinocchio::cholesky::Utv
Mat & Utv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the sparse multiplication using the Cholesky decomposition stored in data and acting in plac...
crba.hpp
pinocchio::CONTACT_3D
@ CONTACT_3D
Definition: algorithm/contact-info.hpp:21
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: multibody/model.hpp:173
pinocchio::cholesky::Utiv
Mat & Utiv(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Mat > &v)
Perform the pivot inversion using the Cholesky decomposition stored in data and acting in place.
simulation-closed-kinematic-chains.rhs
rhs
Definition: simulation-closed-kinematic-chains.py:138
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::getParents_fromRow
const IndexVector & getParents_fromRow() const
Definition: unittest/contact-cholesky.cpp:35
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::cholesky::ContactCholeskyDecompositionAccessorTpl::getNvSubtree_fromRow
const IndexVector & getNvSubtree_fromRow() const
Definition: unittest/contact-cholesky.cpp:43
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:28