delassus.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2023-2024 INRIA CNRS
3 // Copyright (c) 2023 KU Leuven
4 //
5 
6 #include "pinocchio/algorithm/contact-info.hpp"
7 #include "pinocchio/algorithm/contact-dynamics.hpp"
8 #include "pinocchio/algorithm/contact-cholesky.hxx"
9 #include "pinocchio/algorithm/joint-configuration.hpp"
10 #include "pinocchio/multibody/sample-models.hpp"
11 #include "pinocchio/algorithm/delassus.hpp"
12 
13 #include <boost/test/unit_test.hpp>
14 #include <boost/utility/binary.hpp>
15 
16 namespace pinocchio
17 {
18  template<typename Scalar, int Options>
19  struct ContactCholeskyDecompositionAccessorTpl
20  : public ContactCholeskyDecompositionTpl<Scalar, Options>
21  {
22  typedef ContactCholeskyDecompositionTpl<Scalar, Options> Base;
23  typedef typename Base::IndexVector IndexVector;
24  typedef typename Base::BooleanVector BooleanVector;
25 
26  ContactCholeskyDecompositionAccessorTpl(const Base & other)
27  : Base(other)
28  {
29  }
30 
31  const IndexVector & getParents_fromRow() const
32  {
33  return this->parents_fromRow;
34  }
35  const IndexVector & getLastChild() const
36  {
37  return this->last_child;
38  }
39  const IndexVector & getNvSubtree_fromRow() const
40  {
41  return this->nv_subtree_fromRow;
42  }
43  const std::vector<BooleanVector> & getJoint1_indexes() const
44  {
45  return this->joint1_indexes;
46  }
47  const std::vector<BooleanVector> & getJoint2_indexes() const
48  {
49  return this->joint2_indexes;
50  }
51  };
52 
53  typedef ContactCholeskyDecompositionAccessorTpl<double, 0> ContactCholeskyDecompositionAccessor;
54 } // namespace pinocchio
55 
56 using namespace pinocchio;
57 
58 double mu = 1e-4;
59 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
60 
61 BOOST_AUTO_TEST_CASE(contact_6D)
62 {
63  using namespace Eigen;
64  pinocchio::Model model;
65  pinocchio::buildModels::humanoidRandom(model, true);
66  pinocchio::Data data(model);
67 
68  const std::string RA = "rleg6_joint";
69  RigidConstraintModel ci_RA_6D(CONTACT_6D, model, model.getJointId(RA), LOCAL);
70  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
71  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
72  contact_models.push_back(ci_RA_6D);
73  contact_data.push_back(RigidConstraintData(ci_RA_6D));
74 
75  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
76 
77  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
78  VectorXd q = randomConfiguration(model);
79  VectorXd v = Eigen::VectorXd::Random(model.nv);
80 
81  initPvDelassus(model, data, contact_models); // Allocate memory
82 
83  for (int i = 0; i < 10; i++)
84  {
85  computeAllTerms(model, data, q, v);
86  contact_chol.compute(model, data, contact_models, contact_data, mu);
87  contact_chol.inverse(H_inverse);
88 
89  Eigen::MatrixXd dampedDelassusInverse;
90  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
91 
92  Eigen::MatrixXd dampedDelassusInverse2;
93  dampedDelassusInverse2.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
94 
95  dampedDelassusInverse2 =
96  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim());
97  computeDampedDelassusMatrixInverse(
98  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
99  dampedDelassusInverse.triangularView<StrictlyLower>() =
100  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
101  BOOST_CHECK(dampedDelassusInverse2.isApprox(dampedDelassusInverse, 1e-10));
102 
103  computeDampedDelassusMatrixInverse(
104  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
105  dampedDelassusInverse.triangularView<StrictlyLower>() =
106  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
107  BOOST_CHECK(dampedDelassusInverse.isApprox(
108  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-10));
109 
110  q = randomConfiguration(model);
111  v = Eigen::VectorXd::Random(model.nv);
112  }
113 }
114 
115 BOOST_AUTO_TEST_CASE(contact_6D6D)
116 {
117  using namespace Eigen;
118  pinocchio::Model model;
119  pinocchio::buildModels::humanoidRandom(model, true);
120  pinocchio::Data data(model);
121 
122  const std::string RA = "rleg6_joint";
123  const std::string LA = "lleg6_joint";
124  RigidConstraintModel ci_LA_6D(CONTACT_6D, model, model.getJointId(LA), LOCAL);
125  RigidConstraintModel ci_RA_6D(CONTACT_6D, model, model.getJointId(RA), LOCAL);
126  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
127  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
128  contact_models.push_back(ci_RA_6D);
129  contact_data.push_back(RigidConstraintData(ci_RA_6D));
130  contact_models.push_back(ci_LA_6D);
131  contact_data.push_back(RigidConstraintData(ci_LA_6D));
132 
133  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
134 
135  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
136  VectorXd q = randomConfiguration(model);
137  VectorXd v = Eigen::VectorXd::Random(model.nv);
138 
139  computeAllTerms(model, data, q, v);
140  contact_chol.compute(model, data, contact_models, contact_data, mu);
141  contact_chol.inverse(H_inverse);
142 
143  Data::MatrixXs dampedDelassusInverse;
144  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
145 
146  initPvDelassus(model, data, contact_models); // Allocate memory
147  computeDampedDelassusMatrixInverse(
148  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
149  dampedDelassusInverse.triangularView<StrictlyLower>() =
150  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
151  BOOST_CHECK(dampedDelassusInverse.isApprox(
152  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
153 
154  computeDampedDelassusMatrixInverse(
155  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
156  dampedDelassusInverse.triangularView<StrictlyLower>() =
157  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
158  BOOST_CHECK(dampedDelassusInverse.isApprox(
159  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
160 }
161 
162 BOOST_AUTO_TEST_CASE(contact_6D4)
163 {
164  using namespace Eigen;
165  pinocchio::Model model;
166  pinocchio::buildModels::humanoidRandom(model, true);
167  pinocchio::Data data(model);
168 
169  const std::string RF = "rleg6_joint";
170  const std::string LF = "lleg6_joint";
171  const std::string RA = "rarm6_joint";
172  const std::string LA = "larm6_joint";
173  RigidConstraintModel ci_LA_6D(CONTACT_6D, model, model.getJointId(LA), LOCAL);
174  RigidConstraintModel ci_RA_6D(CONTACT_6D, model, model.getJointId(RA), LOCAL);
175  RigidConstraintModel ci_LF_6D(CONTACT_6D, model, model.getJointId(LF), LOCAL);
176  RigidConstraintModel ci_RF_6D(CONTACT_6D, model, model.getJointId(RF), LOCAL);
177  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
178  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
179  contact_models.push_back(ci_RA_6D);
180  contact_data.push_back(RigidConstraintData(ci_RA_6D));
181  contact_models.push_back(ci_LA_6D);
182  contact_data.push_back(RigidConstraintData(ci_LA_6D));
183  contact_models.push_back(ci_RF_6D);
184  contact_data.push_back(RigidConstraintData(ci_RF_6D));
185  contact_models.push_back(ci_LF_6D);
186  contact_data.push_back(RigidConstraintData(ci_LF_6D));
187 
188  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
189 
190  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
191  VectorXd q = randomConfiguration(model);
192  VectorXd v = Eigen::VectorXd::Random(model.nv);
193 
194  computeAllTerms(model, data, q, v);
195  contact_chol.compute(model, data, contact_models, contact_data, mu);
196  contact_chol.inverse(H_inverse);
197 
198  Data::MatrixXs dampedDelassusInverse;
199  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
200 
201  initPvDelassus(model, data, contact_models); // Allocate memory
202  computeDampedDelassusMatrixInverse(
203  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
204  dampedDelassusInverse.triangularView<StrictlyLower>() =
205  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
206  BOOST_CHECK(dampedDelassusInverse.isApprox(
207  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
208 
209  computeDampedDelassusMatrixInverse(
210  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
211  dampedDelassusInverse.triangularView<StrictlyLower>() =
212  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
213  BOOST_CHECK(dampedDelassusInverse.isApprox(
214  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-10));
215 }
216 
217 BOOST_AUTO_TEST_CASE(contact_6D_repeated)
218 {
219  using namespace Eigen;
220  pinocchio::Model model;
221  pinocchio::buildModels::humanoidRandom(model, true);
222  pinocchio::Data data(model);
223 
224  const std::string RA = "rleg6_joint";
225  RigidConstraintModel ci_RA_6D(CONTACT_6D, model, model.getJointId(RA), LOCAL);
226  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
227  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
228  contact_models.push_back(ci_RA_6D);
229  contact_data.push_back(RigidConstraintData(ci_RA_6D));
230  contact_models.push_back(ci_RA_6D);
231  contact_data.push_back(RigidConstraintData(ci_RA_6D));
232 
233  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
234 
235  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
236  VectorXd q = randomConfiguration(model);
237  VectorXd v = Eigen::VectorXd::Random(model.nv);
238 
239  computeAllTerms(model, data, q, v);
240  contact_chol.compute(model, data, contact_models, contact_data, mu);
241  contact_chol.inverse(H_inverse);
242 
243  Data::MatrixXs dampedDelassusInverse;
244  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
245 
246  initPvDelassus(model, data, contact_models); // Allocate memory
247  computeDampedDelassusMatrixInverse(
248  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
249  dampedDelassusInverse.triangularView<StrictlyLower>() =
250  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
251  BOOST_CHECK(dampedDelassusInverse.isApprox(
252  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
253 
254  computeDampedDelassusMatrixInverse(
255  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
256  dampedDelassusInverse.triangularView<StrictlyLower>() =
257  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
258  BOOST_CHECK(dampedDelassusInverse.isApprox(
259  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
260 }
261 
262 BOOST_AUTO_TEST_CASE(contact_6D_repeated_6D3)
263 {
264  using namespace Eigen;
265  pinocchio::Model model;
266  pinocchio::buildModels::humanoidRandom(model, true);
267  pinocchio::Data data(model);
268 
269  const std::string RF = "rleg6_joint";
270  const std::string LF = "lleg6_joint";
271  const std::string RA = "rarm6_joint";
272  const std::string LA = "larm6_joint";
273  RigidConstraintModel ci_LA_6D(CONTACT_6D, model, model.getJointId(LA), LOCAL);
274  RigidConstraintModel ci_RA_6D(CONTACT_6D, model, model.getJointId(RA), LOCAL);
275  RigidConstraintModel ci_LF_6D(CONTACT_6D, model, model.getJointId(LF), LOCAL);
276  RigidConstraintModel ci_RF_6D(CONTACT_6D, model, model.getJointId(RF), LOCAL);
277  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
278  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
279  contact_models.push_back(ci_RA_6D);
280  contact_data.push_back(RigidConstraintData(ci_RA_6D));
281  contact_models.push_back(ci_RA_6D);
282  contact_data.push_back(RigidConstraintData(ci_RA_6D));
283 
284  contact_models.push_back(ci_LA_6D);
285  contact_data.push_back(RigidConstraintData(ci_LA_6D));
286 
287  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
288 
289  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
290  VectorXd q = randomConfiguration(model);
291  VectorXd v = Eigen::VectorXd::Random(model.nv);
292 
293  computeAllTerms(model, data, q, v);
294  contact_chol.compute(model, data, contact_models, contact_data, mu);
295  contact_chol.inverse(H_inverse);
296 
297  Data::MatrixXs dampedDelassusInverse;
298  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
299 
300  initPvDelassus(model, data, contact_models); // Allocate memory
301  computeDampedDelassusMatrixInverse(
302  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
303  dampedDelassusInverse.triangularView<StrictlyLower>() =
304  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
305  BOOST_CHECK(dampedDelassusInverse.isApprox(
306  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
307 
308  computeDampedDelassusMatrixInverse(
309  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
310  dampedDelassusInverse.triangularView<StrictlyLower>() =
311  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
312  BOOST_CHECK(dampedDelassusInverse.isApprox(
313  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
314 }
315 
316 BOOST_AUTO_TEST_CASE(contact_3D)
317 {
318  using namespace Eigen;
319  pinocchio::Model model;
320  pinocchio::buildModels::humanoidRandom(model, true);
321  pinocchio::Data data(model);
322 
323  const std::string RA = "rleg6_joint";
324  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
325  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
326  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
327  contact_models.push_back(ci_RA_3D);
328  contact_data.push_back(RigidConstraintData(ci_RA_3D));
329 
330  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
331 
332  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
333  VectorXd q = randomConfiguration(model);
334  VectorXd v = Eigen::VectorXd::Random(model.nv);
335 
336  computeAllTerms(model, data, q, v);
337  contact_chol.compute(model, data, contact_models, contact_data, mu);
338  contact_chol.inverse(H_inverse);
339 
340  Data::MatrixXs dampedDelassusInverse;
341  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
342 
343  initPvDelassus(model, data, contact_models); // Allocate memory
344  computeDampedDelassusMatrixInverse(
345  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
346  dampedDelassusInverse.triangularView<StrictlyLower>() =
347  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
348  BOOST_CHECK(dampedDelassusInverse.isApprox(
349  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
350 
351  computeDampedDelassusMatrixInverse(
352  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
353  dampedDelassusInverse.triangularView<StrictlyLower>() =
354  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
355  BOOST_CHECK(dampedDelassusInverse.isApprox(
356  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-7));
357 }
358 
359 BOOST_AUTO_TEST_CASE(contact_3D3D)
360 {
361  using namespace Eigen;
362  pinocchio::Model model;
363  pinocchio::buildModels::humanoidRandom(model, true);
364  pinocchio::Data data(model);
365 
366  const std::string RA = "rleg6_joint";
367  const std::string LA = "lleg6_joint";
368  RigidConstraintModel ci_LA_3D(CONTACT_3D, model, model.getJointId(LA), LOCAL);
369  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
370  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
371  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
372  contact_models.push_back(ci_RA_3D);
373  contact_data.push_back(RigidConstraintData(ci_RA_3D));
374  contact_models.push_back(ci_LA_3D);
375  contact_data.push_back(RigidConstraintData(ci_LA_3D));
376 
377  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
378 
379  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
380  VectorXd q = randomConfiguration(model);
381  VectorXd v = Eigen::VectorXd::Random(model.nv);
382 
383  computeAllTerms(model, data, q, v);
384  contact_chol.compute(model, data, contact_models, contact_data, mu);
385  contact_chol.inverse(H_inverse);
386 
387  Data::MatrixXs dampedDelassusInverse;
388  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
389 
390  initPvDelassus(model, data, contact_models); // Allocate memory
391  computeDampedDelassusMatrixInverse(
392  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
393  dampedDelassusInverse.triangularView<StrictlyLower>() =
394  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
395  BOOST_CHECK(dampedDelassusInverse.isApprox(
396  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
397 
398  computeDampedDelassusMatrixInverse(
399  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
400  dampedDelassusInverse.triangularView<StrictlyLower>() =
401  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
402  BOOST_CHECK(dampedDelassusInverse.isApprox(
403  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-7));
404 }
405 
406 BOOST_AUTO_TEST_CASE(contact_3D4)
407 {
408  using namespace Eigen;
409  pinocchio::Model model;
410  pinocchio::buildModels::humanoidRandom(model, true);
411  pinocchio::Data data(model);
412 
413  const std::string RF = "rleg6_joint";
414  const std::string LF = "lleg6_joint";
415  const std::string RA = "rarm6_joint";
416  const std::string LA = "larm6_joint";
417  RigidConstraintModel ci_LA_3D(CONTACT_3D, model, model.getJointId(LA), LOCAL);
418  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
419  RigidConstraintModel ci_LF_3D(CONTACT_3D, model, model.getJointId(LF), LOCAL);
420  RigidConstraintModel ci_RF_3D(CONTACT_3D, model, model.getJointId(RF), LOCAL);
421  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
422  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
423  contact_models.push_back(ci_RA_3D);
424  contact_data.push_back(RigidConstraintData(ci_RA_3D));
425  contact_models.push_back(ci_LA_3D);
426  contact_data.push_back(RigidConstraintData(ci_LA_3D));
427  contact_models.push_back(ci_RF_3D);
428  contact_data.push_back(RigidConstraintData(ci_RF_3D));
429  contact_models.push_back(ci_LF_3D);
430  contact_data.push_back(RigidConstraintData(ci_LF_3D));
431 
432  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
433 
434  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
435  VectorXd q = randomConfiguration(model);
436  VectorXd v = Eigen::VectorXd::Random(model.nv);
437 
438  computeAllTerms(model, data, q, v);
439  contact_chol.compute(model, data, contact_models, contact_data, mu);
440  contact_chol.inverse(H_inverse);
441 
442  Data::MatrixXs dampedDelassusInverse;
443  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
444 
445  initPvDelassus(model, data, contact_models); // Allocate memory
446  computeDampedDelassusMatrixInverse(
447  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
448  dampedDelassusInverse.triangularView<StrictlyLower>() =
449  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
450  BOOST_CHECK(dampedDelassusInverse.isApprox(
451  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
452 
453  computeDampedDelassusMatrixInverse(
454  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
455  dampedDelassusInverse.triangularView<StrictlyLower>() =
456  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
457  BOOST_CHECK(dampedDelassusInverse.isApprox(
458  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-7));
459 }
460 
461 BOOST_AUTO_TEST_CASE(contact_3D_repeated)
462 {
463  using namespace Eigen;
464  pinocchio::Model model;
465  pinocchio::buildModels::humanoidRandom(model, true);
466  pinocchio::Data data(model);
467 
468  double mu = 1e-3;
469  const std::string RA = "rleg6_joint";
470  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
471  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
472  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
473  contact_models.push_back(ci_RA_3D);
474  contact_data.push_back(RigidConstraintData(ci_RA_3D));
475  contact_models.push_back(ci_RA_3D);
476  contact_data.push_back(RigidConstraintData(ci_RA_3D));
477 
478  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
479 
480  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
481  VectorXd q = randomConfiguration(model);
482  VectorXd v = Eigen::VectorXd::Random(model.nv);
483 
484  computeAllTerms(model, data, q, v);
485  contact_chol.compute(model, data, contact_models, contact_data, mu);
486  contact_chol.inverse(H_inverse);
487 
488  Data::MatrixXs dampedDelassusInverse;
489  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
490 
491  initPvDelassus(model, data, contact_models); // Allocate memory
492  computeDampedDelassusMatrixInverse(
493  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
494  dampedDelassusInverse.triangularView<StrictlyLower>() =
495  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
496  BOOST_CHECK(dampedDelassusInverse.isApprox(
497  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
498 
499  computeDampedDelassusMatrixInverse(
500  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
501  dampedDelassusInverse.triangularView<StrictlyLower>() =
502  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
503  BOOST_CHECK(dampedDelassusInverse.isApprox(
504  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-7));
505 }
506 
507 BOOST_AUTO_TEST_CASE(contact_3D_repeated4)
508 {
509  using namespace Eigen;
510  pinocchio::Model model;
511  pinocchio::buildModels::humanoidRandom(model, true);
512  pinocchio::Data data(model);
513 
514  const std::string RA = "rleg6_joint";
515  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
516  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
517  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
518  contact_models.push_back(ci_RA_3D);
519  contact_data.push_back(RigidConstraintData(ci_RA_3D));
520  contact_models.push_back(ci_RA_3D);
521  contact_data.push_back(RigidConstraintData(ci_RA_3D));
522  contact_models.push_back(ci_RA_3D);
523  contact_data.push_back(RigidConstraintData(ci_RA_3D));
524  contact_models.push_back(ci_RA_3D);
525  contact_data.push_back(RigidConstraintData(ci_RA_3D));
526 
527  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
528 
529  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
530  VectorXd q = randomConfiguration(model);
531  VectorXd v = Eigen::VectorXd::Random(model.nv);
532 
533  computeAllTerms(model, data, q, v);
534  contact_chol.compute(model, data, contact_models, contact_data, mu);
535  contact_chol.inverse(H_inverse);
536 
537  Data::MatrixXs dampedDelassusInverse;
538  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
539 
540  initPvDelassus(model, data, contact_models); // Allocate memory
541  computeDampedDelassusMatrixInverse(
542  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
543  dampedDelassusInverse.triangularView<StrictlyLower>() =
544  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
545  BOOST_CHECK(dampedDelassusInverse.isApprox(
546  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-10));
547 
548  computeDampedDelassusMatrixInverse(
549  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
550  dampedDelassusInverse.triangularView<StrictlyLower>() =
551  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
552  BOOST_CHECK(dampedDelassusInverse.isApprox(
553  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-10));
554 }
555 
556 BOOST_AUTO_TEST_CASE(contact_3D_repeated4_6D4)
557 {
558  using namespace Eigen;
559  pinocchio::Model model;
560  pinocchio::buildModels::humanoidRandom(model, true);
561  pinocchio::Data data(model);
562 
563  const std::string RA = "rleg6_joint";
564  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
565  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
566  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
567  contact_models.push_back(ci_RA_3D);
568  contact_data.push_back(RigidConstraintData(ci_RA_3D));
569  contact_models.push_back(ci_RA_3D);
570  contact_data.push_back(RigidConstraintData(ci_RA_3D));
571  contact_models.push_back(ci_RA_3D);
572  contact_data.push_back(RigidConstraintData(ci_RA_3D));
573  contact_models.push_back(ci_RA_3D);
574  contact_data.push_back(RigidConstraintData(ci_RA_3D));
575 
576  const std::string RF = "rleg6_joint";
577  const std::string LF = "lleg6_joint";
578  const std::string LA = "larm6_joint";
579  RigidConstraintModel ci_LA_6D(CONTACT_6D, model, model.getJointId(LA), LOCAL);
580  RigidConstraintModel ci_RA_6D(CONTACT_6D, model, model.getJointId(RA), LOCAL);
581  RigidConstraintModel ci_LF_6D(CONTACT_6D, model, model.getJointId(LF), LOCAL);
582  RigidConstraintModel ci_RF_6D(CONTACT_6D, model, model.getJointId(RF), LOCAL);
583  contact_models.push_back(ci_RA_6D);
584  contact_data.push_back(RigidConstraintData(ci_RA_6D));
585  contact_models.push_back(ci_LA_6D);
586  contact_data.push_back(RigidConstraintData(ci_LA_6D));
587  contact_models.push_back(ci_RF_6D);
588  contact_data.push_back(RigidConstraintData(ci_RF_6D));
589  contact_models.push_back(ci_LF_6D);
590  contact_data.push_back(RigidConstraintData(ci_LF_6D));
591 
592  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
593 
594  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
595  VectorXd q = randomConfiguration(model);
596  VectorXd v = Eigen::VectorXd::Random(model.nv);
597 
598  computeAllTerms(model, data, q, v);
599  contact_chol.compute(model, data, contact_models, contact_data, mu);
600  contact_chol.inverse(H_inverse);
601 
602  Data::MatrixXs dampedDelassusInverse;
603  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
604 
605  initPvDelassus(model, data, contact_models); // Allocate memory
606  computeDampedDelassusMatrixInverse(
607  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
608  dampedDelassusInverse.triangularView<StrictlyLower>() =
609  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
610  BOOST_CHECK(dampedDelassusInverse.isApprox(
611  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
612 
613  computeDampedDelassusMatrixInverse(
614  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
615  dampedDelassusInverse.triangularView<StrictlyLower>() =
616  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
617  BOOST_CHECK(dampedDelassusInverse.isApprox(
618  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-7));
619 }
620 
621 BOOST_AUTO_TEST_CASE(contact_3D_ancestors)
622 {
623  using namespace Eigen;
624  pinocchio::Model model;
625  pinocchio::buildModels::humanoidRandom(model, true);
626  pinocchio::Data data(model);
627 
628  double mu = 1e-3;
629  const std::string RA = "rleg6_joint";
630  const std::string LA = "rleg4_joint";
631  RigidConstraintModel ci_LA_3D(CONTACT_3D, model, model.getJointId(LA), LOCAL);
632  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
633  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
634  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
635  contact_models.push_back(ci_RA_3D);
636  contact_data.push_back(RigidConstraintData(ci_RA_3D));
637  contact_models.push_back(ci_LA_3D);
638  contact_data.push_back(RigidConstraintData(ci_LA_3D));
639 
640  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
641 
642  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
643 
644  VectorXd v = Eigen::VectorXd::Random(model.nv);
645  VectorXd q = randomConfiguration(model);
646 
647  computeAllTerms(model, data, q, v);
648  contact_chol.compute(model, data, contact_models, contact_data, mu);
649  contact_chol.inverse(H_inverse);
650 
651  initPvDelassus(model, data, contact_models); // Allocate memory
652 
653  for (int i = 0; i < 2; i++)
654  {
655  Eigen::MatrixXd dampedDelassusInverse;
656  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
657  dampedDelassusInverse.setZero();
658 
659  computeAllTerms(model, data, q, v);
660  contact_chol.compute(model, data, contact_models, contact_data, mu);
661  contact_chol.inverse(H_inverse);
662 
663  computeDampedDelassusMatrixInverse(
664  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
665  dampedDelassusInverse.triangularView<StrictlyLower>() =
666  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
667  BOOST_CHECK(dampedDelassusInverse.isApprox(
668  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-10));
669 
670  computeDampedDelassusMatrixInverse(
671  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
672  dampedDelassusInverse.triangularView<StrictlyLower>() =
673  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
674  BOOST_CHECK(dampedDelassusInverse.isApprox(
675  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-10));
676 
677  q = randomConfiguration(model);
678  v = Eigen::VectorXd::Random(model.nv);
679  }
680 }
681 
682 BOOST_AUTO_TEST_CASE(contact_3D_6D_ancestor)
683 {
684  using namespace Eigen;
685  pinocchio::Model model;
686  pinocchio::buildModels::humanoidRandom(model, true);
687  pinocchio::Data data(model);
688 
689  const std::string RA = "rleg6_joint";
690  const std::string LA = "rleg4_joint";
691  RigidConstraintModel ci_LA_6D(CONTACT_6D, model, model.getJointId(LA), LOCAL);
692  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
693  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
694  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
695  contact_models.push_back(ci_RA_3D);
696  contact_data.push_back(RigidConstraintData(ci_RA_3D));
697  contact_models.push_back(ci_LA_6D);
698  contact_data.push_back(RigidConstraintData(ci_LA_6D));
699 
700  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
701 
702  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
703  VectorXd q = randomConfiguration(model);
704  VectorXd v = Eigen::VectorXd::Random(model.nv);
705 
706  computeAllTerms(model, data, q, v);
707  contact_chol.compute(model, data, contact_models, contact_data, mu);
708  contact_chol.inverse(H_inverse);
709 
710  Data::MatrixXs dampedDelassusInverse;
711  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
712 
713  initPvDelassus(model, data, contact_models); // Allocate memory
714  computeDampedDelassusMatrixInverse(
715  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
716  dampedDelassusInverse.triangularView<StrictlyLower>() =
717  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
718  BOOST_CHECK(dampedDelassusInverse.isApprox(
719  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
720 
721  computeDampedDelassusMatrixInverse(
722  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
723  dampedDelassusInverse.triangularView<StrictlyLower>() =
724  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
725  BOOST_CHECK(dampedDelassusInverse.isApprox(
726  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-7));
727 }
728 
729 BOOST_AUTO_TEST_CASE(contact_3D_6D_ancestor_6D4)
730 {
731  using namespace Eigen;
732  pinocchio::Model model;
733  pinocchio::buildModels::humanoidRandom(model, true);
734  pinocchio::Data data(model);
735 
736  const std::string RA = "rleg6_joint";
737  const std::string LA = "rleg4_joint";
738  RigidConstraintModel ci_LA_6D(CONTACT_6D, model, model.getJointId(LA), LOCAL);
739  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, model.getJointId(RA), LOCAL);
740  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
741  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data;
742  contact_models.push_back(ci_RA_3D);
743  contact_data.push_back(RigidConstraintData(ci_RA_3D));
744  contact_models.push_back(ci_LA_6D);
745  contact_data.push_back(RigidConstraintData(ci_LA_6D));
746 
747  const std::string RF = "rleg6_joint";
748  const std::string LF = "lleg6_joint";
749  RigidConstraintModel ci_LF_6D(CONTACT_6D, model, model.getJointId(LF), LOCAL);
750  RigidConstraintModel ci_RF_6D(CONTACT_6D, model, model.getJointId(RF), LOCAL);
751  contact_models.push_back(ci_RF_6D);
752  contact_data.push_back(RigidConstraintData(ci_RF_6D));
753  contact_models.push_back(ci_LF_6D);
754  contact_data.push_back(RigidConstraintData(ci_LF_6D));
755 
756  pinocchio::Data::ContactCholeskyDecomposition contact_chol(model, contact_models);
757 
758  MatrixXd H_inverse(contact_chol.size(), contact_chol.size());
759  VectorXd q = randomConfiguration(model);
760  VectorXd v = Eigen::VectorXd::Random(model.nv);
761 
762  computeAllTerms(model, data, q, v);
763  contact_chol.compute(model, data, contact_models, contact_data, mu);
764  contact_chol.inverse(H_inverse);
765 
766  Data::MatrixXs dampedDelassusInverse;
767  dampedDelassusInverse.resize(contact_chol.constraintDim(), contact_chol.constraintDim());
768 
769  initPvDelassus(model, data, contact_models); // Allocate memory
770  computeDampedDelassusMatrixInverse(
771  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu);
772  dampedDelassusInverse.triangularView<StrictlyLower>() =
773  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
774  BOOST_CHECK(dampedDelassusInverse.isApprox(
775  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-11));
776 
777  computeDampedDelassusMatrixInverse(
778  model, data, q, contact_models, contact_data, dampedDelassusInverse, mu, false, false);
779  dampedDelassusInverse.triangularView<StrictlyLower>() =
780  dampedDelassusInverse.triangularView<StrictlyUpper>().transpose();
781  BOOST_CHECK(dampedDelassusInverse.isApprox(
782  -H_inverse.topLeftCorner(contact_chol.constraintDim(), contact_chol.constraintDim()), 1e-7));
783 }
784 
785 BOOST_AUTO_TEST_SUITE_END()
pinocchio::RigidConstraintData
RigidConstraintDataTpl< context::Scalar, context::Options > RigidConstraintData
Definition: algorithm/fwd.hpp:27
pinocchio::DataTpl::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: multibody/data.hpp:74
pinocchio::computeDampedDelassusMatrixInverse
void computeDampedDelassusMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, DataAllocator > &contact_data, const Eigen::MatrixBase< MatrixType > &damped_delassus_inverse, const Scalar mu, const bool scaled=false, const bool Pv=true)
Computes the inverse of the Delassus matrix associated to a set of given constraints.
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
delassus.hpp
pinocchio::ContactCholeskyDecompositionAccessorTpl::getLastChild
const IndexVector & getLastChild() const
Definition: delassus.cpp:35
pinocchio::DataTpl::ContactCholeskyDecomposition
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS typedef ContactCholeskyDecompositionTpl< Scalar, Options > ContactCholeskyDecomposition
Definition: multibody/data.hpp:110
contact-cholesky.contact_data
list contact_data
Definition: contact-cholesky.py:36
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:20
pinocchio::ContactCholeskyDecompositionAccessorTpl::getNvSubtree_fromRow
const IndexVector & getNvSubtree_fromRow() const
Definition: delassus.cpp:39
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::initPvDelassus
void initPvDelassus(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
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:325
pinocchio::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition: algorithm/contact-info.hpp:22
pinocchio::ContactCholeskyDecompositionAccessorTpl::ContactCholeskyDecompositionAccessorTpl
ContactCholeskyDecompositionAccessorTpl(const Base &other)
Definition: delassus.cpp:26
joint-configuration.hpp
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
pinocchio::ContactCholeskyDecompositionAccessorTpl::getJoint1_indexes
const std::vector< BooleanVector > & getJoint1_indexes() const
Definition: delassus.cpp:43
pinocchio::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
pinocchio::ContactCholeskyDecompositionAccessorTpl::Base
ContactCholeskyDecompositionTpl< Scalar, Options > Base
Definition: delassus.cpp:22
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
pinocchio::ContactCholeskyDecompositionAccessorTpl::getParents_fromRow
const IndexVector & getParents_fromRow() const
Definition: delassus.cpp:31
pinocchio::ContactCholeskyDecompositionAccessorTpl
Definition: delassus.cpp:19
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1118
mu
double mu
Definition: delassus.cpp:58
pinocchio::ContactCholeskyDecompositionAccessorTpl::BooleanVector
Base::BooleanVector BooleanVector
Definition: delassus.cpp:24
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:25
contact-info.hpp
pinocchio::ContactCholeskyDecompositionTpl
Definition: algorithm/fwd.hpp:17
contact-dynamics.hpp
sample-models.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::ContactCholeskyDecompositionAccessorTpl::IndexVector
Base::IndexVector IndexVector
Definition: delassus.cpp:23
pinocchio::CONTACT_3D
@ CONTACT_3D
Definition: algorithm/contact-info.hpp:21
pinocchio::computeAllTerms
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...
pinocchio::ContactCholeskyDecompositionAccessor
ContactCholeskyDecompositionAccessorTpl< double, 0 > ContactCholeskyDecompositionAccessor
Definition: delassus.cpp:53
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(contact_6D)
Definition: delassus.cpp:61
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50
pinocchio::ContactCholeskyDecompositionAccessorTpl::getJoint2_indexes
const std::vector< BooleanVector > & getJoint2_indexes() const
Definition: delassus.cpp:47

pinocchio
Author(s):
autogenerated on Sat Jun 22 2024 02:41:46