contact-dynamics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020-2022 CNRS INRIA
3 //
4 
5 #include <iostream>
6 
7 #include "pinocchio/algorithm/jacobian.hpp"
8 #include "pinocchio/algorithm/frames.hpp"
9 #include "pinocchio/algorithm/kinematics.hpp"
10 #include "pinocchio/algorithm/crba.hpp"
11 #include "pinocchio/parsers/srdf.hpp"
12 #include "pinocchio/algorithm/aba-derivatives.hpp"
13 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
14 #include "pinocchio/algorithm/frames-derivatives.hpp"
15 #include "pinocchio/algorithm/constrained-dynamics.hpp"
16 #include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp"
17 #include "pinocchio/algorithm/joint-configuration.hpp"
18 #include "pinocchio/multibody/sample-models.hpp"
19 #include "pinocchio/spatial/classic-acceleration.hpp"
20 
21 #include <boost/test/unit_test.hpp>
22 #include <boost/utility/binary.hpp>
23 
24 #ifdef PINOCCHIO_WITH_SDFORMAT
25 
26  #include "pinocchio/parsers/sdf.hpp"
27 
28 #endif // PINOCCHIO_WITH_SDFORMAT
29 
30 #define KP 10
31 #define KD 10
32 
33 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
34 
35 using namespace Eigen;
36 using namespace pinocchio;
37 
38 BOOST_AUTO_TEST_CASE(test_sparse_constraint_dynamics_derivatives_no_contact)
39 {
40  using namespace Eigen;
41  using namespace pinocchio;
42 
43  Model model;
44  buildModels::humanoidRandom(model, true);
45  Data data(model), data_ref(model);
46 
47  model.lowerPositionLimit.head<3>().fill(-1.);
48  model.upperPositionLimit.head<3>().fill(1.);
49  VectorXd q = randomConfiguration(model);
50 
51  VectorXd v = VectorXd::Random(model.nv);
52  VectorXd tau = VectorXd::Random(model.nv);
53 
54  // Contact models and data
55  const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_constraint_models;
56  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) empty_constraint_data;
57 
58  const double mu0 = 0.;
59  ProximalSettings prox_settings(1e-12, mu0, 1);
60 
61  initConstraintDynamics(model, data, empty_constraint_models);
62  constraintDynamics(
63  model, data, q, v, tau, empty_constraint_models, empty_constraint_data, prox_settings);
64  data.M.triangularView<Eigen::StrictlyLower>() =
65  data.M.transpose().triangularView<Eigen::StrictlyLower>();
66  computeConstraintDynamicsDerivatives(
67  model, data, empty_constraint_models, empty_constraint_data, prox_settings);
68 
69  // Reference values
70  computeABADerivatives(model, data_ref, q, v, tau);
71  forwardKinematics(model, data_ref, q, v, data_ref.ddq);
72  BOOST_CHECK(data_ref.ddq.isApprox(data.ddq));
73 
74  for (size_t k = 1; k < (size_t)model.njoints; ++k)
75  {
76  BOOST_CHECK(data_ref.oMi[k].isApprox(data.oMi[k]));
77  BOOST_CHECK(data_ref.ov[k].isApprox(data.ov[k]));
78  BOOST_CHECK(data_ref.v[k].isApprox(data.v[k]));
79  BOOST_CHECK(data_ref.a[k].isApprox(data.a[k]));
80  BOOST_CHECK(data_ref.oa[k].isApprox(data.oa[k]));
81  }
82 
83  BOOST_CHECK(data_ref.ddq.isApprox(data.ddq));
84  BOOST_CHECK(data_ref.dVdq.isApprox(data.dVdq));
85  BOOST_CHECK(data_ref.J.isApprox(data.J));
86  BOOST_CHECK(data_ref.dAdq.isApprox(data.dAdq));
87  BOOST_CHECK(data_ref.dAdv.isApprox(data.dAdv));
88  BOOST_CHECK(data_ref.dFdq.isApprox(data.dFdq));
89  BOOST_CHECK(data_ref.dFdv.isApprox(data.dFdv));
90 
91  BOOST_CHECK(data_ref.dtau_dq.isApprox(data.dtau_dq));
92  BOOST_CHECK(data_ref.dtau_dv.isApprox(data.dtau_dv));
93 
94  BOOST_CHECK(data_ref.ddq_dq.isApprox(data.ddq_dq));
95  BOOST_CHECK(data_ref.ddq_dv.isApprox(data.ddq_dv));
96  BOOST_CHECK(data_ref.Minv.isApprox(data.ddq_dtau));
97 }
98 
99 BOOST_AUTO_TEST_CASE(test_sparse_constraint_dynamics_derivatives)
100 {
101  using namespace Eigen;
102  using namespace pinocchio;
103  Model model;
104  buildModels::humanoidRandom(model, true);
105  Data data(model), data_ref(model);
106 
107  model.lowerPositionLimit.head<3>().fill(-1.);
108  model.upperPositionLimit.head<3>().fill(1.);
109  VectorXd q = randomConfiguration(model);
110 
111  VectorXd v = VectorXd::Random(model.nv);
112  VectorXd tau = VectorXd::Random(model.nv);
113 
114  const std::string RF = "rleg6_joint";
115  const Model::JointIndex RF_id = model.getJointId(RF);
116  const std::string LF = "lleg6_joint";
117  const Model::JointIndex LF_id = model.getJointId(LF);
118 
119  // Contact models and data
120  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
121  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
122 
123  RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL);
124  ci_LF.joint1_placement.setRandom();
125  RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL);
126  ci_RF.joint1_placement.setRandom();
127 
128  constraint_models.push_back(ci_LF);
129  constraint_data.push_back(RigidConstraintData(ci_LF));
130  constraint_models.push_back(ci_RF);
131  constraint_data.push_back(RigidConstraintData(ci_RF));
132 
133  Eigen::DenseIndex constraint_dim = 0;
134  for (size_t k = 0; k < constraint_models.size(); ++k)
135  constraint_dim += constraint_models[k].size();
136 
137  const double mu0 = 0.;
138  ProximalSettings prox_settings(1e-12, mu0, 1);
139 
140  initConstraintDynamics(model, data, constraint_models);
141  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
142  data.M.triangularView<Eigen::StrictlyLower>() =
143  data.M.transpose().triangularView<Eigen::StrictlyLower>();
144  computeConstraintDynamicsDerivatives(
145  model, data, constraint_models, constraint_data, prox_settings);
146 
147  // Reference values
148  crba(model, data_ref, q, Convention::WORLD);
149  data_ref.M.triangularView<Eigen::StrictlyLower>() =
150  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
151  container::aligned_vector<Force> fext((size_t)model.njoints, Force::Zero());
152  for (size_t k = 0; k < constraint_models.size(); ++k)
153  {
154  const RigidConstraintModel & cmodel = constraint_models[k];
155  const RigidConstraintData & cdata = constraint_data[k];
156  fext[cmodel.joint1_id] = cmodel.joint1_placement.act(cdata.contact_force);
157 
158  BOOST_CHECK(cdata.oMc1.isApprox(data_ref.oMi[cmodel.joint1_id] * cmodel.joint1_placement));
159  }
160 
161  computeABADerivatives(model, data_ref, q, v, tau, fext);
162  forwardKinematics(model, data_ref, q, v);
163 
164  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
165  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
166  BOOST_CHECK(data.J.isApprox(data_ref.J));
167  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
168  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
169  BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq));
170  BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv));
171 
172  MatrixXd vLF_partial_dq(MatrixXd::Zero(6, model.nv)), aLF_partial_dq(MatrixXd::Zero(6, model.nv)),
173  aLF_partial_dv(MatrixXd::Zero(6, model.nv)), aLF_partial_da(MatrixXd::Zero(6, model.nv));
174 
175  MatrixXd vRF_partial_dq(MatrixXd::Zero(6, model.nv)), aRF_partial_dq(MatrixXd::Zero(6, model.nv)),
176  aRF_partial_dv(MatrixXd::Zero(6, model.nv)), aRF_partial_da(MatrixXd::Zero(6, model.nv));
177 
178  getFrameAccelerationDerivatives(
179  model, data_ref, LF_id, ci_LF.joint1_placement, LOCAL, vLF_partial_dq, aLF_partial_dq,
180  aLF_partial_dv, aLF_partial_da);
181  getFrameAccelerationDerivatives(
182  model, data_ref, RF_id, ci_RF.joint1_placement, LOCAL, vRF_partial_dq, aRF_partial_dq,
183  aRF_partial_dv, aRF_partial_da);
184 
185  MatrixXd Jc(constraint_dim, model.nv);
186  Jc << aLF_partial_da, aRF_partial_da.topRows<3>();
187 
188  MatrixXd K(model.nv + constraint_dim, model.nv + constraint_dim);
189  K << data_ref.M, Jc.transpose(), Jc, MatrixXd::Zero(constraint_dim, constraint_dim);
190  const MatrixXd Kinv = K.inverse();
191 
192  MatrixXd osim((Jc * data_ref.M.inverse() * Jc.transpose()).inverse());
193  BOOST_CHECK(data.osim.isApprox(osim));
194 
195  MatrixXd ac_partial_dq(constraint_dim, model.nv);
196 
197  BOOST_CHECK(data.ov[RF_id].isApprox(data_ref.oMi[RF_id].act(data_ref.v[RF_id])));
198  aRF_partial_dq.topRows<3>() +=
199  cross(ci_RF.joint1_placement.actInv(data_ref.v[RF_id]).angular(), vRF_partial_dq.topRows<3>())
200  - cross(
201  ci_RF.joint1_placement.actInv(data_ref.v[RF_id]).linear(), vRF_partial_dq.bottomRows<3>());
202 
203  BOOST_CHECK(data.ov[LF_id].isApprox(data_ref.oMi[LF_id].act(data_ref.v[LF_id])));
204 
205  ac_partial_dq << aLF_partial_dq, aRF_partial_dq.topRows<3>();
206 
207  MatrixXd dac_dq = ac_partial_dq; // - Jc * data_ref.Minv*data_ref.dtau_dq;
208 
209  BOOST_CHECK(data.dac_dq.isApprox(dac_dq, 1e-8));
210  BOOST_CHECK(Kinv.bottomLeftCorner(constraint_dim, model.nv).isApprox(osim * Jc * data_ref.Minv));
211 
212  MatrixXd df_dq = Kinv.bottomLeftCorner(constraint_dim, model.nv) * data_ref.dtau_dq
213  + Kinv.bottomRightCorner(constraint_dim, constraint_dim) * ac_partial_dq;
214 
215  MatrixXd ddq_dq = data_ref.Minv * (-data_ref.dtau_dq + Jc.transpose() * df_dq);
216 
217  BOOST_CHECK(df_dq.isApprox(data.dlambda_dq));
218  BOOST_CHECK(ddq_dq.isApprox(data.ddq_dq));
219 }
220 
221 pinocchio::Motion computeAcceleration(
222  const pinocchio::Model & model,
223  const pinocchio::Data & data,
224  const pinocchio::JointIndex & joint_id,
225  const pinocchio::ReferenceFrame reference_frame,
226  const pinocchio::ContactType contact_type,
227  const pinocchio::SE3 & placement = pinocchio::SE3::Identity())
228 {
229  PINOCCHIO_UNUSED_VARIABLE(model);
230  using namespace pinocchio;
231  Motion res(Motion::Zero());
232 
233  const Data::SE3 & oMi = data.oMi[joint_id];
234  const Data::SE3 oMc = oMi * placement;
235 
236  const Data::SE3 & iMc = placement;
237  const Motion ov = oMi.act(data.v[joint_id]);
238  const Motion oa = oMi.act(data.a[joint_id]);
239 
240  switch (reference_frame)
241  {
242  case WORLD:
243  if (contact_type == CONTACT_6D)
244  return oa;
245  classicAcceleration(ov, oa, res.linear());
246  res.angular() = oa.angular();
247  break;
248  case LOCAL_WORLD_ALIGNED:
249  if (contact_type == CONTACT_6D)
250  {
251  res.linear() = oMc.rotation() * iMc.actInv(data.a[joint_id]).linear();
252  res.angular() = oMi.rotation() * data.a[joint_id].angular();
253  }
254  else
255  {
256  res.linear() = oMc.rotation() * classicAcceleration(data.v[joint_id], data.a[joint_id], iMc);
257  res.angular() = oMi.rotation() * data.a[joint_id].angular();
258  }
259  break;
260  case LOCAL:
261  if (contact_type == CONTACT_6D)
262  return oMc.actInv(oa);
263  classicAcceleration(data.v[joint_id], data.a[joint_id], iMc, res.linear());
264  res.angular() = iMc.rotation().transpose() * data.a[joint_id].angular();
265  break;
266  default:
267  break;
268  }
269 
270  return res;
271 }
272 
273 pinocchio::Motion getContactAcceleration(
274  const Model & model,
275  const Data & data,
276  const RigidConstraintModel & cmodel,
277  const pinocchio::SE3 & c1Mc2 = SE3::Identity())
278 {
279  const Motion v1 = getFrameVelocity(
280  model, data, cmodel.joint1_id, cmodel.joint1_placement, cmodel.reference_frame);
281  const Motion v2 = getFrameVelocity(
282  model, data, cmodel.joint2_id, cmodel.joint2_placement, cmodel.reference_frame);
283  const Motion v = v1 - c1Mc2.act(v2);
284  const Motion a1 = computeAcceleration(
285  model, data, cmodel.joint1_id, cmodel.reference_frame, cmodel.type, cmodel.joint1_placement);
286  const Motion a2 = computeAcceleration(
287  model, data, cmodel.joint2_id, cmodel.reference_frame, cmodel.type, cmodel.joint2_placement);
288  return a1 - c1Mc2.act(a2) + v.cross(c1Mc2.act(v2));
289 }
290 
291 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_fd)
292 {
293  using namespace Eigen;
294  using namespace pinocchio;
295 
296  Model model;
297  buildModels::humanoidRandom(model, true);
298  Data data(model), data_fd(model);
299 
300  model.lowerPositionLimit.head<3>().fill(-1.);
301  model.upperPositionLimit.head<3>().fill(1.);
302  VectorXd q = randomConfiguration(model);
303 
304  VectorXd v = VectorXd::Random(model.nv);
305  VectorXd tau = VectorXd::Random(model.nv);
306 
307  const std::string LF = "lleg6_joint";
308  const Model::JointIndex LF_id = model.getJointId(LF);
309 
310  // Contact models and data
311  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
312  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
313 
314  RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL);
315  ci_LF.joint1_placement.setRandom();
316  ci_LF.corrector.Kp.array() = KP;
317  ci_LF.corrector.Kd.array() = KD;
318 
319  constraint_models.push_back(ci_LF);
320  constraint_data.push_back(RigidConstraintData(ci_LF));
321 
322  Eigen::DenseIndex constraint_dim = 0;
323  for (size_t k = 0; k < constraint_models.size(); ++k)
324  constraint_dim += constraint_models[k].size();
325 
326  const double mu0 = 0.;
327  ProximalSettings prox_settings(1e-12, mu0, 1);
328 
329  initConstraintDynamics(model, data, constraint_models);
330  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
331  const Data::TangentVectorType a = data.ddq;
332  computeConstraintDynamicsDerivatives(
333  model, data, constraint_models, constraint_data, prox_settings);
334 
335  // Data_fd
336  initConstraintDynamics(model, data_fd, constraint_models);
337 
338  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
339  ddq_partial_dq_fd.setZero();
340  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
341  ddq_partial_dv_fd.setZero();
342  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
343  ddq_partial_dtau_fd.setZero();
344 
345  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
346  lambda_partial_dtau_fd.setZero();
347  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
348  lambda_partial_dq_fd.setZero();
349  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
350  lambda_partial_dv_fd.setZero();
351 
352  const VectorXd ddq0 = constraintDynamics(
353  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
354  const VectorXd lambda0 = data_fd.lambda_c;
355  VectorXd v_eps(VectorXd::Zero(model.nv));
356  VectorXd q_plus(model.nq);
357  VectorXd ddq_plus(model.nv);
358 
359  VectorXd lambda_plus(constraint_dim);
360 
361  const double alpha = 1e-8;
362  forwardKinematics(model, data, q, v, a);
363 
364  for (int k = 0; k < model.nv; ++k)
365  {
366  v_eps[k] += alpha;
367  q_plus = integrate(model, q, v_eps);
368  ddq_plus = constraintDynamics(
369  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
370  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
371  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
372  v_eps[k] = 0.;
373  }
374 
375  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
376  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
377 
378  VectorXd v_plus(v);
379  for (int k = 0; k < model.nv; ++k)
380  {
381  v_plus[k] += alpha;
382  ddq_plus = constraintDynamics(
383  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
384  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
385  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
386  v_plus[k] -= alpha;
387  }
388 
389  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
390  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
391 
392  VectorXd tau_plus(tau);
393  for (int k = 0; k < model.nv; ++k)
394  {
395  tau_plus[k] += alpha;
396  ddq_plus = constraintDynamics(
397  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
398  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
399  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
400  tau_plus[k] -= alpha;
401  }
402 
403  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
404  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
405 }
406 
407 PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData)
408 createData(
409  const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel)
410  & constraint_models)
411 {
412  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) constraint_datas;
413  for (size_t k = 0; k < constraint_models.size(); ++k)
414  constraint_datas.push_back(pinocchio::RigidConstraintData(constraint_models[k]));
415 
416  return constraint_datas;
417 }
418 
419 BOOST_AUTO_TEST_CASE(test_correction_6D)
420 {
421  using namespace Eigen;
422  using namespace pinocchio;
423 
424  Model model;
425  buildModels::humanoidRandom(model, true);
426  Data data(model), data_fd(model);
427 
428  model.lowerPositionLimit.head<3>().fill(-1.);
429  model.upperPositionLimit.head<3>().fill(1.);
430  VectorXd q = randomConfiguration(model);
431 
432  VectorXd v = VectorXd::Random(model.nv);
433  VectorXd tau = VectorXd::Random(model.nv);
434  const double mu = 0.;
435  ProximalSettings prox_settings(1e-12, mu, 1);
436 
437  const std::string RF = "rleg6_joint";
438  const Model::JointIndex RF_id = model.getJointId(RF);
439  const std::string LF = "lleg6_joint";
440  const Model::JointIndex LF_id = model.getJointId(LF);
441 
442  // Contact models and data
443  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
444 
445  RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL);
446  ci_RF.joint1_placement.setRandom();
447  ci_RF.joint2_placement.setRandom();
448  ci_RF.corrector.Kp.array() = KP;
449  ci_RF.corrector.Kd.array() = KD;
450  constraint_models.push_back(ci_RF);
451 
452  RigidConstraintModel ci_LF(CONTACT_3D, model, LF_id, LOCAL);
453  ci_LF.joint1_placement.setRandom();
454  ci_LF.joint2_placement.setRandom();
455  ci_LF.corrector.Kp.array() = KP;
456  ci_LF.corrector.Kd.array() = KD;
457  constraint_models.push_back(ci_LF);
458 
459  Eigen::DenseIndex constraint_dim = 0;
460  for (size_t k = 0; k < constraint_models.size(); ++k)
461  constraint_dim += constraint_models[k].size();
462 
463  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData)
464  constraint_datas = createData(constraint_models);
465  initConstraintDynamics(model, data, constraint_models);
466  const Eigen::VectorXd ddq0 =
467  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_datas, prox_settings);
468 
469  Eigen::MatrixXd ddq_dq(model.nv, model.nv), ddq_dv(model.nv, model.nv),
470  ddq_dtau(model.nv, model.nv);
471  Eigen::MatrixXd dlambda_dq(constraint_dim, model.nv), dlambda_dv(constraint_dim, model.nv),
472  dlambda_dtau(constraint_dim, model.nv);
473 
474  computeConstraintDynamicsDerivatives(
475  model, data, constraint_models, constraint_datas, prox_settings, ddq_dq, ddq_dv, ddq_dtau,
476  dlambda_dq, dlambda_dv, dlambda_dtau);
477  computeForwardKinematicsDerivatives(model, data, q, v, 0 * v);
478 
479  Data::Matrix6x dv_RF_dq_L(Data::Matrix6x::Zero(6, model.nv));
480  Data::Matrix6x dv_RF_dv_L(Data::Matrix6x::Zero(6, model.nv));
481  getFrameVelocityDerivatives(
482  model, data, ci_RF.joint1_id, ci_RF.joint1_placement, ci_RF.reference_frame, dv_RF_dq_L,
483  dv_RF_dv_L);
484 
485  Data::Matrix6x dv_LF_dq_L(Data::Matrix6x::Zero(6, model.nv));
486  Data::Matrix6x dv_LF_dv_L(Data::Matrix6x::Zero(6, model.nv));
487  getFrameVelocityDerivatives(
488  model, data, ci_LF.joint1_id, ci_LF.joint1_placement, ci_LF.reference_frame, dv_LF_dq_L,
489  dv_LF_dv_L);
490 
491  const double eps = 1e-8;
492  Data::Matrix6x dacc_corrector_RF_dq(6, model.nv);
493  dacc_corrector_RF_dq.setZero();
494  Data::Matrix6x dacc_corrector_RF_dv(6, model.nv);
495  dacc_corrector_RF_dv.setZero();
496  Data::Matrix3x dacc_corrector_LF_dq(3, model.nv);
497  dacc_corrector_LF_dq.setZero();
498  Data::Matrix3x dacc_corrector_LF_dv(3, model.nv);
499  dacc_corrector_LF_dv.setZero();
500 
501  {
502  const SE3::Matrix6 Jlog = Jlog6(constraint_datas[0].c1Mc2.inverse());
503  dacc_corrector_RF_dq = -(ci_RF.corrector.Kp.asDiagonal() * Jlog * dv_RF_dv_L);
504  dacc_corrector_RF_dq -= ci_RF.corrector.Kd.asDiagonal() * dv_RF_dq_L;
505 
506  dacc_corrector_RF_dv = -(ci_RF.corrector.Kd.asDiagonal() * dv_RF_dv_L);
507  BOOST_CHECK(dv_RF_dv_L.isApprox(data.contact_chol.matrix().topRightCorner(6, model.nv)));
508  }
509 
510  {
511  dacc_corrector_LF_dq = -(ci_LF.corrector.Kp.asDiagonal() * dv_LF_dv_L.topRows<3>());
512  dacc_corrector_LF_dq -= ci_LF.corrector.Kd.asDiagonal() * dv_LF_dq_L.topRows<3>();
513  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
514  {
515  dacc_corrector_LF_dq.col(k) +=
516  ci_LF.corrector.Kp.asDiagonal()
517  * dv_LF_dv_L.col(k).tail<3>().cross(constraint_datas[1].contact_placement_error.linear());
518  }
519 
520  dacc_corrector_LF_dv = -(ci_LF.corrector.Kd.asDiagonal() * dv_LF_dv_L.topRows<3>());
521  BOOST_CHECK(dv_LF_dv_L.topRows<3>().isApprox(
522  data.contact_chol.matrix().topRightCorner(9, model.nv).bottomRows<3>()));
523  }
524 
525  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData)
526  constraint_datas_fd = createData(constraint_models);
527  initConstraintDynamics(model, data_fd, constraint_models);
528 
529  Data::Matrix6x dacc_corrector_RF_dq_fd(6, model.nv);
530  Data::Matrix3x dacc_corrector_LF_dq_fd(3, model.nv);
531 
532  Eigen::MatrixXd ddq_dq_fd(model.nv, model.nv), ddq_dv_fd(model.nv, model.nv),
533  ddq_dtau_fd(model.nv, model.nv);
534  Eigen::MatrixXd dlambda_dq_fd(constraint_dim, model.nv), dlambda_dv_fd(constraint_dim, model.nv),
535  dlambda_dtau_fd(constraint_dim, model.nv);
536 
537  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
538  {
539  Eigen::VectorXd v_eps = Eigen::VectorXd::Zero(model.nv);
540  v_eps[k] = eps;
541  const Eigen::VectorXd q_plus = integrate(model, q, v_eps);
542 
543  Eigen::VectorXd ddq_plus = constraintDynamics(
544  model, data_fd, q_plus, v, tau, constraint_models, constraint_datas_fd, prox_settings);
545  dacc_corrector_RF_dq_fd.col(k) = (constraint_datas_fd[0].contact_acceleration_error
546  - constraint_datas[0].contact_acceleration_error)
547  .toVector()
548  / eps;
549  dacc_corrector_LF_dq_fd.col(k) = (constraint_datas_fd[1].contact_acceleration_error.linear()
550  - constraint_datas[1].contact_acceleration_error.linear())
551  / eps;
552 
553  ddq_dq_fd.col(k) = (ddq_plus - ddq0) / eps;
554  }
555 
556  BOOST_CHECK(ddq_dq_fd.isApprox(ddq_dq, sqrt(eps)));
557  // std::cout << "ddq_dq_fd:\n" << ddq_dq_fd - ddq_dq << std::endl;
558  // std::cout << "ddq_dq:\n" << ddq_dq << std::endl;
559  BOOST_CHECK(dacc_corrector_RF_dq.isApprox(dacc_corrector_RF_dq_fd, sqrt(eps)));
560  BOOST_CHECK(dacc_corrector_LF_dq.isApprox(dacc_corrector_LF_dq_fd, sqrt(eps)));
561  // std::cout << "dacc_corrector_RF_dq:\n" << dacc_corrector_RF_dq << std::endl;
562  // std::cout << "dacc_corrector_RF_dq_fd:\n" << dacc_corrector_RF_dq_fd << std::endl;
563 
564  Data::Matrix6x dacc_corrector_RF_dv_fd(6, model.nv);
565  Data::Matrix3x dacc_corrector_LF_dv_fd(3, model.nv);
566  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
567  {
568  Eigen::VectorXd v_plus(v);
569  v_plus[k] += eps;
570 
571  Eigen::VectorXd ddq_plus = constraintDynamics(
572  model, data_fd, q, v_plus, tau, constraint_models, constraint_datas_fd, prox_settings);
573  dacc_corrector_RF_dv_fd.col(k) = (constraint_datas_fd[0].contact_acceleration_error
574  - constraint_datas[0].contact_acceleration_error)
575  .toVector()
576  / eps;
577  dacc_corrector_LF_dv_fd.col(k) = (constraint_datas_fd[1].contact_acceleration_error.linear()
578  - constraint_datas[1].contact_acceleration_error.linear())
579  / eps;
580 
581  ddq_dv_fd.col(k) = (ddq_plus - ddq0) / eps;
582  }
583  BOOST_CHECK(ddq_dv_fd.isApprox(ddq_dv, sqrt(eps)));
584  BOOST_CHECK(dacc_corrector_RF_dv.isApprox(dacc_corrector_RF_dv_fd, sqrt(eps)));
585  BOOST_CHECK(dacc_corrector_LF_dv.isApprox(dacc_corrector_LF_dv_fd, sqrt(eps)));
586 
587  Data::Matrix6x dacc_corrector_RF_dtau_fd(6, model.nv);
588  Data::Matrix3x dacc_corrector_LF_dtau_fd(3, model.nv);
589  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
590  {
591  Eigen::VectorXd tau_plus(tau);
592  tau_plus[k] += eps;
593 
594  Eigen::VectorXd ddq_plus = constraintDynamics(
595  model, data_fd, q, v, tau_plus, constraint_models, constraint_datas_fd, prox_settings);
596  dacc_corrector_RF_dtau_fd.col(k) = (constraint_datas_fd[0].contact_acceleration_error
597  - constraint_datas[0].contact_acceleration_error)
598  .toVector()
599  / eps;
600  dacc_corrector_LF_dtau_fd.col(k) = (constraint_datas_fd[1].contact_acceleration_error.linear()
601  - constraint_datas[1].contact_acceleration_error.linear())
602  / eps;
603 
604  ddq_dtau_fd.col(k) = (ddq_plus - ddq0) / eps;
605  }
606  BOOST_CHECK(ddq_dtau_fd.isApprox(ddq_dtau, sqrt(eps)));
607  BOOST_CHECK(dacc_corrector_RF_dtau_fd.isZero(sqrt(eps)));
608  BOOST_CHECK(dacc_corrector_LF_dtau_fd.isZero(sqrt(eps)));
609 }
610 
611 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_fd)
612 {
613  using namespace Eigen;
614  using namespace pinocchio;
615 
616  Model model;
617  buildModels::humanoidRandom(model, true);
618  Data data(model), data_fd(model);
619 
620  model.lowerPositionLimit.head<3>().fill(-1.);
621  model.upperPositionLimit.head<3>().fill(1.);
622  VectorXd q = randomConfiguration(model);
623 
624  VectorXd v = VectorXd::Random(model.nv);
625  VectorXd tau = VectorXd::Random(model.nv);
626 
627  const std::string RF = "rleg6_joint";
628  const Model::JointIndex RF_id = model.getJointId(RF);
629  const std::string LF = "lleg6_joint";
630  // const Model::JointIndex LF_id = model.getJointId(LF);
631 
632  // Contact models and data
633  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
634  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
635 
636  RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL);
637  ci_RF.joint1_placement.setRandom();
638  ci_RF.corrector.Kp.array() = KP;
639  ci_RF.corrector.Kd.array() = KD;
640  constraint_models.push_back(ci_RF);
641  constraint_data.push_back(RigidConstraintData(ci_RF));
642 
643  Eigen::DenseIndex constraint_dim = 0;
644  for (size_t k = 0; k < constraint_models.size(); ++k)
645  constraint_dim += constraint_models[k].size();
646 
647  const double mu0 = 0.;
648  ProximalSettings prox_settings(1e-12, mu0, 1);
649 
650  initConstraintDynamics(model, data, constraint_models);
651  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
652  const Data::TangentVectorType a = data.ddq;
653  data.M.triangularView<Eigen::StrictlyLower>() =
654  data.M.transpose().triangularView<Eigen::StrictlyLower>();
655  computeConstraintDynamicsDerivatives(
656  model, data, constraint_models, constraint_data, prox_settings);
657 
658  // Data_fd
659  initConstraintDynamics(model, data_fd, constraint_models);
660 
661  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
662  ddq_partial_dq_fd.setZero();
663  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
664  ddq_partial_dv_fd.setZero();
665  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
666  ddq_partial_dtau_fd.setZero();
667 
668  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
669  lambda_partial_dtau_fd.setZero();
670  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
671  lambda_partial_dq_fd.setZero();
672  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
673  lambda_partial_dv_fd.setZero();
674 
675  const VectorXd ddq0 = constraintDynamics(
676  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
677  const VectorXd lambda0 = data_fd.lambda_c;
678  VectorXd v_eps(VectorXd::Zero(model.nv));
679  VectorXd q_plus(model.nq);
680  VectorXd ddq_plus(model.nv);
681 
682  VectorXd lambda_plus(constraint_dim);
683 
684  const double alpha = 1e-8;
685  forwardKinematics(model, data, q, v, a);
686 
687  for (int k = 0; k < model.nv; ++k)
688  {
689  v_eps[k] += alpha;
690  q_plus = integrate(model, q, v_eps);
691  ddq_plus = constraintDynamics(
692  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
693  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
694  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
695  v_eps[k] = 0.;
696  }
697 
698  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
699  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
700 
701  VectorXd v_plus(v);
702  for (int k = 0; k < model.nv; ++k)
703  {
704  v_plus[k] += alpha;
705  ddq_plus = constraintDynamics(
706  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
707  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
708  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
709  v_plus[k] -= alpha;
710  }
711 
712  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
713  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
714 
715  VectorXd tau_plus(tau);
716  for (int k = 0; k < model.nv; ++k)
717  {
718  tau_plus[k] += alpha;
719  ddq_plus = constraintDynamics(
720  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
721  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
722  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
723  tau_plus[k] -= alpha;
724  }
725 
726  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
727  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
728 }
729 
730 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_fd_prox)
731 {
732  using namespace Eigen;
733  using namespace pinocchio;
734 
735  Model model;
736  buildModels::humanoidRandom(model, true);
737  Data data(model), data_fd(model);
738 
739  model.lowerPositionLimit.head<3>().fill(-1.);
740  model.upperPositionLimit.head<3>().fill(1.);
741  VectorXd q = randomConfiguration(model);
742 
743  VectorXd v = VectorXd::Random(model.nv);
744  VectorXd tau = VectorXd::Random(model.nv);
745 
746  const std::string RF = "rleg6_joint";
747  const Model::JointIndex RF_id = model.getJointId(RF);
748  const std::string LF = "lleg6_joint";
749  // const Model::JointIndex LF_id = model.getJointId(LF);
750 
751  // Contact models and data
752  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
753  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
754 
755  RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL);
756  ci_RF.joint1_placement.setRandom();
757  ci_RF.corrector.Kp.array() = KP;
758  ci_RF.corrector.Kd.array() = KD;
759  constraint_models.push_back(ci_RF);
760  constraint_data.push_back(RigidConstraintData(ci_RF));
761 
762  Eigen::DenseIndex constraint_dim = 0;
763  for (size_t k = 0; k < constraint_models.size(); ++k)
764  constraint_dim += constraint_models[k].size();
765 
766  const double mu0 = 1e-4;
767  ProximalSettings prox_settings(1e-12, mu0, 20);
768 
769  initConstraintDynamics(model, data, constraint_models);
770  VectorXd a_res =
771  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
772  BOOST_CHECK(prox_settings.iter > 1 && prox_settings.iter <= prox_settings.max_iter);
773  const Data::TangentVectorType a = data.ddq;
774  data.M.triangularView<Eigen::StrictlyLower>() =
775  data.M.transpose().triangularView<Eigen::StrictlyLower>();
776  computeConstraintDynamicsDerivatives(
777  model, data, constraint_models, constraint_data, prox_settings);
778 
779  ProximalSettings prox_settings_fd(1e-12, mu0, 20);
780  // Data_fd
781  initConstraintDynamics(model, data_fd, constraint_models);
782 
783  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
784  ddq_partial_dq_fd.setZero();
785  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
786  ddq_partial_dv_fd.setZero();
787  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
788  ddq_partial_dtau_fd.setZero();
789 
790  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
791  lambda_partial_dtau_fd.setZero();
792  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
793  lambda_partial_dq_fd.setZero();
794  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
795  lambda_partial_dv_fd.setZero();
796 
797  const VectorXd ddq0 = constraintDynamics(
798  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings_fd);
799  BOOST_CHECK(a_res.isApprox(ddq0));
800  const VectorXd lambda0 = data_fd.lambda_c;
801 
802  computeConstraintDynamicsDerivatives(
803  model, data_fd, constraint_models, constraint_data, prox_settings_fd);
804  BOOST_CHECK(data_fd.dlambda_dtau.isApprox(data.dlambda_dtau));
805 
806  VectorXd v_eps(VectorXd::Zero(model.nv));
807  VectorXd q_plus(model.nq);
808  VectorXd ddq_plus(model.nv);
809 
810  VectorXd lambda_plus(constraint_dim);
811 
812  const double alpha = 1e-8;
813  forwardKinematics(model, data, q, v, a);
814 
815  for (int k = 0; k < model.nv; ++k)
816  {
817  v_eps[k] += alpha;
818  q_plus = integrate(model, q, v_eps);
819  ddq_plus = constraintDynamics(
820  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings_fd);
821  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
822  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
823  v_eps[k] = 0.;
824  }
825 
826  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
827  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
828 
829  VectorXd v_plus(v);
830  for (int k = 0; k < model.nv; ++k)
831  {
832  v_plus[k] += alpha;
833  ddq_plus = constraintDynamics(
834  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings_fd);
835  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
836  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
837  v_plus[k] -= alpha;
838  }
839 
840  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
841  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
842 
843  VectorXd tau_plus(tau);
844  for (int k = 0; k < model.nv; ++k)
845  {
846  tau_plus[k] += alpha;
847  ddq_plus = constraintDynamics(
848  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings_fd);
849  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
850  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
851  tau_plus[k] -= alpha;
852  }
853 
854  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
855  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
856 }
857 
858 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_loop_closure_3D_fd_prox)
859 {
860  using namespace Eigen;
861  using namespace pinocchio;
862 
863  Model model;
864  buildModels::humanoidRandom(model, true);
865  Data data(model), data_fd(model);
866 
867  model.lowerPositionLimit.head<3>().fill(-1.);
868  model.upperPositionLimit.head<3>().fill(1.);
869  VectorXd q = randomConfiguration(model);
870 
871  VectorXd v = VectorXd::Random(model.nv);
872  VectorXd tau = VectorXd::Random(model.nv);
873 
874  const std::string RF = "rleg6_joint";
875  const Model::JointIndex RF_id = model.getJointId(RF);
876  const std::string LF = "lleg6_joint";
877  const Model::JointIndex LF_id = model.getJointId(LF);
878 
879  // Contact models and data
880  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
881  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
882 
883  RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LF_id, LOCAL);
884  ci_RF.corrector.Kp.array() = KP;
885  ci_RF.corrector.Kd.array() = KD;
886  ci_RF.joint1_placement.setRandom();
887  forwardKinematics(model, data, q);
888  // data.oMi[LF_id] * ci_RF.joint2_placement = data.oMi[RF_id] * ci_RF.joint1_placement;
889  ci_RF.joint2_placement = data.oMi[LF_id].inverse() * data.oMi[RF_id] * ci_RF.joint1_placement;
890 
891  constraint_models.push_back(ci_RF);
892  constraint_data.push_back(RigidConstraintData(ci_RF));
893 
894  Eigen::DenseIndex constraint_dim = 0;
895  for (size_t k = 0; k < constraint_models.size(); ++k)
896  constraint_dim += constraint_models[k].size();
897 
898  const double mu0 = 1e-4;
899  ProximalSettings prox_settings(1e-12, mu0, 20);
900 
901  initConstraintDynamics(model, data, constraint_models);
902  VectorXd a_res =
903  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
904  BOOST_CHECK(prox_settings.iter > 1 && prox_settings.iter <= prox_settings.max_iter);
905  const Data::TangentVectorType a = data.ddq;
906  data.M.triangularView<Eigen::StrictlyLower>() =
907  data.M.transpose().triangularView<Eigen::StrictlyLower>();
908  computeConstraintDynamicsDerivatives(
909  model, data, constraint_models, constraint_data, prox_settings);
910 
911  ProximalSettings prox_settings_fd(1e-12, mu0, 20);
912  // Data_fd
913  initConstraintDynamics(model, data_fd, constraint_models);
914 
915  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
916  ddq_partial_dq_fd.setZero();
917  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
918  ddq_partial_dv_fd.setZero();
919  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
920  ddq_partial_dtau_fd.setZero();
921 
922  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
923  lambda_partial_dtau_fd.setZero();
924  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
925  lambda_partial_dq_fd.setZero();
926  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
927  lambda_partial_dv_fd.setZero();
928 
929  const VectorXd ddq0 = constraintDynamics(
930  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings_fd);
931  BOOST_CHECK(a_res.isApprox(ddq0));
932  const VectorXd lambda0 = data_fd.lambda_c;
933 
934  computeConstraintDynamicsDerivatives(
935  model, data_fd, constraint_models, constraint_data, prox_settings_fd);
936  BOOST_CHECK(data_fd.dlambda_dtau.isApprox(data.dlambda_dtau));
937 
938  VectorXd v_eps(VectorXd::Zero(model.nv));
939  VectorXd q_plus(model.nq);
940  VectorXd ddq_plus(model.nv);
941 
942  VectorXd lambda_plus(constraint_dim);
943 
944  const double alpha = 1e-8;
945  forwardKinematics(model, data, q, v, a);
946 
947  for (int k = 0; k < model.nv; ++k)
948  {
949  v_eps[k] += alpha;
950  q_plus = integrate(model, q, v_eps);
951  ddq_plus = constraintDynamics(
952  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings_fd);
953  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
954  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
955  v_eps[k] = 0.;
956  }
957 
958  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
959  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
960 
961  VectorXd v_plus(v);
962  for (int k = 0; k < model.nv; ++k)
963  {
964  v_plus[k] += alpha;
965  ddq_plus = constraintDynamics(
966  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings_fd);
967  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
968  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
969  v_plus[k] -= alpha;
970  }
971 
972  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
973  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
974 
975  VectorXd tau_plus(tau);
976  for (int k = 0; k < model.nv; ++k)
977  {
978  tau_plus[k] += alpha;
979  ddq_plus = constraintDynamics(
980  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings_fd);
981  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
982  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
983  tau_plus[k] -= alpha;
984  }
985 
986  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
987  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
988 }
989 
990 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_loop_closure_j2_fd)
991 {
992  using namespace Eigen;
993  using namespace pinocchio;
994 
995  Model model;
996  buildModels::humanoidRandom(model, true);
997  Data data(model), data_fd(model);
998 
999  model.lowerPositionLimit.head<3>().fill(-1.);
1000  model.upperPositionLimit.head<3>().fill(1.);
1001  VectorXd q = randomConfiguration(model);
1002 
1003  VectorXd v = VectorXd::Random(model.nv);
1004  VectorXd tau = VectorXd::Random(model.nv);
1005 
1006  const std::string RF = "rleg6_joint";
1007  // const Model::JointIndex RF_id = model.getJointId(RF);
1008  const std::string LF = "lleg6_joint";
1009  // const Model::JointIndex LF_id = model.getJointId(LF);
1010 
1011  // Contact models and data
1012  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
1013  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
1014 
1015  // Add Loop Closure Constraint
1016 
1017  const std::string RA = "rarm5_joint";
1018  const Model::JointIndex RA_id = model.getJointId(RA);
1019  const std::string LA = "larm5_joint";
1020  // const Model::JointIndex LA_id = model.getJointId(LA);
1021 
1022  RigidConstraintModel ci_closure(
1023  CONTACT_3D, model, 0, SE3::Identity(), RA_id, SE3::Random(), LOCAL);
1024  ci_closure.corrector.Kp.array() = KP;
1025  ci_closure.corrector.Kd.array() = KD;
1026  constraint_models.push_back(ci_closure);
1027  constraint_data.push_back(RigidConstraintData(ci_closure));
1028  // End of Loopo Closure Constraint
1029 
1030  Eigen::DenseIndex constraint_dim = 0;
1031  for (size_t k = 0; k < constraint_models.size(); ++k)
1032  constraint_dim += constraint_models[k].size();
1033 
1034  const double mu0 = 0.;
1035  ProximalSettings prox_settings(1e-12, mu0, 1);
1036 
1037  initConstraintDynamics(model, data, constraint_models);
1038  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
1039  const Data::TangentVectorType a = data.ddq;
1040  data.M.triangularView<Eigen::StrictlyLower>() =
1041  data.M.transpose().triangularView<Eigen::StrictlyLower>();
1042  computeConstraintDynamicsDerivatives(
1043  model, data, constraint_models, constraint_data, prox_settings);
1044 
1045  // Data_fd
1046  initConstraintDynamics(model, data_fd, constraint_models);
1047 
1048  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
1049  ddq_partial_dq_fd.setZero();
1050  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
1051  ddq_partial_dv_fd.setZero();
1052  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
1053  ddq_partial_dtau_fd.setZero();
1054 
1055  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
1056  lambda_partial_dtau_fd.setZero();
1057  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
1058  lambda_partial_dq_fd.setZero();
1059  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
1060  lambda_partial_dv_fd.setZero();
1061 
1062  const VectorXd ddq0 = constraintDynamics(
1063  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
1064  const VectorXd lambda0 = data_fd.lambda_c;
1065  VectorXd v_eps(VectorXd::Zero(model.nv));
1066  VectorXd q_plus(model.nq);
1067  VectorXd ddq_plus(model.nv);
1068 
1069  VectorXd lambda_plus(constraint_dim);
1070 
1071  const double alpha = 1e-8;
1072  forwardKinematics(model, data, q, v, a);
1073 
1074  for (int k = 0; k < model.nv; ++k)
1075  {
1076  v_eps[k] += alpha;
1077  q_plus = integrate(model, q, v_eps);
1078  ddq_plus = constraintDynamics(
1079  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
1080  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
1081  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1082  v_eps[k] = 0.;
1083  }
1084 
1085  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
1086  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
1087 
1088  VectorXd v_plus(v);
1089  for (int k = 0; k < model.nv; ++k)
1090  {
1091  v_plus[k] += alpha;
1092  ddq_plus = constraintDynamics(
1093  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
1094  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
1095  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1096  v_plus[k] -= alpha;
1097  }
1098 
1099  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
1100  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
1101 
1102  VectorXd tau_plus(tau);
1103  for (int k = 0; k < model.nv; ++k)
1104  {
1105  tau_plus[k] += alpha;
1106  ddq_plus = constraintDynamics(
1107  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
1108  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
1109  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1110  tau_plus[k] -= alpha;
1111  }
1112 
1113  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
1114  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
1115 }
1116 
1117 void computeVelocityAndAccelerationErrors(
1118  const Model & model,
1119  const RigidConstraintModel & cmodel,
1120  const VectorXd & q,
1121  const VectorXd & v,
1122  const VectorXd & a,
1123  Motion & v_error,
1124  Motion & a_error,
1125  const VectorXd & Kp,
1126  const VectorXd & Kd)
1127 {
1128  Data data(model);
1129  forwardKinematics(model, data, q, v, a);
1130 
1131  const SE3 oMc1 = data.oMi[cmodel.joint1_id] * cmodel.joint1_placement;
1132  const SE3 oMc2 = data.oMi[cmodel.joint2_id] * cmodel.joint2_placement;
1133 
1134  const SE3 c1Mc2 = oMc1.actInv(oMc2);
1135 
1136  const Motion v1 = cmodel.joint1_placement.actInv(data.v[cmodel.joint1_id]);
1137  const Motion v2 = cmodel.joint2_placement.actInv(data.v[cmodel.joint2_id]);
1138 
1139  const Motion a1 = cmodel.joint1_placement.actInv(data.a[cmodel.joint1_id]);
1140  const Motion a2 = cmodel.joint2_placement.actInv(data.a[cmodel.joint2_id]);
1141 
1142  v_error = v1 - c1Mc2.act(v2);
1143  a_error = a1 - c1Mc2.act(a2) + v_error.cross(c1Mc2.act(v2));
1144  a_error.toVector() +=
1145  Kd.asDiagonal() * v_error.toVector() + Kp.asDiagonal() * (-log6(c1Mc2).toVector());
1146 }
1147 
1148 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_loop_closure_j2_fd)
1149 {
1150  using namespace Eigen;
1151  using namespace pinocchio;
1152 
1153  Model model;
1154  buildModels::humanoidRandom(model, true);
1155  Data data(model), data_fd(model);
1156 
1157  model.lowerPositionLimit.head<3>().fill(-1.);
1158  model.upperPositionLimit.head<3>().fill(1.);
1159  VectorXd q = randomConfiguration(model);
1160 
1161  VectorXd v = VectorXd::Random(model.nv);
1162  VectorXd tau = VectorXd::Random(model.nv);
1163 
1164  // Contact models and data
1165  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
1166  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData)
1167  constraint_data, constraint_data_fd;
1168 
1169  // Add loop closure constraint
1170  const std::string RA = "rarm5_joint";
1171  const Model::JointIndex RA_id = model.getJointId(RA);
1172 
1173  RigidConstraintModel ci_closure(
1174  CONTACT_6D, model, 0, SE3::Identity(), RA_id, SE3::Identity(), LOCAL);
1175  ci_closure.corrector.Kp.array() = KP;
1176  ci_closure.corrector.Kd.array() = KD;
1177  constraint_models.push_back(ci_closure);
1178  constraint_data.push_back(RigidConstraintData(ci_closure));
1179  constraint_data_fd.push_back(RigidConstraintData(ci_closure));
1180 
1181  Eigen::DenseIndex constraint_dim = 0;
1182  for (size_t k = 0; k < constraint_models.size(); ++k)
1183  constraint_dim += constraint_models[k].size();
1184 
1185  const double mu0 = 0.;
1186  ProximalSettings prox_settings(1e-12, mu0, 100);
1187 
1188  initConstraintDynamics(model, data, constraint_models);
1189  const VectorXd ddq0 =
1190  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
1191  BOOST_CHECK(
1192  prox_settings.absolute_residual <= prox_settings.absolute_accuracy
1193  || prox_settings.relative_residual <= prox_settings.relative_accuracy);
1194  // BOOST_CHECK(prox_settings.iter == 1);
1195 
1196  const VectorXd a = data.ddq;
1197  data.M.triangularView<Eigen::StrictlyLower>() =
1198  data.M.transpose().triangularView<Eigen::StrictlyLower>();
1199  computeConstraintDynamicsDerivatives(
1200  model, data, constraint_models, constraint_data, prox_settings);
1201 
1202  Motion v_error, a_error;
1203  computeVelocityAndAccelerationErrors(
1204  model, ci_closure, q, v, ddq0, v_error, a_error, ci_closure.corrector.Kp,
1205  ci_closure.corrector.Kd);
1206  BOOST_CHECK(a_error.isZero());
1207 
1208  const Motion constraint_velocity_error = constraint_data[0].contact_velocity_error;
1209  const VectorXd constraint_acceleration_error = -data.primal_rhs_contact.head(constraint_dim);
1210  BOOST_CHECK(constraint_velocity_error.isApprox(v_error));
1211  BOOST_CHECK(constraint_acceleration_error.isApprox(a_error.toVector() - data.dac_da * ddq0));
1212 
1213  const VectorXd lambda0 = data.lambda_c;
1214  VectorXd v_eps(VectorXd::Zero(model.nv));
1215  VectorXd q_plus(model.nq);
1216  VectorXd ddq_plus(model.nv);
1217 
1218  const double alpha = 1e-8;
1219 
1220  // d./dq
1221  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
1222  ddq_partial_dq_fd.setZero();
1223  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
1224  lambda_partial_dq_fd.setZero();
1225  MatrixXd dconstraint_velocity_error_dq_fd(6, model.nv);
1226  dconstraint_velocity_error_dq_fd.setZero();
1227  MatrixXd dconstraint_velocity_error_dq2_fd(6, model.nv);
1228  dconstraint_velocity_error_dq2_fd.setZero();
1229  MatrixXd dconstraint_acceleration_error_dq_fd(6, model.nv);
1230  dconstraint_acceleration_error_dq_fd.setZero();
1231 
1232  initConstraintDynamics(model, data_fd, constraint_models);
1233  for (int k = 0; k < model.nv; ++k)
1234  {
1235  v_eps[k] += alpha;
1236  q_plus = integrate(model, q, v_eps);
1237  ddq_plus = constraintDynamics(
1238  model, data_fd, q_plus, v, tau, constraint_models, constraint_data_fd, prox_settings);
1239 
1240  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
1241  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1242 
1243  Motion v_error_plus, a_error_plus;
1244  computeVelocityAndAccelerationErrors(
1245  model, ci_closure, q_plus, v, ddq0, v_error_plus, a_error_plus, ci_closure.corrector.Kp,
1246  ci_closure.corrector.Kd);
1247 
1248  const Motion & constraint_velocity_error_plus = constraint_data_fd[0].contact_velocity_error;
1249  const VectorXd constraint_acceleration_error_plus =
1250  -data_fd.primal_rhs_contact.head(constraint_dim);
1251  dconstraint_velocity_error_dq_fd.col(k) =
1252  (constraint_velocity_error_plus - constraint_velocity_error).toVector() / alpha;
1253  dconstraint_velocity_error_dq2_fd.col(k) = (v_error_plus - v_error).toVector() / alpha;
1254  dconstraint_acceleration_error_dq_fd.col(k) = (a_error_plus - a_error).toVector() / alpha;
1255 
1256  v_eps[k] = 0.;
1257  }
1258 
1259  BOOST_CHECK(dconstraint_velocity_error_dq_fd.isApprox(data.dvc_dq, sqrt(alpha)));
1260  BOOST_CHECK(dconstraint_acceleration_error_dq_fd.isApprox(data.dac_dq, sqrt(alpha)));
1261  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
1262  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
1263 
1264  // d./dv
1265  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
1266  ddq_partial_dv_fd.setZero();
1267  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
1268  lambda_partial_dv_fd.setZero();
1269  MatrixXd dconstraint_velocity_error_dv_fd(6, model.nv);
1270  dconstraint_velocity_error_dv_fd.setZero();
1271  MatrixXd dconstraint_acceleration_error_dv_fd(6, model.nv);
1272  dconstraint_acceleration_error_dv_fd.setZero();
1273 
1274  VectorXd v_plus(v);
1275  for (int k = 0; k < model.nv; ++k)
1276  {
1277  v_plus[k] += alpha;
1278  ddq_plus = constraintDynamics(
1279  model, data_fd, q, v_plus, tau, constraint_models, constraint_data_fd, prox_settings);
1280 
1281  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
1282  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1283 
1284  Motion v_error_plus, a_error_plus;
1285  computeVelocityAndAccelerationErrors(
1286  model, ci_closure, q, v_plus, ddq0, v_error_plus, a_error_plus, ci_closure.corrector.Kp,
1287  ci_closure.corrector.Kd);
1288 
1289  const Motion & constraint_velocity_error_plus = constraint_data_fd[0].contact_velocity_error;
1290  dconstraint_velocity_error_dv_fd.col(k) =
1291  (constraint_velocity_error_plus - constraint_velocity_error).toVector() / alpha;
1292  dconstraint_acceleration_error_dv_fd.col(k) = (a_error_plus - a_error).toVector() / alpha;
1293 
1294  v_plus[k] -= alpha;
1295  }
1296 
1297  BOOST_CHECK(dconstraint_velocity_error_dv_fd.isApprox(data.dac_da, sqrt(alpha)));
1298  BOOST_CHECK(dconstraint_acceleration_error_dv_fd.isApprox(data.dac_dv, sqrt(alpha)));
1299  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
1300  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
1301 
1302  // d./dtau
1303  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
1304  ddq_partial_dtau_fd.setZero();
1305  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
1306  lambda_partial_dtau_fd.setZero();
1307 
1308  VectorXd tau_plus(tau);
1309  for (int k = 0; k < model.nv; ++k)
1310  {
1311  tau_plus[k] += alpha;
1312  ddq_plus = constraintDynamics(
1313  model, data_fd, q, v, tau_plus, constraint_models, constraint_data_fd, prox_settings);
1314  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
1315  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1316  tau_plus[k] -= alpha;
1317  }
1318 
1319  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
1320  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
1321 }
1322 
1323 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_loop_closure_j1j2_fd)
1324 {
1325  using namespace Eigen;
1326  using namespace pinocchio;
1327 
1328  Model model;
1329  buildModels::humanoidRandom(model, true);
1330  Data data(model), data_fd(model);
1331 
1332  model.lowerPositionLimit.head<3>().fill(-1.);
1333  model.upperPositionLimit.head<3>().fill(1.);
1334  VectorXd q = randomConfiguration(model);
1335 
1336  VectorXd v = VectorXd::Random(model.nv);
1337  VectorXd tau = VectorXd::Random(model.nv);
1338 
1339  const std::string RF = "rleg6_joint";
1340  const std::string LF = "lleg6_joint";
1341 
1342  // Contact models and data
1343  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
1344  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData)
1345  constraint_data, constraint_data_fd;
1346 
1347  const std::string RA = "rarm5_joint";
1348  const Model::JointIndex RA_id = model.getJointId(RA);
1349  const std::string LA = "larm5_joint";
1350  const Model::JointIndex LA_id = model.getJointId(LA);
1351 
1352  // Add loop closure constraint
1353  RigidConstraintModel ci_closure(
1354  CONTACT_6D, model, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL);
1355  ci_closure.corrector.Kp.array() = KP;
1356  ci_closure.corrector.Kd.array() = KD;
1357 
1358  constraint_models.push_back(ci_closure);
1359  constraint_data.push_back(RigidConstraintData(ci_closure));
1360  constraint_data_fd.push_back(RigidConstraintData(ci_closure));
1361 
1362  Eigen::DenseIndex constraint_dim = 0;
1363  for (size_t k = 0; k < constraint_models.size(); ++k)
1364  constraint_dim += constraint_models[k].size();
1365 
1366  const double mu0 = 0.;
1367  ProximalSettings prox_settings(1e-12, mu0, 100);
1368 
1369  initConstraintDynamics(model, data, constraint_models);
1370  const VectorXd ddq0 =
1371  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
1372  const VectorXd lambda0 = data.lambda_c;
1373 
1374  computeConstraintDynamicsDerivatives(
1375  model, data, constraint_models, constraint_data, prox_settings);
1376 
1377  Motion v_error, a_error;
1378  computeVelocityAndAccelerationErrors(
1379  model, ci_closure, q, v, ddq0, v_error, a_error, ci_closure.corrector.Kp,
1380  ci_closure.corrector.Kd);
1381  BOOST_CHECK(a_error.isZero());
1382 
1383  const Motion constraint_velocity_error = constraint_data[0].contact_velocity_error;
1384  const VectorXd constraint_acceleration_error = -data.primal_rhs_contact.head(constraint_dim);
1385  BOOST_CHECK(constraint_velocity_error.isApprox(v_error));
1386  BOOST_CHECK(constraint_acceleration_error.isApprox(a_error.toVector() - data.dac_da * ddq0));
1387 
1388  // Data_fd
1389  initConstraintDynamics(model, data_fd, constraint_models);
1390 
1391  VectorXd v_eps(VectorXd::Zero(model.nv));
1392  VectorXd q_plus(model.nq);
1393  VectorXd ddq_plus(model.nv);
1394 
1395  VectorXd lambda_plus(constraint_dim);
1396 
1397  const double alpha = 1e-8;
1398 
1399  // d./dq
1400  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
1401  ddq_partial_dq_fd.setZero();
1402  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
1403  lambda_partial_dq_fd.setZero();
1404  MatrixXd dconstraint_velocity_error_dq_fd(6, model.nv);
1405  dconstraint_velocity_error_dq_fd.setZero();
1406  MatrixXd dconstraint_acceleration_error_dq_fd(6, model.nv);
1407  dconstraint_acceleration_error_dq_fd.setZero();
1408 
1409  for (int k = 0; k < model.nv; ++k)
1410  {
1411  v_eps[k] += alpha;
1412 
1413  q_plus = integrate(model, q, v_eps);
1414  ddq_plus = constraintDynamics(
1415  model, data_fd, q_plus, v, tau, constraint_models, constraint_data_fd, prox_settings);
1416  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
1417  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1418 
1419  Motion v_error_plus, a_error_plus;
1420  computeVelocityAndAccelerationErrors(
1421  model, ci_closure, q_plus, v, ddq0, v_error_plus, a_error_plus, ci_closure.corrector.Kp,
1422  ci_closure.corrector.Kd);
1423 
1424  const Motion & constraint_velocity_error_plus = constraint_data_fd[0].contact_velocity_error;
1425  const VectorXd constraint_acceleration_error_plus =
1426  -data_fd.primal_rhs_contact.head(constraint_dim);
1427  dconstraint_velocity_error_dq_fd.col(k) =
1428  (constraint_velocity_error_plus - constraint_velocity_error).toVector() / alpha;
1429  dconstraint_acceleration_error_dq_fd.col(k) = (a_error_plus - a_error).toVector() / alpha;
1430 
1431  v_eps[k] = 0.;
1432  }
1433 
1434  BOOST_CHECK(dconstraint_velocity_error_dq_fd.isApprox(data.dvc_dq, sqrt(alpha)));
1435  BOOST_CHECK(dconstraint_acceleration_error_dq_fd.isApprox(data.dac_dq, sqrt(alpha)));
1436 
1437  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
1438  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
1439 
1440  // d./dv
1441  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
1442  ddq_partial_dv_fd.setZero();
1443  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
1444  lambda_partial_dv_fd.setZero();
1445  MatrixXd dconstraint_velocity_error_dv_fd(6, model.nv);
1446  dconstraint_velocity_error_dv_fd.setZero();
1447  MatrixXd dconstraint_acceleration_error_dv_fd(6, model.nv);
1448  dconstraint_acceleration_error_dv_fd.setZero();
1449 
1450  VectorXd v_plus(v);
1451  for (int k = 0; k < model.nv; ++k)
1452  {
1453  v_plus[k] += alpha;
1454 
1455  ddq_plus = constraintDynamics(
1456  model, data_fd, q, v_plus, tau, constraint_models, constraint_data_fd, prox_settings);
1457  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
1458  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1459 
1460  Motion v_error_plus, a_error_plus;
1461  computeVelocityAndAccelerationErrors(
1462  model, ci_closure, q, v_plus, ddq0, v_error_plus, a_error_plus, ci_closure.corrector.Kp,
1463  ci_closure.corrector.Kd);
1464 
1465  const Motion & constraint_velocity_error_plus = constraint_data_fd[0].contact_velocity_error;
1466  dconstraint_velocity_error_dv_fd.col(k) =
1467  (constraint_velocity_error_plus - constraint_velocity_error).toVector() / alpha;
1468  dconstraint_acceleration_error_dv_fd.col(k) = (a_error_plus - a_error).toVector() / alpha;
1469 
1470  v_plus[k] -= alpha;
1471  }
1472 
1473  BOOST_CHECK(dconstraint_velocity_error_dv_fd.isApprox(data.dac_da, sqrt(alpha)));
1474  BOOST_CHECK(dconstraint_acceleration_error_dv_fd.isApprox(data.dac_dv, sqrt(alpha)));
1475 
1476  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
1477  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
1478 
1479  // d./dtau
1480  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
1481  ddq_partial_dtau_fd.setZero();
1482  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
1483  lambda_partial_dtau_fd.setZero();
1484 
1485  VectorXd tau_plus(tau);
1486  for (int k = 0; k < model.nv; ++k)
1487  {
1488  tau_plus[k] += alpha;
1489  ddq_plus = constraintDynamics(
1490  model, data_fd, q, v, tau_plus, constraint_models, constraint_data_fd, prox_settings);
1491  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
1492  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1493  tau_plus[k] -= alpha;
1494  }
1495 
1496  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
1497  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
1498 }
1499 
1500 BOOST_AUTO_TEST_CASE(
1501  test_constraint_dynamics_derivatives_LOCAL_WORL_ALIGNED_6D_loop_closure_j1j2_fd)
1502 {
1503  using namespace Eigen;
1504  using namespace pinocchio;
1505 
1506  Model model;
1507  buildModels::humanoidRandom(model, true);
1508  Data data(model), data_fd(model);
1509 
1510  model.lowerPositionLimit.head<3>().fill(-1.);
1511  model.upperPositionLimit.head<3>().fill(1.);
1512  VectorXd q = randomConfiguration(model);
1513 
1514  VectorXd v = VectorXd::Random(model.nv);
1515  VectorXd tau = VectorXd::Random(model.nv);
1516 
1517  const std::string RF = "rleg6_joint";
1518  // const Model::JointIndex RF_id = model.getJointId(RF);
1519  const std::string LF = "lleg6_joint";
1520  // const Model::JointIndex LF_id = model.getJointId(LF);
1521 
1522  // Contact models and data
1523  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
1524  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
1525 
1526  // Add Loop Closure Constraint
1527 
1528  const std::string RA = "rarm5_joint";
1529  const Model::JointIndex RA_id = model.getJointId(RA);
1530  const std::string LA = "larm5_joint";
1531  const Model::JointIndex LA_id = model.getJointId(LA);
1532 
1533  RigidConstraintModel ci_closure(
1534  CONTACT_6D, model, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL_WORLD_ALIGNED);
1535  ci_closure.corrector.Kp.array() = 0.;
1536  ci_closure.corrector.Kd.array() = 0;
1537 
1538  constraint_models.push_back(ci_closure);
1539  constraint_data.push_back(RigidConstraintData(ci_closure));
1540  // End of Loopo Closure Constraint
1541 
1542  Eigen::DenseIndex constraint_dim = 0;
1543  for (size_t k = 0; k < constraint_models.size(); ++k)
1544  constraint_dim += constraint_models[k].size();
1545 
1546  const double mu0 = 0.;
1547  ProximalSettings prox_settings(1e-12, mu0, 1);
1548 
1549  initConstraintDynamics(model, data, constraint_models);
1550  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
1551  const Data::TangentVectorType a = data.ddq;
1552  data.M.triangularView<Eigen::StrictlyLower>() =
1553  data.M.transpose().triangularView<Eigen::StrictlyLower>();
1554  computeConstraintDynamicsDerivatives(
1555  model, data, constraint_models, constraint_data, prox_settings);
1556 
1557  // Data_fd
1558  initConstraintDynamics(model, data_fd, constraint_models);
1559 
1560  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
1561  ddq_partial_dq_fd.setZero();
1562  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
1563  ddq_partial_dv_fd.setZero();
1564  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
1565  ddq_partial_dtau_fd.setZero();
1566 
1567  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
1568  lambda_partial_dtau_fd.setZero();
1569  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
1570  lambda_partial_dq_fd.setZero();
1571  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
1572  lambda_partial_dv_fd.setZero();
1573 
1574  const VectorXd ddq0 = constraintDynamics(
1575  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
1576  const VectorXd lambda0 = data_fd.lambda_c;
1577  VectorXd v_eps(VectorXd::Zero(model.nv));
1578  VectorXd q_plus(model.nq);
1579  VectorXd ddq_plus(model.nv);
1580 
1581  VectorXd lambda_plus(constraint_dim);
1582 
1583  const double alpha = 1e-8;
1584  forwardKinematics(model, data, q, v, a);
1585 
1586  for (int k = 0; k < model.nv; ++k)
1587  {
1588  v_eps[k] += alpha;
1589  q_plus = integrate(model, q, v_eps);
1590  ddq_plus = constraintDynamics(
1591  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
1592  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
1593  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1594  v_eps[k] = 0.;
1595  }
1596 
1597  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
1598  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
1599 
1600  VectorXd v_plus(v);
1601  for (int k = 0; k < model.nv; ++k)
1602  {
1603  v_plus[k] += alpha;
1604  ddq_plus = constraintDynamics(
1605  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
1606  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
1607  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1608  v_plus[k] -= alpha;
1609  }
1610 
1611  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
1612  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
1613 
1614  VectorXd tau_plus(tau);
1615  for (int k = 0; k < model.nv; ++k)
1616  {
1617  tau_plus[k] += alpha;
1618  ddq_plus = constraintDynamics(
1619  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
1620  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
1621  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1622  tau_plus[k] -= alpha;
1623  }
1624 
1625  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
1626  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
1627 }
1628 
1629 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_loop_closure_j1j2_fd)
1630 {
1631  using namespace Eigen;
1632  using namespace pinocchio;
1633 
1634  Model model;
1635  buildModels::humanoidRandom(model, true);
1636  Data data(model), data_fd(model);
1637 
1638  model.lowerPositionLimit.head<3>().fill(-1.);
1639  model.upperPositionLimit.head<3>().fill(1.);
1640  VectorXd q = randomConfiguration(model);
1641 
1642  VectorXd v = VectorXd::Random(model.nv);
1643  VectorXd tau = VectorXd::Random(model.nv);
1644 
1645  const std::string RF = "rleg6_joint";
1646  // const Model::JointIndex RF_id = model.getJointId(RF);
1647  const std::string LF = "lleg6_joint";
1648  // const Model::JointIndex LF_id = model.getJointId(LF);
1649 
1650  // Contact models and data
1651  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
1652  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
1653 
1654  // Add Loop Closure Constraint
1655 
1656  const std::string RA = "rarm5_joint";
1657  const Model::JointIndex RA_id = model.getJointId(RA);
1658  const std::string LA = "larm5_joint";
1659  const Model::JointIndex LA_id = model.getJointId(LA);
1660 
1661  RigidConstraintModel ci_closure(
1662  CONTACT_3D, model, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL);
1663  ci_closure.corrector.Kp.array() = KP;
1664  ci_closure.corrector.Kd.array() = KD;
1665  constraint_models.push_back(ci_closure);
1666  constraint_data.push_back(RigidConstraintData(ci_closure));
1667  // End of Loopo Closure Constraint
1668 
1669  Eigen::DenseIndex constraint_dim = 0;
1670  for (size_t k = 0; k < constraint_models.size(); ++k)
1671  constraint_dim += constraint_models[k].size();
1672 
1673  const double mu0 = 0.;
1674  ProximalSettings prox_settings(1e-12, mu0, 1);
1675 
1676  initConstraintDynamics(model, data, constraint_models);
1677  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
1678  const Data::TangentVectorType a = data.ddq;
1679  data.M.triangularView<Eigen::StrictlyLower>() =
1680  data.M.transpose().triangularView<Eigen::StrictlyLower>();
1681  computeConstraintDynamicsDerivatives(
1682  model, data, constraint_models, constraint_data, prox_settings);
1683 
1684  // Data_fd
1685  initConstraintDynamics(model, data_fd, constraint_models);
1686 
1687  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
1688  ddq_partial_dq_fd.setZero();
1689  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
1690  ddq_partial_dv_fd.setZero();
1691  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
1692  ddq_partial_dtau_fd.setZero();
1693 
1694  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
1695  lambda_partial_dtau_fd.setZero();
1696  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
1697  lambda_partial_dq_fd.setZero();
1698  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
1699  lambda_partial_dv_fd.setZero();
1700 
1701  const VectorXd ddq0 = constraintDynamics(
1702  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
1703  const VectorXd lambda0 = data_fd.lambda_c;
1704  VectorXd v_eps(VectorXd::Zero(model.nv));
1705  VectorXd q_plus(model.nq);
1706  VectorXd ddq_plus(model.nv);
1707 
1708  VectorXd lambda_plus(constraint_dim);
1709 
1710  const double alpha = 1e-8;
1711  forwardKinematics(model, data, q, v, a);
1712 
1713  for (int k = 0; k < model.nv; ++k)
1714  {
1715  v_eps[k] += alpha;
1716  q_plus = integrate(model, q, v_eps);
1717  ddq_plus = constraintDynamics(
1718  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
1719  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
1720  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1721  v_eps[k] = 0.;
1722  }
1723 
1724  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
1725  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
1726  VectorXd v_plus(v);
1727  for (int k = 0; k < model.nv; ++k)
1728  {
1729  v_plus[k] += alpha;
1730  ddq_plus = constraintDynamics(
1731  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
1732  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
1733  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1734  v_plus[k] -= alpha;
1735  }
1736 
1737  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
1738  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
1739 
1740  VectorXd tau_plus(tau);
1741  for (int k = 0; k < model.nv; ++k)
1742  {
1743  tau_plus[k] += alpha;
1744  ddq_plus = constraintDynamics(
1745  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
1746  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
1747  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1748  tau_plus[k] -= alpha;
1749  }
1750 
1751  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
1752  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
1753 }
1754 
1755 BOOST_AUTO_TEST_CASE(
1756  test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_3D_loop_closure_j1j2_fd)
1757 {
1758  using namespace Eigen;
1759  using namespace pinocchio;
1760 
1761  Model model;
1762  buildModels::humanoidRandom(model, true);
1763  Data data(model), data_fd(model);
1764 
1765  model.lowerPositionLimit.head<3>().fill(-1.);
1766  model.upperPositionLimit.head<3>().fill(1.);
1767  VectorXd q = randomConfiguration(model);
1768 
1769  VectorXd v = VectorXd::Random(model.nv);
1770  VectorXd tau = VectorXd::Random(model.nv);
1771 
1772  const std::string RF = "rleg6_joint";
1773  // const Model::JointIndex RF_id = model.getJointId(RF);
1774  const std::string LF = "lleg6_joint";
1775  // const Model::JointIndex LF_id = model.getJointId(LF);
1776 
1777  // Contact models and data
1778  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
1779  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
1780 
1781  // Add Loop Closure Constraint
1782 
1783  const std::string RA = "rarm5_joint";
1784  const Model::JointIndex RA_id = model.getJointId(RA);
1785  const std::string LA = "larm5_joint";
1786  const Model::JointIndex LA_id = model.getJointId(LA);
1787 
1788  RigidConstraintModel ci_closure(
1789  CONTACT_3D, model, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL_WORLD_ALIGNED);
1790 
1791  ci_closure.corrector.Kp.array() = KP;
1792  ci_closure.corrector.Kd.array() = KD;
1793 
1794  constraint_models.push_back(ci_closure);
1795  constraint_data.push_back(RigidConstraintData(ci_closure));
1796  // End of Loopo Closure Constraint
1797 
1798  Eigen::DenseIndex constraint_dim = 0;
1799  for (size_t k = 0; k < constraint_models.size(); ++k)
1800  constraint_dim += constraint_models[k].size();
1801 
1802  const double mu0 = 0.;
1803  ProximalSettings prox_settings(1e-12, mu0, 1);
1804 
1805  initConstraintDynamics(model, data, constraint_models);
1806  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
1807  const Data::TangentVectorType a = data.ddq;
1808  data.M.triangularView<Eigen::StrictlyLower>() =
1809  data.M.transpose().triangularView<Eigen::StrictlyLower>();
1810  computeConstraintDynamicsDerivatives(
1811  model, data, constraint_models, constraint_data, prox_settings);
1812 
1813  // Data_fd
1814  initConstraintDynamics(model, data_fd, constraint_models);
1815 
1816  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
1817  ddq_partial_dq_fd.setZero();
1818  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
1819  ddq_partial_dv_fd.setZero();
1820  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
1821  ddq_partial_dtau_fd.setZero();
1822 
1823  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
1824  lambda_partial_dtau_fd.setZero();
1825  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
1826  lambda_partial_dq_fd.setZero();
1827  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
1828  lambda_partial_dv_fd.setZero();
1829 
1830  const VectorXd ddq0 = constraintDynamics(
1831  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
1832  const VectorXd lambda0 = data_fd.lambda_c;
1833  VectorXd v_eps(VectorXd::Zero(model.nv));
1834  VectorXd q_plus(model.nq);
1835  VectorXd ddq_plus(model.nv);
1836 
1837  VectorXd lambda_plus(constraint_dim);
1838 
1839  const double alpha = 1e-8;
1840  forwardKinematics(model, data, q, v, a);
1841 
1842  for (int k = 0; k < model.nv; ++k)
1843  {
1844  v_eps[k] += alpha;
1845  q_plus = integrate(model, q, v_eps);
1846  ddq_plus = constraintDynamics(
1847  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
1848  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
1849  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1850  v_eps[k] = 0.;
1851  }
1852 
1853  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
1854  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
1855 
1856  VectorXd v_plus(v);
1857  for (int k = 0; k < model.nv; ++k)
1858  {
1859  v_plus[k] += alpha;
1860  ddq_plus = constraintDynamics(
1861  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
1862  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
1863  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1864  v_plus[k] -= alpha;
1865  }
1866 
1867  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
1868  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
1869 
1870  VectorXd tau_plus(tau);
1871  for (int k = 0; k < model.nv; ++k)
1872  {
1873  tau_plus[k] += alpha;
1874  ddq_plus = constraintDynamics(
1875  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
1876  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
1877  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1878  tau_plus[k] -= alpha;
1879  }
1880 
1881  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
1882  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
1883 }
1884 
1885 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_6D_fd)
1886 {
1887  using namespace Eigen;
1888  using namespace pinocchio;
1889 
1890  Model model;
1891  buildModels::humanoidRandom(model, true);
1892  Data data(model), data_fd(model);
1893 
1894  model.lowerPositionLimit.head<3>().fill(-1.);
1895  model.upperPositionLimit.head<3>().fill(1.);
1896  VectorXd q = randomConfiguration(model);
1897 
1898  VectorXd v = VectorXd::Random(model.nv);
1899  VectorXd tau = VectorXd::Random(model.nv);
1900 
1901  const std::string RF = "rleg6_joint";
1902  // const Model::JointIndex RF_id = model.getJointId(RF);
1903  const std::string LF = "lleg6_joint";
1904  const Model::JointIndex LF_id = model.getJointId(LF);
1905 
1906  // Contact models and data
1907  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
1908  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
1909 
1910  RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL_WORLD_ALIGNED);
1911  ci_LF.corrector.Kp.array() = 0; // TODO: Add support for KP >0
1912  ci_LF.corrector.Kd.array() = KD;
1913 
1914  ci_LF.joint1_placement.setRandom();
1915  constraint_models.push_back(ci_LF);
1916  constraint_data.push_back(RigidConstraintData(ci_LF));
1917 
1918  Eigen::DenseIndex constraint_dim = 0;
1919  for (size_t k = 0; k < constraint_models.size(); ++k)
1920  constraint_dim += constraint_models[k].size();
1921 
1922  const double mu0 = 0.;
1923  ProximalSettings prox_settings(1e-12, mu0, 1);
1924 
1925  initConstraintDynamics(model, data, constraint_models);
1926  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
1927 
1928  data.M.triangularView<Eigen::StrictlyLower>() =
1929  data.M.transpose().triangularView<Eigen::StrictlyLower>();
1930  computeConstraintDynamicsDerivatives(
1931  model, data, constraint_models, constraint_data, prox_settings);
1932 
1933  // Data_fd
1934  initConstraintDynamics(model, data_fd, constraint_models);
1935 
1936  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
1937  ddq_partial_dq_fd.setZero();
1938  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
1939  ddq_partial_dv_fd.setZero();
1940  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
1941  ddq_partial_dtau_fd.setZero();
1942 
1943  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
1944  lambda_partial_dtau_fd.setZero();
1945  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
1946  lambda_partial_dq_fd.setZero();
1947  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
1948  lambda_partial_dv_fd.setZero();
1949 
1950  const VectorXd ddq0 = constraintDynamics(
1951  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
1952  const VectorXd lambda0 = data_fd.lambda_c;
1953  VectorXd v_eps(VectorXd::Zero(model.nv));
1954  VectorXd q_plus(model.nq);
1955  VectorXd ddq_plus(model.nv);
1956 
1957  VectorXd lambda_plus(constraint_dim);
1958 
1959  const double alpha = 1e-8;
1960  for (int k = 0; k < model.nv; ++k)
1961  {
1962  v_eps[k] += alpha;
1963  q_plus = integrate(model, q, v_eps);
1964  ddq_plus = constraintDynamics(
1965  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
1966  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
1967  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1968  v_eps[k] = 0.;
1969  }
1970 
1971  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
1972  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
1973 
1974  VectorXd v_plus(v);
1975  for (int k = 0; k < model.nv; ++k)
1976  {
1977  v_plus[k] += alpha;
1978  ddq_plus = constraintDynamics(
1979  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
1980  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
1981  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1982  v_plus[k] -= alpha;
1983  }
1984 
1985  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
1986  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
1987 
1988  VectorXd tau_plus(tau);
1989  for (int k = 0; k < model.nv; ++k)
1990  {
1991  tau_plus[k] += alpha;
1992  ddq_plus = constraintDynamics(
1993  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
1994  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
1995  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
1996  tau_plus[k] -= alpha;
1997  }
1998 
1999  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
2000  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
2001 }
2002 
2003 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_3D_fd)
2004 {
2005  using namespace Eigen;
2006  using namespace pinocchio;
2007 
2008  Model model;
2009  buildModels::humanoidRandom(model, true);
2010  Data data(model), data_fd(model);
2011 
2012  model.lowerPositionLimit.head<3>().fill(-1.);
2013  model.upperPositionLimit.head<3>().fill(1.);
2014  VectorXd q = randomConfiguration(model);
2015 
2016  VectorXd v = VectorXd::Random(model.nv);
2017  VectorXd tau = VectorXd::Random(model.nv);
2018 
2019  const std::string RF = "rleg6_joint";
2020  const Model::JointIndex RF_id = model.getJointId(RF);
2021  const std::string LF = "lleg6_joint";
2022  // const Model::JointIndex LF_id = model.getJointId(LF);
2023 
2024  // Contact models and data
2025  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
2026  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
2027 
2028  RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL_WORLD_ALIGNED);
2029  ci_RF.corrector.Kp.array() = KP;
2030  ci_RF.corrector.Kd.array() = KD;
2031  ci_RF.joint1_placement.setRandom();
2032  constraint_models.push_back(ci_RF);
2033  constraint_data.push_back(RigidConstraintData(ci_RF));
2034 
2035  Eigen::DenseIndex constraint_dim = 0;
2036  for (size_t k = 0; k < constraint_models.size(); ++k)
2037  constraint_dim += constraint_models[k].size();
2038 
2039  const double mu0 = 0.;
2040  ProximalSettings prox_settings(1e-12, mu0, 1);
2041 
2042  initConstraintDynamics(model, data, constraint_models);
2043  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
2044 
2045  data.M.triangularView<Eigen::StrictlyLower>() =
2046  data.M.transpose().triangularView<Eigen::StrictlyLower>();
2047  computeConstraintDynamicsDerivatives(
2048  model, data, constraint_models, constraint_data, prox_settings);
2049 
2050  // Data_fd
2051  initConstraintDynamics(model, data_fd, constraint_models);
2052 
2053  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
2054  ddq_partial_dq_fd.setZero();
2055  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
2056  ddq_partial_dv_fd.setZero();
2057  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
2058  ddq_partial_dtau_fd.setZero();
2059 
2060  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
2061  lambda_partial_dtau_fd.setZero();
2062  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
2063  lambda_partial_dq_fd.setZero();
2064  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
2065  lambda_partial_dv_fd.setZero();
2066 
2067  const VectorXd ddq0 = constraintDynamics(
2068  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
2069  const VectorXd lambda0 = data_fd.lambda_c;
2070  VectorXd v_eps(VectorXd::Zero(model.nv));
2071  VectorXd q_plus(model.nq);
2072  VectorXd ddq_plus(model.nv);
2073 
2074  VectorXd lambda_plus(constraint_dim);
2075 
2076  const double alpha = 1e-8;
2077  for (int k = 0; k < model.nv; ++k)
2078  {
2079  v_eps[k] += alpha;
2080  q_plus = integrate(model, q, v_eps);
2081  ddq_plus = constraintDynamics(
2082  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
2083  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
2084  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2085  v_eps[k] = 0.;
2086  }
2087 
2088  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
2089  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
2090 
2091  VectorXd v_plus(v);
2092  for (int k = 0; k < model.nv; ++k)
2093  {
2094  v_plus[k] += alpha;
2095  ddq_plus = constraintDynamics(
2096  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
2097  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
2098  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2099  v_plus[k] -= alpha;
2100  }
2101 
2102  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
2103  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
2104 
2105  VectorXd tau_plus(tau);
2106  for (int k = 0; k < model.nv; ++k)
2107  {
2108  tau_plus[k] += alpha;
2109  ddq_plus = constraintDynamics(
2110  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
2111  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
2112  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2113  tau_plus[k] -= alpha;
2114  }
2115 
2116  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
2117  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
2118 }
2119 
2120 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_mix_fd)
2121 {
2122  using namespace Eigen;
2123  using namespace pinocchio;
2124 
2125  Model model;
2126  buildModels::humanoidRandom(model, true);
2127  Data data(model), data_fd(model);
2128 
2129  model.lowerPositionLimit.head<3>().fill(-1.);
2130  model.upperPositionLimit.head<3>().fill(1.);
2131  VectorXd q = randomConfiguration(model);
2132 
2133  VectorXd v = VectorXd::Random(model.nv);
2134  VectorXd tau = VectorXd::Random(model.nv);
2135 
2136  const std::string RF = "rleg6_joint";
2137  const Model::JointIndex RF_id = model.getJointId(RF);
2138  const std::string LF = "lleg6_joint";
2139  const Model::JointIndex LF_id = model.getJointId(LF);
2140  const std::string RH = "rarm6_joint";
2141  const Model::JointIndex RH_id = model.getJointId(RH);
2142  const std::string LH = "larm6_joint";
2143  const Model::JointIndex LH_id = model.getJointId(LH);
2144 
2145  // Contact models and data
2146  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
2147  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
2148 
2149  RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL_WORLD_ALIGNED);
2150  ci_LF.corrector.Kp.array() = 0; // TODO: fix local_world_aligned for 6d with kp non-zero
2151  ci_LF.corrector.Kd.array() = KD;
2152  ci_LF.joint1_placement.setRandom();
2153  constraint_models.push_back(ci_LF);
2154  constraint_data.push_back(RigidConstraintData(ci_LF));
2155  RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL);
2156  ci_RF.corrector.Kp.array() = KP;
2157  ci_RF.corrector.Kd.array() = KD;
2158  ci_RF.joint1_placement.setRandom();
2159  constraint_models.push_back(ci_RF);
2160  constraint_data.push_back(RigidConstraintData(ci_RF));
2161 
2162  RigidConstraintModel ci_LH(CONTACT_3D, model, LH_id, LOCAL_WORLD_ALIGNED);
2163  ci_LH.corrector.Kp.array() = KP;
2164  ci_LH.corrector.Kd.array() = KD;
2165  ci_LH.joint1_placement.setRandom();
2166  constraint_models.push_back(ci_LH);
2167  constraint_data.push_back(RigidConstraintData(ci_LH));
2168  RigidConstraintModel ci_RH(CONTACT_3D, model, RH_id, LOCAL);
2169  ci_RH.corrector.Kp.array() = KP;
2170  ci_RH.corrector.Kd.array() = KD;
2171  ci_RH.joint1_placement.setRandom();
2172  constraint_models.push_back(ci_RH);
2173  constraint_data.push_back(RigidConstraintData(ci_RH));
2174 
2175  Eigen::DenseIndex constraint_dim = 0;
2176  for (size_t k = 0; k < constraint_models.size(); ++k)
2177  constraint_dim += constraint_models[k].size();
2178 
2179  const double mu0 = 0.;
2180  ProximalSettings prox_settings(1e-12, mu0, 1);
2181 
2182  initConstraintDynamics(model, data, constraint_models);
2183  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
2184  const Data::TangentVectorType a = data.ddq;
2185  data.M.triangularView<Eigen::StrictlyLower>() =
2186  data.M.transpose().triangularView<Eigen::StrictlyLower>();
2187  computeConstraintDynamicsDerivatives(
2188  model, data, constraint_models, constraint_data, prox_settings);
2189 
2190  // Data_fd
2191  initConstraintDynamics(model, data_fd, constraint_models);
2192 
2193  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
2194  ddq_partial_dq_fd.setZero();
2195  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
2196  ddq_partial_dv_fd.setZero();
2197  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
2198  ddq_partial_dtau_fd.setZero();
2199 
2200  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
2201  lambda_partial_dtau_fd.setZero();
2202  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
2203  lambda_partial_dq_fd.setZero();
2204  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
2205  lambda_partial_dv_fd.setZero();
2206 
2207  const VectorXd ddq0 = constraintDynamics(
2208  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
2209  const VectorXd lambda0 = data_fd.lambda_c;
2210  VectorXd v_eps(VectorXd::Zero(model.nv));
2211  VectorXd q_plus(model.nq);
2212  VectorXd ddq_plus(model.nv);
2213 
2214  VectorXd lambda_plus(constraint_dim);
2215 
2216  const double alpha = 1e-8;
2217  forwardKinematics(model, data, q, v, a);
2218 
2219  const Eigen::MatrixXd Jc = data.dac_da;
2220  const Eigen::MatrixXd Jc_ref =
2221  data.contact_chol.matrix().topRightCorner(constraint_dim, model.nv);
2222 
2223  BOOST_CHECK(Jc.isApprox(Jc_ref));
2224 
2225  const Eigen::MatrixXd JMinv = Jc * data.Minv;
2226  const Eigen::MatrixXd dac_dq = data.dac_dq;
2227 
2228  Eigen::MatrixXd dac_dq_fd(constraint_dim, model.nv);
2229 
2230  Eigen::VectorXd contact_acc0(constraint_dim);
2231  Eigen::DenseIndex row_id = 0;
2232 
2233  forwardKinematics(model, data, q, v, data.ddq);
2234  for (size_t k = 0; k < constraint_models.size(); ++k)
2235  {
2236  const RigidConstraintModel & cmodel = constraint_models[k];
2237  const RigidConstraintData & cdata = constraint_data[k];
2238  const Eigen::DenseIndex size = cmodel.size();
2239 
2240  const Motion contact_acc = getContactAcceleration(model, data, cmodel);
2241 
2242  if (cmodel.type == CONTACT_3D)
2243  contact_acc0.segment<3>(row_id) =
2244  contact_acc.linear() - cdata.contact_acceleration_error.linear();
2245  else
2246  contact_acc0.segment<6>(row_id) =
2247  contact_acc.toVector() - cdata.contact_acceleration_error.toVector();
2248 
2249  row_id += size;
2250  }
2251 
2252  for (int k = 0; k < model.nv; ++k)
2253  {
2254  v_eps[k] += alpha;
2255  q_plus = integrate(model, q, v_eps);
2256  ddq_plus = constraintDynamics(
2257  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
2258 
2259  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
2260  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2261 
2262  Eigen::VectorXd contact_acc_plus(constraint_dim);
2263  Eigen::DenseIndex row_id = 0;
2264  forwardKinematics(model, data_fd, q_plus, v, data.ddq);
2265  for (size_t k = 0; k < constraint_models.size(); ++k)
2266  {
2267  const RigidConstraintModel & cmodel = constraint_models[k];
2268  const RigidConstraintData & cdata = constraint_data[k];
2269  const Eigen::DenseIndex size = cmodel.size();
2270 
2271  const Motion contact_acc = getContactAcceleration(model, data_fd, cmodel);
2272 
2273  if (cmodel.type == CONTACT_3D)
2274  contact_acc_plus.segment<3>(row_id) =
2275  contact_acc.linear() - cdata.contact_acceleration_error.linear();
2276  else
2277  contact_acc_plus.segment<6>(row_id) =
2278  contact_acc.toVector() - cdata.contact_acceleration_error.toVector();
2279 
2280  row_id += size;
2281  }
2282 
2283  dac_dq_fd.col(k) = (contact_acc_plus - contact_acc0) / alpha;
2284 
2285  v_eps[k] = 0.;
2286  }
2287 
2288  BOOST_CHECK(dac_dq_fd.isApprox(dac_dq, 1e-6));
2289 
2290  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
2291  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
2292 
2293  VectorXd v_plus(v);
2294  for (int k = 0; k < model.nv; ++k)
2295  {
2296  v_plus[k] += alpha;
2297  ddq_plus = constraintDynamics(
2298  model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings);
2299  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
2300  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2301  v_plus[k] -= alpha;
2302  }
2303 
2304  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
2305  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
2306 
2307  VectorXd tau_plus(tau);
2308  for (int k = 0; k < model.nv; ++k)
2309  {
2310  tau_plus[k] += alpha;
2311  ddq_plus = constraintDynamics(
2312  model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings);
2313  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
2314  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2315  tau_plus[k] -= alpha;
2316  }
2317 
2318  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
2319  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
2320 }
2321 
2322 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_loop_closure_kinematics_fd)
2323 {
2324  using namespace Eigen;
2325  using namespace pinocchio;
2326 
2327  Model model;
2328  buildModels::humanoidRandom(model, true);
2329  Data data(model), data_fd(model);
2330 
2331  model.lowerPositionLimit.head<3>().fill(-1.);
2332  model.upperPositionLimit.head<3>().fill(1.);
2333  VectorXd q = randomConfiguration(model);
2334 
2335  VectorXd v = VectorXd::Random(model.nv);
2336  VectorXd tau = VectorXd::Random(model.nv);
2337 
2338  const std::string RH = "rarm6_joint";
2339  const Model::JointIndex RH_id = model.getJointId(RH);
2340  const std::string LH = "larm6_joint";
2341  const Model::JointIndex LH_id = model.getJointId(LH);
2342 
2343  // Contact models and data
2344  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
2345  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
2346 
2347  RigidConstraintModel ci_RH(CONTACT_6D, model, RH_id, SE3::Random(), LH_id, SE3::Random(), LOCAL);
2348  ci_RH.corrector.Kp.array() = 0;
2349  ci_RH.corrector.Kd.array() = 0;
2350 
2351  constraint_models.push_back(ci_RH);
2352  constraint_data.push_back(RigidConstraintData(ci_RH));
2353 
2354  Eigen::DenseIndex constraint_dim = 0;
2355  for (size_t k = 0; k < constraint_models.size(); ++k)
2356  constraint_dim += constraint_models[k].size();
2357 
2358  const double mu0 = 0.;
2359  ProximalSettings prox_settings(1e-12, mu0, 1);
2360 
2361  initConstraintDynamics(model, data, constraint_models);
2362  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings);
2363  const Data::TangentVectorType a = data.ddq;
2364  data.M.triangularView<Eigen::StrictlyLower>() =
2365  data.M.transpose().triangularView<Eigen::StrictlyLower>();
2366  computeConstraintDynamicsDerivatives(
2367  model, data, constraint_models, constraint_data, prox_settings);
2368 
2369  // Data_fd
2370  initConstraintDynamics(model, data_fd, constraint_models);
2371  const VectorXd ddq0 = constraintDynamics(
2372  model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings);
2373  const VectorXd lambda0 = data_fd.lambda_c;
2374  VectorXd v_eps(VectorXd::Zero(model.nv));
2375  VectorXd q_plus(model.nq);
2376  VectorXd ddq_plus(model.nv);
2377 
2378  VectorXd lambda_plus(constraint_dim);
2379 
2380  const double alpha = 1e-8;
2381  forwardKinematics(model, data, q, v, a);
2382 
2383  const Eigen::MatrixXd Jc = data.dac_da;
2384  const Eigen::MatrixXd Jc_ref =
2385  data.contact_chol.matrix().topRightCorner(constraint_dim, model.nv);
2386 
2387  BOOST_CHECK(Jc.isApprox(Jc_ref));
2388 
2389  const Eigen::MatrixXd JMinv = Jc * data.Minv;
2390  const Eigen::MatrixXd dac_dq = data.dac_dq;
2391  Eigen::MatrixXd dac_dq_fd(constraint_dim, model.nv);
2392 
2393  Eigen::VectorXd contact_acc0(constraint_dim);
2394  Eigen::DenseIndex row_id = 0;
2395 
2396  forwardKinematics(model, data, q, v, data.ddq);
2397  for (size_t k = 0; k < constraint_models.size(); ++k)
2398  {
2399  const RigidConstraintModel & cmodel = constraint_models[k];
2400  const RigidConstraintData & cdata = constraint_data[k];
2401  const Eigen::DenseIndex size = cmodel.size();
2402 
2403  const Motion contact_acc = getContactAcceleration(model, data, cmodel, cdata.c1Mc2);
2404 
2405  if (cmodel.type == CONTACT_3D)
2406  contact_acc0.segment<3>(row_id) = contact_acc.linear();
2407  else
2408  contact_acc0.segment<6>(row_id) = contact_acc.toVector();
2409 
2410  row_id += size;
2411  }
2412 
2413  for (int k = 0; k < model.nv; ++k)
2414  {
2415  v_eps[k] += alpha;
2416  q_plus = integrate(model, q, v_eps);
2417  ddq_plus = constraintDynamics(
2418  model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings);
2419 
2420  Eigen::VectorXd contact_acc_plus(constraint_dim);
2421  Eigen::DenseIndex row_id = 0;
2422  forwardKinematics(model, data_fd, q_plus, v, data.ddq);
2423  for (size_t k = 0; k < constraint_models.size(); ++k)
2424  {
2425  const RigidConstraintModel & cmodel = constraint_models[k];
2426  const RigidConstraintData & cdata = constraint_data[k];
2427  const Eigen::DenseIndex size = cmodel.size();
2428 
2429  const Motion contact_acc = getContactAcceleration(model, data_fd, cmodel, cdata.c1Mc2);
2430 
2431  if (cmodel.type == CONTACT_3D)
2432  contact_acc_plus.segment<3>(row_id) = contact_acc.linear();
2433  else
2434  contact_acc_plus.segment<6>(row_id) = contact_acc.toVector();
2435 
2436  row_id += size;
2437  }
2438 
2439  dac_dq_fd.col(k) = (contact_acc_plus - contact_acc0) / alpha;
2440 
2441  v_eps[k] = 0.;
2442  }
2443 
2444  BOOST_CHECK(dac_dq_fd.isApprox(dac_dq, 1e-6));
2445 }
2446 
2447 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_dirty_data)
2448 {
2449  // Verify that a dirty data doesn't affect the results of the contact dynamics derivs
2450  using namespace Eigen;
2451  using namespace pinocchio;
2452 
2453  Model model;
2454  buildModels::humanoidRandom(model, true);
2455  Data data_dirty(model);
2456 
2457  model.lowerPositionLimit.head<3>().fill(-1.);
2458  model.upperPositionLimit.head<3>().fill(1.);
2459  VectorXd q = randomConfiguration(model);
2460 
2461  VectorXd v = VectorXd::Random(model.nv);
2462  VectorXd tau = VectorXd::Random(model.nv);
2463 
2464  const std::string RF = "rleg6_joint";
2465  const Model::JointIndex RF_id = model.getJointId(RF);
2466  const std::string LF = "lleg6_joint";
2467  const Model::JointIndex LF_id = model.getJointId(LF);
2468 
2469  // Contact models and data
2470  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models;
2471  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data;
2472 
2473  RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL);
2474  RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL);
2475 
2476  ci_LF.corrector.Kp.array() = KP;
2477  ci_LF.corrector.Kd.array() = KD;
2478  ci_RF.corrector.Kp.array() = KP;
2479  ci_RF.corrector.Kd.array() = KD;
2480  constraint_models.push_back(ci_LF);
2481  constraint_data.push_back(RigidConstraintData(ci_LF));
2482  constraint_models.push_back(ci_RF);
2483  constraint_data.push_back(RigidConstraintData(ci_RF));
2484 
2485  Eigen::DenseIndex constraint_dim = 0;
2486  for (size_t k = 0; k < constraint_models.size(); ++k)
2487  constraint_dim += constraint_models[k].size();
2488 
2489  const double mu0 = 0.;
2490  ProximalSettings prox_settings(1e-12, mu0, 1);
2491 
2492  initConstraintDynamics(model, data_dirty, constraint_models);
2493  constraintDynamics(
2494  model, data_dirty, q, v, tau, constraint_models, constraint_data, prox_settings);
2495  computeConstraintDynamicsDerivatives(
2496  model, data_dirty, constraint_models, constraint_data, prox_settings);
2497 
2498  // Reuse the same data with new configurations
2499  q = randomConfiguration(model);
2500  v = VectorXd::Random(model.nv);
2501  tau = VectorXd::Random(model.nv);
2502  constraintDynamics(
2503  model, data_dirty, q, v, tau, constraint_models, constraint_data, prox_settings);
2504  computeConstraintDynamicsDerivatives(
2505  model, data_dirty, constraint_models, constraint_data, prox_settings);
2506 
2507  // Test with fresh data
2508  Data data_fresh(model);
2509  initConstraintDynamics(model, data_fresh, constraint_models);
2510  constraintDynamics(
2511  model, data_fresh, q, v, tau, constraint_models, constraint_data, prox_settings);
2512  computeConstraintDynamicsDerivatives(
2513  model, data_fresh, constraint_models, constraint_data, prox_settings);
2514  const double alpha = 1e-12;
2515 
2516  BOOST_CHECK(data_dirty.ddq_dq.isApprox(data_fresh.ddq_dq, sqrt(alpha)));
2517  BOOST_CHECK(data_dirty.ddq_dv.isApprox(data_fresh.ddq_dv, sqrt(alpha)));
2518  BOOST_CHECK(data_dirty.ddq_dtau.isApprox(data_fresh.ddq_dtau, sqrt(alpha)));
2519  BOOST_CHECK(data_dirty.dlambda_dq.isApprox(data_fresh.dlambda_dq, sqrt(alpha)));
2520  BOOST_CHECK(data_dirty.dlambda_dv.isApprox(data_fresh.dlambda_dv, sqrt(alpha)));
2521  BOOST_CHECK(data_dirty.dlambda_dtau.isApprox(data_fresh.dlambda_dtau, sqrt(alpha)));
2522 }
2523 
2524 #ifdef PINOCCHIO_WITH_SDFORMAT
2525 
2526 BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES(test_constraint_dynamics_derivatives_cassie_proximal, 5)
2527 BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_cassie_proximal)
2528 {
2529  // TODO: 4 fd tests (ddq/dtau, ddq/dq, ddq/dv, dlambda/dq, dlambda/dv) fail for cassie
2530  const std::string filename =
2531  PINOCCHIO_MODEL_DIR
2532  + std::string("/example-robot-data/robots/cassie_description/robots/cassie.sdf");
2533  const std::string srdf_filename =
2534  PINOCCHIO_MODEL_DIR
2535  + std::string("/example-robot-data/robots/cassie_description/srdf/cassie_v2.srdf");
2536  const std::string dir = PINOCCHIO_MODEL_DIR;
2537 
2538  pinocchio::Model model;
2539  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) constraint_models;
2540  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_datas;
2541 
2542  pinocchio::sdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model, constraint_models);
2543  pinocchio::srdf::loadReferenceConfigurations(model, srdf_filename, false);
2544 
2545  Eigen::VectorXd q = model.referenceConfigurations["standing"];
2546  VectorXd v = VectorXd::Random(model.nv);
2547  VectorXd tau = VectorXd::Random(model.nv);
2548 
2549  const double mu0 = 1e-5;
2550  ProximalSettings prox_settings(1e-12, mu0, 10);
2551 
2552  Data data(model), data_fd(model);
2553 
2554  initConstraintDynamics(model, data, constraint_models);
2555  for (int k = 0; k < (int)constraint_models.size(); ++k)
2556  {
2557  constraint_datas.push_back(RigidConstraintData(constraint_models[(pinocchio::JointIndex)k]));
2558  }
2559 
2560  Eigen::DenseIndex constraint_dim = 0;
2561  for (size_t k = 0; k < constraint_models.size(); ++k)
2562  constraint_dim += constraint_models[k].size();
2563 
2564  initConstraintDynamics(model, data, constraint_models);
2565  constraintDynamics(model, data, q, v, tau, constraint_models, constraint_datas, prox_settings);
2566  data.M.triangularView<Eigen::StrictlyLower>() =
2567  data.M.transpose().triangularView<Eigen::StrictlyLower>();
2568  computeConstraintDynamicsDerivatives(
2569  model, data, constraint_models, constraint_datas, prox_settings);
2570 
2571  initConstraintDynamics(model, data_fd, constraint_models);
2572  MatrixXd ddq_partial_dq_fd(model.nv, model.nv);
2573  ddq_partial_dq_fd.setZero();
2574  MatrixXd ddq_partial_dv_fd(model.nv, model.nv);
2575  ddq_partial_dv_fd.setZero();
2576  MatrixXd ddq_partial_dtau_fd(model.nv, model.nv);
2577  ddq_partial_dtau_fd.setZero();
2578 
2579  MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv);
2580  lambda_partial_dtau_fd.setZero();
2581  MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv);
2582  lambda_partial_dq_fd.setZero();
2583  MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv);
2584  lambda_partial_dv_fd.setZero();
2585 
2586  const VectorXd ddq0 = constraintDynamics(
2587  model, data_fd, q, v, tau, constraint_models, constraint_datas, prox_settings);
2588  const VectorXd lambda0 = data_fd.lambda_c;
2589  VectorXd v_eps(VectorXd::Zero(model.nv));
2590  VectorXd q_plus(model.nq);
2591  VectorXd ddq_plus(model.nv);
2592 
2593  VectorXd lambda_plus(constraint_dim);
2594  const double alpha = 1e-8;
2595  forwardKinematics(model, data, q, v);
2596  for (int k = 0; k < model.nv; ++k)
2597  {
2598  v_eps[k] += alpha;
2599  q_plus = integrate(model, q, v_eps);
2600  ddq_plus = constraintDynamics(
2601  model, data_fd, q_plus, v, tau, constraint_models, constraint_datas, prox_settings);
2602  ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha;
2603  lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2604  v_eps[k] = 0.;
2605  }
2606 
2607  BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha)));
2608 
2609  BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha)));
2610 
2611  VectorXd v_plus(v);
2612  for (int k = 0; k < model.nv; ++k)
2613  {
2614  v_plus[k] += alpha;
2615  ddq_plus = constraintDynamics(
2616  model, data_fd, q, v_plus, tau, constraint_models, constraint_datas, prox_settings);
2617  ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha;
2618  lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2619  v_plus[k] -= alpha;
2620  }
2621 
2622  BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha)));
2623  BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha)));
2624 
2625  VectorXd tau_plus(tau);
2626  for (int k = 0; k < model.nv; ++k)
2627  {
2628  tau_plus[k] += alpha;
2629  ddq_plus = constraintDynamics(
2630  model, data_fd, q, v, tau_plus, constraint_models, constraint_datas, prox_settings);
2631  ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha;
2632  lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha;
2633  tau_plus[k] -= alpha;
2634  }
2635 
2636  BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha)));
2637  BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha)));
2638 }
2639 
2640 #endif // PINOCCHIO_WITH_SDFORMAT
2641 
2642 BOOST_AUTO_TEST_SUITE_END()
pinocchio::RigidConstraintData
RigidConstraintDataTpl< context::Scalar, context::Options > RigidConstraintData
Definition: algorithm/fwd.hpp:27
pinocchio::DataTpl::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: multibody/data.hpp:89
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
pinocchio::DataTpl::ddq_dv
RowMatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition: multibody/data.hpp:398
display-shapes-meshcat.placement
placement
Definition: display-shapes-meshcat.py:23
pinocchio::DataTpl::ddq
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: multibody/data.hpp:256
pinocchio::DataTpl::dFdq
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Definition: multibody/data.hpp:211
frames.hpp
Eigen
computeAcceleration
pinocchio::Motion computeAcceleration(const pinocchio::Model &model, const pinocchio::Data &data, const pinocchio::JointIndex &joint_id, const pinocchio::ReferenceFrame reference_frame, const pinocchio::ContactType contact_type, const pinocchio::SE3 &placement=pinocchio::SE3::Identity())
Definition: contact-dynamics-derivatives.cpp:221
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::DataTpl::dAdv
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Definition: multibody/data.hpp:383
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
pinocchio::getFrameAccelerationDerivatives
void getFrameAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &a_partial_dq, const Eigen::MatrixBase< Matrix6xOut3 > &a_partial_dv, const Eigen::MatrixBase< Matrix6xOut4 > &a_partial_da)
Computes the partial derivatives of the spatial acceleration of a frame given by its relative placeme...
pinocchio::sdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) &contact_models, const std::string &rootLinkName="", const std::vector< std::string > &parentGuidance={}, const bool verbose=false)
Build the model from a SDF file with a particular joint as root of the model tree inside the model gi...
pinocchio::computeConstraintDynamicsDerivatives
void computeConstraintDynamicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const ProximalSettingsTpl< Scalar > &settings, const Eigen::MatrixBase< MatrixType1 > &ddq_partial_dq, const Eigen::MatrixBase< MatrixType2 > &ddq_partial_dv, const Eigen::MatrixBase< MatrixType3 > &ddq_partial_dtau, const Eigen::MatrixBase< MatrixType4 > &lambda_partial_dq, const Eigen::MatrixBase< MatrixType5 > &lambda_partial_dv, const Eigen::MatrixBase< MatrixType6 > &lambda_partial_dtau)
pinocchio::DataTpl::dlambda_dv
MatrixXs dlambda_dv
Definition: multibody/data.hpp:417
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: multibody/fwd.hpp:46
pinocchio::SE3Tpl< context::Scalar, context::Options >
pinocchio::SE3Tpl< context::Scalar, context::Options >::Matrix6
traits< SE3Tpl >::Matrix6 Matrix6
Definition: spatial/se3-tpl.hpp:59
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....
pinocchio::DataTpl::dFdv
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Definition: multibody/data.hpp:214
kinematics.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::classicAcceleration
void classicAcceleration(const MotionDense< Motion1 > &spatial_velocity, const MotionDense< Motion2 > &spatial_acceleration, const Eigen::MatrixBase< Vector3Like > &res)
Computes the classic acceleration from a given spatial velocity and spatial acceleration.
Definition: spatial/classic-acceleration.hpp:29
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::DataTpl::lambda_c
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: multibody/data.hpp:471
contact-cholesky.cmodel
cmodel
Definition: contact-cholesky.py:28
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
simulation-closed-kinematic-chains.constraint_data
constraint_data
Definition: simulation-closed-kinematic-chains.py:106
pinocchio::JointModelFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:110
computeVelocityAndAccelerationErrors
void computeVelocityAndAccelerationErrors(const Model &model, const RigidConstraintModel &cmodel, const VectorXd &q, const VectorXd &v, const VectorXd &a, Motion &v_error, Motion &a_error, const VectorXd &Kp, const VectorXd &Kd)
Definition: contact-dynamics-derivatives.cpp:1117
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_sparse_constraint_dynamics_derivatives_no_contact)
Definition: contact-dynamics-derivatives.cpp:38
pinocchio::Jlog6
void Jlog6(const SE3Tpl< Scalar, Options > &M, const Eigen::MatrixBase< Matrix6Like > &Jlog)
Derivative of log6.
Definition: spatial/explog.hpp:668
constrained-dynamics-derivatives.hpp
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
aba-derivatives.hpp
bindings_com_velocity_derivatives.df_dq
def df_dq(model, func, q, h=1e-9)
Definition: bindings_com_velocity_derivatives.py:7
createData
createData(const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) &constraint_models)
Definition: contact-dynamics-derivatives.cpp:408
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
pinocchio::DataTpl::ddq_dtau
RowMatrixXs ddq_dtau
Partial derivative of the joint acceleration vector with respect to the joint torques.
Definition: multibody/data.hpp:402
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:23
res
res
forward-dynamics-derivatives.ddq_dtau
ddq_dtau
Definition: forward-dynamics-derivatives.py:33
pinocchio::computeForwardKinematicsDerivatives
void computeForwardKinematicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Computes all the terms required to compute the derivatives of the placement, spatial velocity and acc...
joint-configuration.hpp
KD
#define KD
Definition: contact-dynamics-derivatives.cpp:31
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:159
pinocchio::ProximalSettingsTpl
Structure containing all the settings paramters for the proximal algorithms.
Definition: algorithm/fwd.hpp:13
sdf.hpp
anymal-simulation.constraint_datas
list constraint_datas
Definition: anymal-simulation.py:61
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: multibody/data.hpp:94
pinocchio::DataTpl::dtau_dq
RowMatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: multibody/data.hpp:387
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:72
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
autodiff-rnea.v
v
Definition: autodiff-rnea.py:15
filename
filename
pinocchio::getFrameVelocityDerivatives
void getFrameVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivatives of the spatial velocity of a frame given by its relative placement,...
srdf.hpp
pinocchio::DataTpl::Minv
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:202
pinocchio::DataTpl::dlambda_dq
MatrixXs dlambda_dq
Partial derivatives of the constraints forces with respect to the joint configuration,...
Definition: multibody/data.hpp:416
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.
KP
#define KP
Definition: contact-dynamics-derivatives.cpp:30
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
size
FCL_REAL size() const
pinocchio::DataTpl::primal_rhs_contact
VectorXs primal_rhs_contact
Primal RHS in contact dynamic equations.
Definition: multibody/data.hpp:538
collisions.srdf_filename
string srdf_filename
Definition: collisions.py:28
q
q
constraintDynamics
VectorXd constraintDynamics(const Model &model, Data &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Model::JointIndex id)
Definition: unittest/constrained-dynamics-derivatives.cpp:185
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
forward-dynamics-derivatives.ddq_dq
ddq_dq
Definition: forward-dynamics-derivatives.py:31
a
Vec3f a
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:360
mu
double mu
Definition: delassus.cpp:58
pinocchio::container::aligned_vector
std::vector< T, Eigen::aligned_allocator< T > > aligned_vector
Definition: container/aligned-vector.hpp:21
forward-dynamics-derivatives.ddq_dv
ddq_dv
Definition: forward-dynamics-derivatives.py:32
fill
fill
pinocchio::log6
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Definition: spatial/explog.hpp:435
pinocchio::DataTpl::ddq_dq
RowMatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition: multibody/data.hpp:394
pinocchio::ContactType
ContactType
&#160;
Definition: algorithm/contact-info.hpp:19
pinocchio::srdf::loadReferenceConfigurations
void loadReferenceConfigurations(ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const bool verbose=false)
Get the reference configurations of a given model associated to a SRDF file. It throws if the SRDF fi...
pinocchio::SE3Tpl< context::Scalar, context::Options >::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
pinocchio::cross
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition: skew.hpp:228
kinematics-derivatives.hpp
dcrba.eps
int eps
Definition: dcrba.py:439
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
simulation-closed-kinematic-chains.Kd
Kd
Definition: simulation-closed-kinematic-chains.py:161
pinocchio::DataTpl::dlambda_dtau
MatrixXs dlambda_dtau
Definition: multibody/data.hpp:418
anymal-simulation.constraint_models
list constraint_models
Definition: anymal-simulation.py:32
pinocchio::python::computeABADerivatives
bp::tuple computeABADerivatives(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &tau)
Definition: expose-aba-derivatives.cpp:20
pinocchio::DataTpl::dtau_dv
RowMatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity.
Definition: multibody/data.hpp:390
anymal-simulation.constraint_dim
constraint_dim
Definition: anymal-simulation.py:67
sample-models.hpp
pinocchio::MotionTpl
Definition: context/casadi.hpp:27
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
constrained-dynamics.hpp
simulation-closed-kinematic-chains.prox_settings
prox_settings
Definition: simulation-closed-kinematic-chains.py:163
classic-acceleration.hpp
crba.hpp
pinocchio::CONTACT_3D
@ CONTACT_3D
Definition: algorithm/contact-info.hpp:21
pinocchio::DataTpl::dVdq
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: multibody/data.hpp:377
getContactAcceleration
pinocchio::Motion getContactAcceleration(const Model &model, const Data &data, const RigidConstraintModel &cmodel, const pinocchio::SE3 &c1Mc2=SE3::Identity())
Definition: contact-dynamics-derivatives.cpp:273
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
PINOCCHIO_UNUSED_VARIABLE
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
Definition: include/pinocchio/macros.hpp:73
frames-derivatives.hpp
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
pinocchio::DataTpl::dAdq
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Definition: multibody/data.hpp:380
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

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