unittest/impulse-dynamics.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 CNRS INRIA
3 //
4 
5 #include "pinocchio/algorithm/aba.hpp"
6 #include "pinocchio/algorithm/rnea.hpp"
7 #include "pinocchio/algorithm/frames.hpp"
8 #include "pinocchio/algorithm/jacobian.hpp"
9 #include "pinocchio/algorithm/centroidal.hpp"
10 #include "pinocchio/algorithm/kinematics.hpp"
11 #include "pinocchio/algorithm/contact-info.hpp"
12 #include "pinocchio/algorithm/compute-all-terms.hpp"
13 #include "pinocchio/algorithm/impulse-dynamics.hpp"
14 #include "pinocchio/algorithm/contact-dynamics.hpp"
15 #include "pinocchio/algorithm/joint-configuration.hpp"
16 #include "pinocchio/multibody/sample-models.hpp"
17 #include "pinocchio/utils/timer.hpp"
18 
19 #include <iostream>
20 
21 #include <boost/test/unit_test.hpp>
22 #include <boost/utility/binary.hpp>
23 
24 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
25 
26 BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_empty)
27 {
28  using namespace Eigen;
29  using namespace pinocchio;
30 
31  pinocchio::Model model;
32  pinocchio::buildModels::humanoidRandom(model, true);
33  pinocchio::Data data(model), data_ref(model);
34 
35  model.lowerPositionLimit.head<3>().fill(-1.);
36  model.upperPositionLimit.head<3>().fill(1.);
37  VectorXd q = randomConfiguration(model);
38 
39  VectorXd v = VectorXd::Random(model.nv);
40  VectorXd tau = VectorXd::Random(model.nv);
41 
42  // Contact models and data
43  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
44  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
45 
46  const double mu0 = 0.;
47  ProximalSettings prox_settings(1e-12, mu0, 1);
48  const double r_coeff = 0.5;
49  computeAllTerms(model, data_ref, q, v);
50  data_ref.M.triangularView<Eigen::StrictlyLower>() =
51  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
52 
53  initConstraintDynamics(model, data, contact_models);
54  impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
55 
56  data.M.triangularView<Eigen::StrictlyLower>() =
57  data.M.transpose().triangularView<Eigen::StrictlyLower>();
58 
59  BOOST_CHECK(data.J.isApprox(data_ref.J));
60  BOOST_CHECK(data.M.isApprox(data_ref.M));
61  BOOST_CHECK(data.dq_after.isApprox(v));
62 }
63 
64 BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D)
65 {
66  using namespace Eigen;
67  using namespace pinocchio;
68 
69  pinocchio::Model model;
70  pinocchio::buildModels::humanoidRandom(model, true);
71  pinocchio::Data data(model), data_ref(model);
72 
73  model.lowerPositionLimit.head<3>().fill(-1.);
74  model.upperPositionLimit.head<3>().fill(1.);
75  VectorXd q = randomConfiguration(model);
76 
77  VectorXd v = VectorXd::Random(model.nv);
78  VectorXd tau = VectorXd::Random(model.nv);
79 
80  const std::string RF = "rleg6_joint";
81  // const Model::JointIndex RF_id = model.getJointId(RF);
82  const std::string LF = "lleg6_joint";
83  // const Model::JointIndex LF_id = model.getJointId(LF);
84 
85  // Contact models and data
86  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
87  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
88  RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), WORLD);
89  contact_models.push_back(ci_RF);
90  contact_datas.push_back(RigidConstraintData(ci_RF));
91  RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), WORLD);
92  contact_models.push_back(ci_LF);
93  contact_datas.push_back(RigidConstraintData(ci_LF));
94 
95  Eigen::DenseIndex constraint_dim = 0;
96  for (size_t k = 0; k < contact_models.size(); ++k)
97  constraint_dim += contact_models[k].size();
98 
99  const double mu0 = 0.;
100  ProximalSettings prox_settings(1e-12, mu0, 1);
101  const double r_coeff = 0.5;
102  Eigen::MatrixXd J_ref(constraint_dim, model.nv);
103  J_ref.setZero();
104 
105  computeAllTerms(model, data_ref, q, v);
106  framesForwardKinematics(model, data_ref, q);
107  data_ref.M.triangularView<Eigen::StrictlyLower>() =
108  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
109 
110  updateFramePlacements(model, data_ref);
111  getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_ref.middleRows<6>(0));
112  getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_ref.middleRows<6>(6));
113 
114  Eigen::VectorXd rhs_ref(constraint_dim);
115 
116  rhs_ref.segment<6>(0) = getFrameVelocity(model, data_ref, model.getFrameId(RF), WORLD).toVector();
117  rhs_ref.segment<6>(6) = getFrameVelocity(model, data_ref, model.getFrameId(LF), WORLD).toVector();
118 
119  Eigen::MatrixXd KKT_matrix_ref =
120  Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
121  KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
122  KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
123  KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
124 
125  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
126  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
127  impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
128  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
129 
130  data_ref.M.triangularView<Eigen::StrictlyLower>() =
131  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
132  BOOST_CHECK(
133  (data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
134  .isZero());
135  BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
136 
137  initConstraintDynamics(model, data, contact_models);
138  impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
139  BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
140  data.M.triangularView<Eigen::StrictlyLower>() =
141  data.M.transpose().triangularView<Eigen::StrictlyLower>();
142  BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
143  Data data_ag(model);
144  ccrba(model, data_ag, q, v);
145  BOOST_CHECK(data.J.isApprox(data_ref.J));
146  BOOST_CHECK(data.M.isApprox(data_ref.M));
147  BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
148 
149  for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
150  {
151  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
152  BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
153  BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
154  // BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
155  }
156 
157  // Check that the decomposition is correct
158  const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
159  Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
160 
161  BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
162  .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
163  BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
164 
165  // Check solutions
166  BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
167  BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
168 
169  Eigen::DenseIndex constraint_id = 0;
170  for (size_t k = 0; k < contact_models.size(); ++k)
171  {
172  const RigidConstraintModel & cmodel = contact_models[k];
173  const RigidConstraintData & cdata = contact_datas[k];
174 
175  switch (cmodel.type)
176  {
177  case pinocchio::CONTACT_3D: {
178  BOOST_CHECK(cdata.contact_force.linear().isApprox(
179  data_ref.impulse_c.segment(constraint_id, cmodel.size())));
180  break;
181  }
182 
183  case pinocchio::CONTACT_6D: {
184  ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
185  data_ref.impulse_c.segment<6>(constraint_id));
186  BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
187  break;
188  }
189 
190  default:
191  break;
192  }
193 
194  constraint_id += cmodel.size();
195  }
196 }
197 
198 BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D_LOCAL)
199 {
200  using namespace Eigen;
201  using namespace pinocchio;
202 
203  pinocchio::Model model;
204  pinocchio::buildModels::humanoidRandom(model, true);
205  pinocchio::Data data(model), data_ref(model);
206 
207  model.lowerPositionLimit.head<3>().fill(-1.);
208  model.upperPositionLimit.head<3>().fill(1.);
209  VectorXd q = randomConfiguration(model);
210 
211  VectorXd v = VectorXd::Random(model.nv);
212  VectorXd tau = VectorXd::Random(model.nv);
213 
214  const std::string RF = "rleg6_joint";
215  // const Model::JointIndex RF_id = model.getJointId(RF);
216  const std::string LF = "lleg6_joint";
217  // const Model::JointIndex LF_id = model.getJointId(LF);
218 
219  // Contact models and data
220  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
221  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
222  RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL);
223  contact_models.push_back(ci_RF);
224  contact_datas.push_back(RigidConstraintData(ci_RF));
225  RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL);
226  contact_models.push_back(ci_LF);
227  contact_datas.push_back(RigidConstraintData(ci_LF));
228 
229  Eigen::DenseIndex constraint_dim = 0;
230  for (size_t k = 0; k < contact_models.size(); ++k)
231  constraint_dim += contact_models[k].size();
232 
233  const double mu0 = 0.;
234  ProximalSettings prox_settings(1e-12, mu0, 1);
235  const double r_coeff = 0.5;
236  Eigen::MatrixXd J_ref(constraint_dim, model.nv);
237  J_ref.setZero();
238 
239  computeAllTerms(model, data_ref, q, v);
240  framesForwardKinematics(model, data_ref, q);
241  data_ref.M.triangularView<Eigen::StrictlyLower>() =
242  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
243 
244  updateFramePlacements(model, data_ref);
245  getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_ref.middleRows<6>(0));
246  getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_ref.middleRows<6>(6));
247 
248  Eigen::VectorXd rhs_ref(constraint_dim);
249 
250  rhs_ref.segment<6>(0) = getFrameVelocity(model, data_ref, model.getFrameId(RF), LOCAL).toVector();
251  rhs_ref.segment<6>(6) = getFrameVelocity(model, data_ref, model.getFrameId(LF), LOCAL).toVector();
252 
253  Eigen::MatrixXd KKT_matrix_ref =
254  Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
255  KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
256  KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
257  KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
258 
259  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
260  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
261  impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
262  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
263 
264  data_ref.M.triangularView<Eigen::StrictlyLower>() =
265  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
266  BOOST_CHECK(
267  (data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
268  .isZero());
269  BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
270 
271  initConstraintDynamics(model, data, contact_models);
272  impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
273  BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
274  data.M.triangularView<Eigen::StrictlyLower>() =
275  data.M.transpose().triangularView<Eigen::StrictlyLower>();
276  BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
277 
278  Data data_ag(model);
279  ccrba(model, data_ag, q, v);
280  BOOST_CHECK(data.J.isApprox(data_ref.J));
281  BOOST_CHECK(data.M.isApprox(data_ref.M));
282  BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
283 
284  for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
285  {
286  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
287  BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
288  BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
289  // BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
290  }
291 
292  // Check that the decomposition is correct
293  const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
294  Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
295 
296  BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
297  .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
298  BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
299 
300  // Check solutions
301  BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
302  BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
303 
304  Eigen::DenseIndex constraint_id = 0;
305  for (size_t k = 0; k < contact_models.size(); ++k)
306  {
307  const RigidConstraintModel & cmodel = contact_models[k];
308  const RigidConstraintData & cdata = contact_datas[k];
309 
310  switch (cmodel.type)
311  {
312  case pinocchio::CONTACT_3D: {
313  BOOST_CHECK(cdata.contact_force.linear().isApprox(
314  data_ref.impulse_c.segment(constraint_id, cmodel.size())));
315  break;
316  }
317 
318  case pinocchio::CONTACT_6D: {
319  ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
320  data_ref.impulse_c.segment<6>(constraint_id));
321  BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
322  break;
323  }
324 
325  default:
326  break;
327  }
328 
329  constraint_id += cmodel.size();
330  }
331 }
332 
333 BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D_LOCAL_WORLD_ALIGNED)
334 {
335  using namespace Eigen;
336  using namespace pinocchio;
337 
338  pinocchio::Model model;
339  pinocchio::buildModels::humanoidRandom(model, true);
340  pinocchio::Data data(model), data_ref(model);
341 
342  model.lowerPositionLimit.head<3>().fill(-1.);
343  model.upperPositionLimit.head<3>().fill(1.);
344  VectorXd q = randomConfiguration(model);
345 
346  VectorXd v = VectorXd::Random(model.nv);
347  VectorXd tau = VectorXd::Random(model.nv);
348 
349  const std::string RF = "rleg6_joint";
350  // const Model::JointIndex RF_id = model.getJointId(RF);
351  const std::string LF = "lleg6_joint";
352  // const Model::JointIndex LF_id = model.getJointId(LF);
353 
354  // Contact models and data
355  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
356  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
357  RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL_WORLD_ALIGNED);
358  contact_models.push_back(ci_RF);
359  contact_datas.push_back(RigidConstraintData(ci_RF));
360  RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL_WORLD_ALIGNED);
361  contact_models.push_back(ci_LF);
362  contact_datas.push_back(RigidConstraintData(ci_LF));
363 
364  Eigen::DenseIndex constraint_dim = 0;
365  for (size_t k = 0; k < contact_models.size(); ++k)
366  constraint_dim += contact_models[k].size();
367 
368  const double mu0 = 0.;
369  ProximalSettings prox_settings(1e-12, mu0, 1);
370  const double r_coeff = 0.5;
371  Eigen::MatrixXd J_ref(constraint_dim, model.nv);
372  J_ref.setZero();
373 
374  computeAllTerms(model, data_ref, q, v);
375  framesForwardKinematics(model, data_ref, q);
376  data_ref.M.triangularView<Eigen::StrictlyLower>() =
377  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
378 
379  updateFramePlacements(model, data_ref);
380  getJointJacobian(
381  model, data_ref, model.getJointId(RF), LOCAL_WORLD_ALIGNED, J_ref.middleRows<6>(0));
382  getJointJacobian(
383  model, data_ref, model.getJointId(LF), LOCAL_WORLD_ALIGNED, J_ref.middleRows<6>(6));
384 
385  Eigen::VectorXd rhs_ref(constraint_dim);
386 
387  rhs_ref.segment<6>(0) =
388  getFrameVelocity(model, data_ref, model.getFrameId(RF), LOCAL_WORLD_ALIGNED).toVector();
389  rhs_ref.segment<6>(6) =
390  getFrameVelocity(model, data_ref, model.getFrameId(LF), LOCAL_WORLD_ALIGNED).toVector();
391 
392  Eigen::MatrixXd KKT_matrix_ref =
393  Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
394  KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
395  KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
396  KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
397 
398  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
399  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
400  impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
401  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
402 
403  data_ref.M.triangularView<Eigen::StrictlyLower>() =
404  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
405  BOOST_CHECK(
406  (data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
407  .isZero());
408  BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
409 
410  initConstraintDynamics(model, data, contact_models);
411  impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
412  BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
413  data.M.triangularView<Eigen::StrictlyLower>() =
414  data.M.transpose().triangularView<Eigen::StrictlyLower>();
415  BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
416 
417  Data data_ag(model);
418  ccrba(model, data_ag, q, v);
419  BOOST_CHECK(data.J.isApprox(data_ref.J));
420  BOOST_CHECK(data.M.isApprox(data_ref.M));
421  BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
422 
423  for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
424  {
425  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
426  BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
427  BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
428  // BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
429  }
430 
431  // Check that the decomposition is correct
432  const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
433  Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
434 
435  BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
436  .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
437  BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
438 
439  // Check solutions
440  BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
441  BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
442 
443  Eigen::DenseIndex constraint_id = 0;
444  for (size_t k = 0; k < contact_models.size(); ++k)
445  {
446  const RigidConstraintModel & cmodel = contact_models[k];
447  const RigidConstraintData & cdata = contact_datas[k];
448 
449  switch (cmodel.type)
450  {
451  case pinocchio::CONTACT_3D: {
452  BOOST_CHECK(cdata.contact_force.linear().isApprox(
453  data_ref.impulse_c.segment(constraint_id, cmodel.size())));
454  break;
455  }
456 
457  case pinocchio::CONTACT_6D: {
458  ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
459  data_ref.impulse_c.segment<6>(constraint_id));
460  BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
461  break;
462  }
463 
464  default:
465  break;
466  }
467 
468  constraint_id += cmodel.size();
469  }
470 }
471 
472 BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D_3D)
473 {
474  using namespace Eigen;
475  using namespace pinocchio;
476 
477  pinocchio::Model model;
478  pinocchio::buildModels::humanoidRandom(model, true);
479  pinocchio::Data data(model), data_ref(model);
480 
481  model.lowerPositionLimit.head<3>().fill(-1.);
482  model.upperPositionLimit.head<3>().fill(1.);
483  VectorXd q = randomConfiguration(model);
484 
485  VectorXd v = VectorXd::Random(model.nv);
486  VectorXd tau = VectorXd::Random(model.nv);
487 
488  const std::string RF = "rleg6_joint";
489  // const Model::JointIndex RF_id = model.getJointId(RF);
490  const std::string LF = "lleg6_joint";
491  // const Model::JointIndex LF_id = model.getJointId(LF);
492 
493  // Contact models and data
494  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
495  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
496  RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), WORLD);
497  contact_models.push_back(ci_RF);
498  contact_datas.push_back(RigidConstraintData(ci_RF));
499  RigidConstraintModel ci_LF(CONTACT_3D, model, model.getJointId(LF), WORLD);
500  contact_models.push_back(ci_LF);
501  contact_datas.push_back(RigidConstraintData(ci_LF));
502 
503  Eigen::DenseIndex constraint_dim = 0;
504  for (size_t k = 0; k < contact_models.size(); ++k)
505  constraint_dim += contact_models[k].size();
506 
507  const double mu0 = 0.;
508  ProximalSettings prox_settings(1e-12, mu0, 1);
509  const double r_coeff = 0.5;
510  Eigen::MatrixXd J_ref(constraint_dim, model.nv), Jtmp(6, model.nv);
511  J_ref.setZero();
512  Jtmp.setZero();
513 
514  computeAllTerms(model, data_ref, q, v);
515  framesForwardKinematics(model, data_ref, q);
516  data_ref.M.triangularView<Eigen::StrictlyLower>() =
517  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
518 
519  updateFramePlacements(model, data_ref);
520  getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_ref.middleRows<6>(0));
521  getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, Jtmp);
522  J_ref.middleRows<3>(6) = Jtmp.middleRows<3>(Motion::LINEAR);
523 
524  Eigen::VectorXd rhs_ref(constraint_dim);
525 
526  rhs_ref.segment<6>(0) = getFrameVelocity(model, data_ref, model.getFrameId(RF), WORLD).toVector();
527  rhs_ref.segment<3>(6) = getFrameVelocity(model, data_ref, model.getFrameId(LF), WORLD).linear();
528 
529  Eigen::MatrixXd KKT_matrix_ref =
530  Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
531  KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
532  KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
533  KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
534 
535  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
536  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
537  impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
538  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
539 
540  data_ref.M.triangularView<Eigen::StrictlyLower>() =
541  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
542  BOOST_CHECK(
543  (data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
544  .isZero());
545  BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
546 
547  initConstraintDynamics(model, data, contact_models);
548  impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
549  BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
550  data.M.triangularView<Eigen::StrictlyLower>() =
551  data.M.transpose().triangularView<Eigen::StrictlyLower>();
552  BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
553 
554  Data data_ag(model);
555  ccrba(model, data_ag, q, v);
556  BOOST_CHECK(data.J.isApprox(data_ref.J));
557  BOOST_CHECK(data.M.isApprox(data_ref.M));
558  BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
559 
560  for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
561  {
562  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
563  BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
564  BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
565  // BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
566  }
567 
568  // Check that the decomposition is correct
569  const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
570  Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
571 
572  BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
573  .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
574  BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
575 
576  // Check solutions
577  BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
578  BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
579 
580  Eigen::DenseIndex constraint_id = 0;
581  for (size_t k = 0; k < contact_models.size(); ++k)
582  {
583  const RigidConstraintModel & cmodel = contact_models[k];
584  const RigidConstraintData & cdata = contact_datas[k];
585 
586  switch (cmodel.type)
587  {
588  case pinocchio::CONTACT_3D: {
589  BOOST_CHECK(cdata.contact_force.linear().isApprox(
590  data_ref.impulse_c.segment(constraint_id, cmodel.size())));
591  break;
592  }
593 
594  case pinocchio::CONTACT_6D: {
595  ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
596  data_ref.impulse_c.segment<6>(constraint_id));
597  BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
598  break;
599  }
600 
601  default:
602  break;
603  }
604 
605  constraint_id += cmodel.size();
606  }
607 }
608 
609 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::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
frames.hpp
Eigen
pinocchio::python::ContactCholeskyDecomposition
ContactCholeskyDecompositionTpl< context::Scalar, context::Options > ContactCholeskyDecomposition
Definition: admm-solver.cpp:33
pinocchio::DataTpl
Definition: context/generic.hpp:25
compute-all-terms.hpp
pinocchio::getFrameVelocity
MotionTpl< Scalar, Options > getFrameVelocity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the spatial velocity of the Frame expressed in the desired reference frame....
kinematics.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
#define PINOCCHIO_COMPILER_DIAGNOSTIC_POP
Definition: include/pinocchio/macros.hpp:131
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.
contact-cholesky.cmodel
cmodel
Definition: contact-cholesky.py:28
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
rnea.hpp
bindings_dynamics.r_coeff
float r_coeff
Definition: bindings_dynamics.py:11
timer.hpp
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:12
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:68
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:65
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:23
joint-configuration.hpp
pinocchio::ProximalSettingsTpl
Structure containing all the settings paramters for the proximal algorithms.
Definition: algorithm/fwd.hpp:13
pinocchio::updateFramePlacements
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
autodiff-rnea.v
v
Definition: autodiff-rnea.py:15
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::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
pinocchio::initConstraintDynamics
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
pinocchio::ccrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & ccrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal m...
size
FCL_REAL size() const
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
#define PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
Definition: include/pinocchio/macros.hpp:132
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
Definition: include/pinocchio/macros.hpp:130
pinocchio::DataTpl::Ag
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: multibody/data.hpp:284
q
q
pinocchio::framesForwardKinematics
void framesForwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
First calls the forwardKinematics on the model, then computes the placement of each frame....
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:360
pinocchio::DataTpl::dq_after
TangentVectorType dq_after
Generalized velocity after impact.
Definition: multibody/data.hpp:487
pinocchio::ForceRef
Definition: force-ref.hpp:53
fill
fill
centroidal.hpp
impulse-dynamics.hpp
pinocchio::impulseDynamics
const PINOCCHIO_DEPRECATED DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & impulseDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v_before, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Scalar r_coeff=0., const Scalar inv_damping=0.)
Compute the impulse dynamics with contact constraints. Internally, pinocchio::crba is called.
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:25
contact-info.hpp
contact-dynamics.hpp
anymal-simulation.constraint_dim
constraint_dim
Definition: anymal-simulation.py:67
sample-models.hpp
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
simulation-closed-kinematic-chains.prox_settings
prox_settings
Definition: simulation-closed-kinematic-chains.py:163
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...
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_empty)
Definition: unittest/impulse-dynamics.cpp:26
pinocchio::DataTpl::impulse_c
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Definition: multibody/data.hpp:491
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

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