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_LOCAL)
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), LOCAL);
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), LOCAL);
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), LOCAL, J_ref.middleRows<6>(0));
112  getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, 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), LOCAL).toVector();
117  rhs_ref.segment<6>(6) = getFrameVelocity(model, data_ref, model.getFrameId(LF), LOCAL).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 
144  Data data_ag(model);
145  ccrba(model, data_ag, q, v);
146  BOOST_CHECK(data.J.isApprox(data_ref.J));
147  BOOST_CHECK(data.M.isApprox(data_ref.M));
148  BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
149 
150  for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
151  {
152  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
153  BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
154  BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
155  // BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
156  }
157 
158  // Check that the decomposition is correct
159  const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
160  Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
161 
162  BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
163  .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
164  BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
165 
166  // Check solutions
167  BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
168  BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
169 
170  Eigen::DenseIndex constraint_id = 0;
171  for (size_t k = 0; k < contact_models.size(); ++k)
172  {
173  const RigidConstraintModel & cmodel = contact_models[k];
174  const RigidConstraintData & cdata = contact_datas[k];
175 
176  switch (cmodel.type)
177  {
178  case pinocchio::CONTACT_3D: {
179  BOOST_CHECK(cdata.contact_force.linear().isApprox(
180  data_ref.impulse_c.segment(constraint_id, cmodel.size())));
181  break;
182  }
183 
184  case pinocchio::CONTACT_6D: {
185  ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
186  data_ref.impulse_c.segment<6>(constraint_id));
187  BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
188  break;
189  }
190 
191  default:
192  break;
193  }
194 
195  constraint_id += cmodel.size();
196  }
197 }
198 
199 BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D_LOCAL_WORLD_ALIGNED)
200 {
201  using namespace Eigen;
202  using namespace pinocchio;
203 
204  pinocchio::Model model;
205  pinocchio::buildModels::humanoidRandom(model, true);
206  pinocchio::Data data(model), data_ref(model);
207 
208  model.lowerPositionLimit.head<3>().fill(-1.);
209  model.upperPositionLimit.head<3>().fill(1.);
210  VectorXd q = randomConfiguration(model);
211 
212  VectorXd v = VectorXd::Random(model.nv);
213  VectorXd tau = VectorXd::Random(model.nv);
214 
215  const std::string RF = "rleg6_joint";
216  // const Model::JointIndex RF_id = model.getJointId(RF);
217  const std::string LF = "lleg6_joint";
218  // const Model::JointIndex LF_id = model.getJointId(LF);
219 
220  // Contact models and data
221  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
222  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
223  RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL_WORLD_ALIGNED);
224  contact_models.push_back(ci_RF);
225  contact_datas.push_back(RigidConstraintData(ci_RF));
226  RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL_WORLD_ALIGNED);
227  contact_models.push_back(ci_LF);
228  contact_datas.push_back(RigidConstraintData(ci_LF));
229 
230  Eigen::DenseIndex constraint_dim = 0;
231  for (size_t k = 0; k < contact_models.size(); ++k)
232  constraint_dim += contact_models[k].size();
233 
234  const double mu0 = 0.;
235  ProximalSettings prox_settings(1e-12, mu0, 1);
236  const double r_coeff = 0.5;
237  Eigen::MatrixXd J_ref(constraint_dim, model.nv);
238  J_ref.setZero();
239 
240  computeAllTerms(model, data_ref, q, v);
241  framesForwardKinematics(model, data_ref, q);
242  data_ref.M.triangularView<Eigen::StrictlyLower>() =
243  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
244 
245  updateFramePlacements(model, data_ref);
246  getJointJacobian(
247  model, data_ref, model.getJointId(RF), LOCAL_WORLD_ALIGNED, J_ref.middleRows<6>(0));
248  getJointJacobian(
249  model, data_ref, model.getJointId(LF), LOCAL_WORLD_ALIGNED, J_ref.middleRows<6>(6));
250 
251  Eigen::VectorXd rhs_ref(constraint_dim);
252 
253  rhs_ref.segment<6>(0) =
254  getFrameVelocity(model, data_ref, model.getFrameId(RF), LOCAL_WORLD_ALIGNED).toVector();
255  rhs_ref.segment<6>(6) =
256  getFrameVelocity(model, data_ref, model.getFrameId(LF), LOCAL_WORLD_ALIGNED).toVector();
257 
258  Eigen::MatrixXd KKT_matrix_ref =
259  Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
260  KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
261  KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
262  KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
263 
264  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
265  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
266  impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
267  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
268 
269  data_ref.M.triangularView<Eigen::StrictlyLower>() =
270  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
271  BOOST_CHECK(
272  (data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
273  .isZero());
274  BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
275 
276  initConstraintDynamics(model, data, contact_models);
277  impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
278  BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
279  data.M.triangularView<Eigen::StrictlyLower>() =
280  data.M.transpose().triangularView<Eigen::StrictlyLower>();
281  BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
282 
283  Data data_ag(model);
284  ccrba(model, data_ag, q, v);
285  BOOST_CHECK(data.J.isApprox(data_ref.J));
286  BOOST_CHECK(data.M.isApprox(data_ref.M));
287  BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
288 
289  for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
290  {
291  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
292  BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
293  BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
294  // BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
295  }
296 
297  // Check that the decomposition is correct
298  const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
299  Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
300 
301  BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
302  .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
303  BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
304 
305  // Check solutions
306  BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
307  BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
308 
309  Eigen::DenseIndex constraint_id = 0;
310  for (size_t k = 0; k < contact_models.size(); ++k)
311  {
312  const RigidConstraintModel & cmodel = contact_models[k];
313  const RigidConstraintData & cdata = contact_datas[k];
314 
315  switch (cmodel.type)
316  {
317  case pinocchio::CONTACT_3D: {
318  BOOST_CHECK(cdata.contact_force.linear().isApprox(
319  data_ref.impulse_c.segment(constraint_id, cmodel.size())));
320  break;
321  }
322 
323  case pinocchio::CONTACT_6D: {
324  ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
325  data_ref.impulse_c.segment<6>(constraint_id));
326  BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
327  break;
328  }
329 
330  default:
331  break;
332  }
333 
334  constraint_id += cmodel.size();
335  }
336 }
337 
338 BOOST_AUTO_TEST_CASE(test_sparse_impulse_dynamics_in_contact_6D_3D)
339 {
340  using namespace Eigen;
341  using namespace pinocchio;
342 
343  pinocchio::Model model;
344  pinocchio::buildModels::humanoidRandom(model, true);
345  pinocchio::Data data(model), data_ref(model);
346 
347  model.lowerPositionLimit.head<3>().fill(-1.);
348  model.upperPositionLimit.head<3>().fill(1.);
349  VectorXd q = randomConfiguration(model);
350 
351  VectorXd v = VectorXd::Random(model.nv);
352  VectorXd tau = VectorXd::Random(model.nv);
353 
354  const std::string RF = "rleg6_joint";
355  // const Model::JointIndex RF_id = model.getJointId(RF);
356  const std::string LF = "lleg6_joint";
357  // const Model::JointIndex LF_id = model.getJointId(LF);
358 
359  // Contact models and data
360  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
361  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas;
362  RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL);
363  contact_models.push_back(ci_RF);
364  contact_datas.push_back(RigidConstraintData(ci_RF));
365  RigidConstraintModel ci_LF(CONTACT_3D, model, model.getJointId(LF), LOCAL);
366  contact_models.push_back(ci_LF);
367  contact_datas.push_back(RigidConstraintData(ci_LF));
368 
369  Eigen::DenseIndex constraint_dim = 0;
370  for (size_t k = 0; k < contact_models.size(); ++k)
371  constraint_dim += contact_models[k].size();
372 
373  const double mu0 = 0.;
374  ProximalSettings prox_settings(1e-12, mu0, 1);
375  const double r_coeff = 0.5;
376  Eigen::MatrixXd J_ref(constraint_dim, model.nv), Jtmp(6, model.nv);
377  J_ref.setZero();
378  Jtmp.setZero();
379 
380  computeAllTerms(model, data_ref, q, v);
381  framesForwardKinematics(model, data_ref, q);
382  data_ref.M.triangularView<Eigen::StrictlyLower>() =
383  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
384 
385  updateFramePlacements(model, data_ref);
386  getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_ref.middleRows<6>(0));
387  getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, Jtmp);
388  J_ref.middleRows<3>(6) = Jtmp.middleRows<3>(Motion::LINEAR);
389 
390  Eigen::VectorXd rhs_ref(constraint_dim);
391 
392  rhs_ref.segment<6>(0) = getFrameVelocity(model, data_ref, model.getFrameId(RF), LOCAL).toVector();
393  rhs_ref.segment<3>(6) = getFrameVelocity(model, data_ref, model.getFrameId(LF), LOCAL).linear();
394 
395  Eigen::MatrixXd KKT_matrix_ref =
396  Eigen::MatrixXd::Zero(model.nv + constraint_dim, model.nv + constraint_dim);
397  KKT_matrix_ref.bottomRightCorner(model.nv, model.nv) = data_ref.M;
398  KKT_matrix_ref.topRightCorner(constraint_dim, model.nv) = J_ref;
399  KKT_matrix_ref.bottomLeftCorner(model.nv, constraint_dim) = J_ref.transpose();
400 
401  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
402  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
403  impulseDynamics(model, data_ref, q, v, J_ref, r_coeff, mu0);
404  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
405 
406  data_ref.M.triangularView<Eigen::StrictlyLower>() =
407  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
408  BOOST_CHECK(
409  (data_ref.M * data_ref.dq_after - data_ref.M * v - J_ref.transpose() * data_ref.impulse_c)
410  .isZero());
411  BOOST_CHECK((J_ref * data_ref.dq_after + r_coeff * J_ref * v).isZero());
412 
413  initConstraintDynamics(model, data, contact_models);
414  impulseDynamics(model, data, q, v, contact_models, contact_datas, r_coeff, prox_settings);
415  BOOST_CHECK((J_ref * data.dq_after + r_coeff * J_ref * v).isZero());
416  data.M.triangularView<Eigen::StrictlyLower>() =
417  data.M.transpose().triangularView<Eigen::StrictlyLower>();
418  BOOST_CHECK((data.M * data.dq_after - data.M * v - J_ref.transpose() * data.impulse_c).isZero());
419 
420  Data data_ag(model);
421  ccrba(model, data_ag, q, v);
422  BOOST_CHECK(data.J.isApprox(data_ref.J));
423  BOOST_CHECK(data.M.isApprox(data_ref.M));
424  BOOST_CHECK(data.Ag.isApprox(data_ag.Ag));
425 
426  for (Model::JointIndex k = 1; k < model.joints.size(); ++k)
427  {
428  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
429  BOOST_CHECK(data.liMi[k].isApprox(data_ref.liMi[k]));
430  BOOST_CHECK(data.ov[k].isApprox(data_ref.oMi[k].act(data_ref.v[k])));
431  // BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oMi[k].act(data_ref.a_gf[k])));
432  }
433 
434  // Check that the decomposition is correct
435  const Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
436  Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
437 
438  BOOST_CHECK(KKT_matrix.bottomRightCorner(model.nv, model.nv)
439  .isApprox(KKT_matrix_ref.bottomRightCorner(model.nv, model.nv)));
440  BOOST_CHECK(KKT_matrix.isApprox(KKT_matrix_ref));
441 
442  // Check solutions
443  BOOST_CHECK(data.dq_after.isApprox(data_ref.dq_after));
444  BOOST_CHECK(data.impulse_c.isApprox(data_ref.impulse_c));
445 
446  Eigen::DenseIndex constraint_id = 0;
447  for (size_t k = 0; k < contact_models.size(); ++k)
448  {
449  const RigidConstraintModel & cmodel = contact_models[k];
450  const RigidConstraintData & cdata = contact_datas[k];
451 
452  switch (cmodel.type)
453  {
454  case pinocchio::CONTACT_3D: {
455  BOOST_CHECK(cdata.contact_force.linear().isApprox(
456  data_ref.impulse_c.segment(constraint_id, cmodel.size())));
457  break;
458  }
459 
460  case pinocchio::CONTACT_6D: {
461  ForceRef<Data::VectorXs::FixedSegmentReturnType<6>::Type> f_ref(
462  data_ref.impulse_c.segment<6>(constraint_id));
463  BOOST_CHECK(cdata.contact_force.isApprox(f_ref));
464  break;
465  }
466 
467  default:
468  break;
469  }
470 
471  constraint_id += cmodel.size();
472  }
473 }
474 
475 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
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:130
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:25
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
rnea.hpp
bindings_dynamics.r_coeff
float r_coeff
Definition: bindings_dynamics.py:10
timer.hpp
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
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
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
joint-configuration.hpp
pinocchio::ProximalSettingsTpl
Structure containing all the settings parameters 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
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
cartpole.v
v
Definition: cartpole.py:153
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:131
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
#define PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
macros for pragma push/pop/ignore deprecated warnings
Definition: include/pinocchio/macros.hpp:129
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:22
contact-info.hpp
contact-dynamics.hpp
anymal-simulation.constraint_dim
constraint_dim
Definition: anymal-simulation.py:52
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
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

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