contact-models.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2021 INRIA
3 //
4 
5 #include "pinocchio/algorithm/aba.hpp"
6 #include "pinocchio/algorithm/rnea.hpp"
7 #include "pinocchio/algorithm/frames.hpp"
8 #include "pinocchio/algorithm/jacobian.hpp"
9 #include "pinocchio/algorithm/centroidal.hpp"
10 #include "pinocchio/algorithm/kinematics.hpp"
11 #include "pinocchio/algorithm/contact-info.hpp"
12 #include "pinocchio/algorithm/contact-jacobian.hpp"
13 #include "pinocchio/algorithm/joint-configuration.hpp"
14 #include "pinocchio/multibody/sample-models.hpp"
15 #include "pinocchio/utils/timer.hpp"
16 #include "pinocchio/spatial/classic-acceleration.hpp"
17 
18 #include <iostream>
19 
20 #include <boost/test/unit_test.hpp>
21 #include <boost/utility/binary.hpp>
22 
23 using namespace pinocchio;
24 using namespace Eigen;
25 
26 template<typename T>
27 bool within(const T & elt, const std::vector<T> & vec)
28 {
29  typename std::vector<T>::const_iterator it;
30 
31  it = std::find(vec.begin(), vec.end(), elt);
32  if (it != vec.end())
33  return true;
34  else
35  return false;
36 }
37 
38 template<typename Matrix>
39 bool within(const typename Matrix::Scalar & elt, const Eigen::MatrixBase<Matrix> & mat)
40 {
41  for (DenseIndex i = 0; i < mat.rows(); ++i)
42  for (DenseIndex j = 0; j < mat.rows(); ++j)
43  {
44  if (elt == mat(i, j))
45  return true;
46  }
47 
48  return false;
49 }
50 
51 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
52 
53 BOOST_AUTO_TEST_CASE(contact_models)
54 {
55  pinocchio::Model model;
56  pinocchio::buildModels::humanoidRandom(model, true);
57 
58  // Check complete constructor
59  const SE3 M(SE3::Random());
60  RigidConstraintModel cmodel2(CONTACT_3D, model, 0, M);
61  BOOST_CHECK(cmodel2.type == CONTACT_3D);
62  BOOST_CHECK(cmodel2.joint1_id == 0);
63  BOOST_CHECK(cmodel2.joint1_placement.isApprox(M));
64  BOOST_CHECK(cmodel2.size() == 3);
65 
66  // Check contructor with two arguments
67  RigidConstraintModel cmodel2prime(CONTACT_3D, model, 0);
68  BOOST_CHECK(cmodel2prime.type == CONTACT_3D);
69  BOOST_CHECK(cmodel2prime.joint1_id == 0);
70  BOOST_CHECK(cmodel2prime.joint1_placement.isIdentity());
71  BOOST_CHECK(cmodel2prime.size() == 3);
72 
73  // Check default copy constructor
74  RigidConstraintModel cmodel3(cmodel2);
75  BOOST_CHECK(cmodel3 == cmodel2);
76 
77  // Check complete constructor 6D
78  RigidConstraintModel cmodel4(CONTACT_6D, model, 0);
79  BOOST_CHECK(cmodel4.type == CONTACT_6D);
80  BOOST_CHECK(cmodel4.joint1_id == 0);
81  BOOST_CHECK(cmodel4.joint1_placement.isIdentity());
82  BOOST_CHECK(cmodel4.size() == 6);
83 }
84 
85 void check_A1_and_A2(
86  const Model & model,
87  const Data & data,
88  const RigidConstraintModel & cmodel,
89  RigidConstraintData & cdata)
90 {
91  const RigidConstraintModel::Matrix36 A1 = cmodel.getA1(cdata);
92  const RigidConstraintModel::Matrix36 A1_ref = cdata.oMc1.toActionMatrixInverse().topRows<3>();
93 
94  BOOST_CHECK(A1.isApprox(A1_ref));
95 
96  const RigidConstraintModel::Matrix36 A2 = cmodel.getA2(cdata);
97  const RigidConstraintModel::Matrix36 A2_ref =
98  -cdata.c1Mc2.rotation() * cdata.oMc2.toActionMatrixInverse().topRows<3>();
99 
100  BOOST_CHECK(A2.isApprox(A2_ref));
101 
102  // Check Jacobian
103  Data::MatrixXs J_ref(3, model.nv);
104  J_ref.setZero();
105  getConstraintJacobian(model, data, cmodel, cdata, J_ref);
106  const Data::Matrix6x J1 = getJointJacobian(model, data, cmodel.joint1_id, WORLD);
107  const Data::Matrix6x J2 = getJointJacobian(model, data, cmodel.joint2_id, WORLD);
108  const Data::Matrix3x J = A1 * J1 + A2 * J2;
109 
110  BOOST_CHECK(J.isApprox(J_ref));
111 
112  // Check Jacobian matrix product
113  const Eigen::DenseIndex m = 40;
114  const Data::MatrixXs mat = Data::MatrixXs::Random(model.nv, m);
115 
116  Data::MatrixXs res(cmodel.size(), m);
117  res.setZero();
118  cmodel.jacobian_matrix_product(model, data, cdata, mat, res);
119 
120  const Data::MatrixXs res_ref = J_ref * mat;
121 
122  BOOST_CHECK(res.isApprox(res_ref));
123 }
124 
125 BOOST_AUTO_TEST_CASE(contact_models_sparsity_and_jacobians)
126 {
127 
128  pinocchio::Model model;
129  pinocchio::buildModels::humanoidRandom(model, true);
130  Data data(model);
131 
132  model.lowerPositionLimit.head<3>().fill(-1.);
133  model.upperPositionLimit.head<3>().fill(1.);
134  VectorXd q = randomConfiguration(model);
135  computeJointJacobians(model, data, q);
136 
137  const std::string RF = "rleg6_joint";
138  const std::string LF = "lleg6_joint";
139 
140  // 6D - LOCAL
141  {
142  RigidConstraintModel cm_RF_LOCAL(CONTACT_6D, model, model.getJointId(RF), SE3::Random(), LOCAL);
143  RigidConstraintData cd_RF_LOCAL(cm_RF_LOCAL);
144  RigidConstraintModel cm_LF_LOCAL(CONTACT_6D, model, model.getJointId(LF), SE3::Random(), LOCAL);
145  RigidConstraintData cd_LF_LOCAL(cm_LF_LOCAL);
146  RigidConstraintModel clm_RF_LF_LOCAL(
147  CONTACT_6D, model, cm_RF_LOCAL.joint1_id, cm_RF_LOCAL.joint1_placement, cm_LF_LOCAL.joint1_id,
148  cm_LF_LOCAL.joint1_placement, LOCAL);
149  RigidConstraintData cld_RF_LF_LOCAL(clm_RF_LF_LOCAL);
150 
151  Data::Matrix6x J_RF_LOCAL(6, model.nv);
152  J_RF_LOCAL.setZero();
153  getFrameJacobian(
154  model, data, cm_RF_LOCAL.joint1_id, cm_RF_LOCAL.joint1_placement, cm_RF_LOCAL.reference_frame,
155  J_RF_LOCAL);
156  Data::Matrix6x J_LF_LOCAL(6, model.nv);
157  J_LF_LOCAL.setZero();
158  getFrameJacobian(
159  model, data, cm_LF_LOCAL.joint1_id, cm_LF_LOCAL.joint1_placement, cm_LF_LOCAL.reference_frame,
160  J_LF_LOCAL);
161 
162  for (DenseIndex k = 0; k < model.nv; ++k)
163  {
164  BOOST_CHECK(J_RF_LOCAL.col(k).isZero() != cm_RF_LOCAL.colwise_joint1_sparsity[k]);
165  BOOST_CHECK(J_LF_LOCAL.col(k).isZero() != cm_LF_LOCAL.colwise_joint1_sparsity[k]);
166  }
167  BOOST_CHECK(cm_RF_LOCAL.colwise_joint2_sparsity.isZero());
168  BOOST_CHECK(cm_LF_LOCAL.colwise_joint2_sparsity.isZero());
169 
170  const SE3 oMc1 = data.oMi[clm_RF_LF_LOCAL.joint1_id] * clm_RF_LF_LOCAL.joint1_placement;
171  const SE3 oMc2 = data.oMi[clm_RF_LF_LOCAL.joint2_id] * clm_RF_LF_LOCAL.joint2_placement;
172  const SE3 c1Mc2 = oMc1.actInv(oMc2);
173  const Data::Matrix6x J_clm_LOCAL = J_RF_LOCAL - c1Mc2.toActionMatrix() * J_LF_LOCAL;
174 
175  for (DenseIndex k = 0; k < model.nv; ++k)
176  {
177  BOOST_CHECK(J_clm_LOCAL.col(k).isZero() != within(k, clm_RF_LF_LOCAL.colwise_span_indexes));
178  }
179 
180  // Check Jacobian
181  Data::MatrixXs J_RF_LOCAL_sparse(6, model.nv);
182  J_RF_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized
183  // with CRTP on contact constraints
184  getConstraintJacobian(model, data, cm_RF_LOCAL, cd_RF_LOCAL, J_RF_LOCAL_sparse);
185  BOOST_CHECK(J_RF_LOCAL.isApprox(J_RF_LOCAL_sparse));
186 
187  Data::MatrixXs J_LF_LOCAL_sparse(6, model.nv);
188  J_LF_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized
189  // with CRTP on contact constraints
190  getConstraintJacobian(model, data, cm_LF_LOCAL, cd_LF_LOCAL, J_LF_LOCAL_sparse);
191  BOOST_CHECK(J_LF_LOCAL.isApprox(J_LF_LOCAL_sparse));
192 
193  Data::MatrixXs J_clm_LOCAL_sparse(6, model.nv);
194  J_clm_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized
195  // with CRTP on contact constraints
196  getConstraintJacobian(model, data, clm_RF_LF_LOCAL, cld_RF_LF_LOCAL, J_clm_LOCAL_sparse);
197  BOOST_CHECK(J_clm_LOCAL.isApprox(J_clm_LOCAL_sparse));
198  }
199 
200  // 6D - LOCAL_WORLD_ALIGNED
201  {
202  RigidConstraintModel cm_RF_LWA(
203  CONTACT_6D, model, model.getJointId(RF), SE3::Random(), LOCAL_WORLD_ALIGNED);
204  RigidConstraintData cd_RF_LWA(cm_RF_LWA);
205  RigidConstraintModel cm_LF_LWA(
206  CONTACT_6D, model, model.getJointId(LF), SE3::Random(), LOCAL_WORLD_ALIGNED);
207  RigidConstraintData cd_LF_LWA(cm_LF_LWA);
208  RigidConstraintModel clm_RF_LF_LWA(
209  CONTACT_6D, model, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, cm_LF_LWA.joint1_id,
210  cm_LF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED);
211  RigidConstraintData cld_RF_LF_LWA(clm_RF_LF_LWA);
212 
213  Data::Matrix6x J_RF_LOCAL(6, model.nv);
214  J_RF_LOCAL.setZero();
215  getFrameJacobian(
216  model, data, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, LOCAL, J_RF_LOCAL);
217  Data::Matrix6x J_LF_LOCAL(6, model.nv);
218  J_LF_LOCAL.setZero();
219  getFrameJacobian(
220  model, data, cm_LF_LWA.joint1_id, cm_LF_LWA.joint1_placement, LOCAL, J_LF_LOCAL);
221 
222  Data::Matrix6x J_RF_LWA(6, model.nv);
223  J_RF_LWA.setZero();
224  getFrameJacobian(
225  model, data, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED, J_RF_LWA);
226  Data::Matrix6x J_LF_LWA(6, model.nv);
227  J_LF_LWA.setZero();
228  getFrameJacobian(
229  model, data, cm_LF_LWA.joint1_id, cm_LF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED, J_LF_LWA);
230 
231  for (DenseIndex k = 0; k < model.nv; ++k)
232  {
233  BOOST_CHECK(J_RF_LWA.col(k).isZero() != cm_RF_LWA.colwise_joint1_sparsity[k]);
234  BOOST_CHECK(J_LF_LWA.col(k).isZero() != cm_LF_LWA.colwise_joint1_sparsity[k]);
235  }
236  BOOST_CHECK(cm_RF_LWA.colwise_joint2_sparsity.isZero());
237  BOOST_CHECK(cm_LF_LWA.colwise_joint2_sparsity.isZero());
238 
239  const SE3 oMc1 = data.oMi[clm_RF_LF_LWA.joint1_id] * clm_RF_LF_LWA.joint1_placement;
240  const SE3 oMc2 = data.oMi[clm_RF_LF_LWA.joint2_id] * clm_RF_LF_LWA.joint2_placement;
241  const SE3 c1Mc2 = oMc1.actInv(oMc2);
242  const SE3 oMc1_lwa = SE3(oMc1.rotation(), SE3::Vector3::Zero());
243  const SE3 oMc2_lwa = oMc1_lwa * c1Mc2;
244  const Data::Matrix6x J_clm_LWA =
245  oMc1_lwa.toActionMatrix() * J_RF_LOCAL - oMc2_lwa.toActionMatrix() * J_LF_LOCAL;
246 
247  for (DenseIndex k = 0; k < model.nv; ++k)
248  {
249  BOOST_CHECK(J_clm_LWA.col(k).isZero() != within(k, clm_RF_LF_LWA.colwise_span_indexes));
250  }
251 
252  // Check Jacobian
253  Data::MatrixXs J_RF_LWA_sparse(6, model.nv);
254  J_RF_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized
255  // with CRTP on contact constraints
256  getConstraintJacobian(model, data, cm_RF_LWA, cd_RF_LWA, J_RF_LWA_sparse);
257  BOOST_CHECK(J_RF_LWA.isApprox(J_RF_LWA_sparse));
258 
259  Data::MatrixXs J_LF_LWA_sparse(6, model.nv);
260  J_LF_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized
261  // with CRTP on contact constraints
262  getConstraintJacobian(model, data, cm_LF_LWA, cd_LF_LWA, J_LF_LWA_sparse);
263  BOOST_CHECK(J_LF_LWA.isApprox(J_LF_LWA_sparse));
264 
265  Data::MatrixXs J_clm_LWA_sparse(6, model.nv);
266  J_clm_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized
267  // with CRTP on contact constraints
268  getConstraintJacobian(model, data, clm_RF_LF_LWA, cld_RF_LF_LWA, J_clm_LWA_sparse);
269  BOOST_CHECK(J_clm_LWA.isApprox(J_clm_LWA_sparse));
270  }
271 
272  // 3D - LOCAL
273  {
274  RigidConstraintModel cm_RF_LOCAL(CONTACT_3D, model, model.getJointId(RF), SE3::Random(), LOCAL);
275  RigidConstraintData cd_RF_LOCAL(cm_RF_LOCAL);
276  RigidConstraintModel cm_LF_LOCAL(CONTACT_3D, model, model.getJointId(LF), SE3::Random(), LOCAL);
277  RigidConstraintData cd_LF_LOCAL(cm_LF_LOCAL);
278  RigidConstraintModel clm_RF_LF_LOCAL(
279  CONTACT_3D, model, cm_RF_LOCAL.joint1_id, cm_RF_LOCAL.joint1_placement, cm_LF_LOCAL.joint1_id,
280  cm_LF_LOCAL.joint1_placement, LOCAL);
281  RigidConstraintData cld_RF_LF_LOCAL(clm_RF_LF_LOCAL);
282 
283  Data::Matrix6x J_RF_LOCAL(6, model.nv);
284  J_RF_LOCAL.setZero();
285  getFrameJacobian(
286  model, data, cm_RF_LOCAL.joint1_id, cm_RF_LOCAL.joint1_placement, cm_RF_LOCAL.reference_frame,
287  J_RF_LOCAL);
288  Data::Matrix6x J_LF_LOCAL(6, model.nv);
289  J_LF_LOCAL.setZero();
290  getFrameJacobian(
291  model, data, cm_LF_LOCAL.joint1_id, cm_LF_LOCAL.joint1_placement, cm_LF_LOCAL.reference_frame,
292  J_LF_LOCAL);
293 
294  for (DenseIndex k = 0; k < model.nv; ++k)
295  {
296  BOOST_CHECK(
297  J_RF_LOCAL.middleRows<3>(SE3::LINEAR).col(k).isZero()
298  != cm_RF_LOCAL.colwise_joint1_sparsity[k]);
299  BOOST_CHECK(
300  J_LF_LOCAL.middleRows<3>(SE3::LINEAR).col(k).isZero()
301  != cm_LF_LOCAL.colwise_joint1_sparsity[k]);
302  }
303  BOOST_CHECK(cm_RF_LOCAL.colwise_joint2_sparsity.isZero());
304  BOOST_CHECK(cm_LF_LOCAL.colwise_joint2_sparsity.isZero());
305 
306  const SE3 oMc1 = data.oMi[clm_RF_LF_LOCAL.joint1_id] * clm_RF_LF_LOCAL.joint1_placement;
307  const SE3 oMc2 = data.oMi[clm_RF_LF_LOCAL.joint2_id] * clm_RF_LF_LOCAL.joint2_placement;
308  const SE3 c1Mc2 = oMc1.actInv(oMc2);
309  const Data::Matrix3x J_clm_LOCAL = J_RF_LOCAL.middleRows<3>(SE3::LINEAR)
310  - c1Mc2.rotation() * J_LF_LOCAL.middleRows<3>(SE3::LINEAR);
311 
312  for (DenseIndex k = 0; k < model.nv; ++k)
313  {
314  BOOST_CHECK(J_clm_LOCAL.col(k).isZero(0) != within(k, clm_RF_LF_LOCAL.colwise_span_indexes));
315  }
316 
317  // Check Jacobian
318  Data::MatrixXs J_RF_LOCAL_sparse(3, model.nv);
319  J_RF_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized
320  // with CRTP on contact constraints
321  getConstraintJacobian(model, data, cm_RF_LOCAL, cd_RF_LOCAL, J_RF_LOCAL_sparse);
322  BOOST_CHECK(J_RF_LOCAL.middleRows<3>(SE3::LINEAR).isApprox(J_RF_LOCAL_sparse));
323 
324  Data::MatrixXs J_LF_LOCAL_sparse(3, model.nv);
325  J_LF_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized
326  // with CRTP on contact constraints
327  getConstraintJacobian(model, data, cm_LF_LOCAL, cd_LF_LOCAL, J_LF_LOCAL_sparse);
328  BOOST_CHECK(J_LF_LOCAL.middleRows<3>(SE3::LINEAR).isApprox(J_LF_LOCAL_sparse));
329 
330  Data::MatrixXs J_clm_LOCAL_sparse(3, model.nv);
331  J_clm_LOCAL_sparse.setZero(); // TODO: change input type when all the API would be refactorized
332  // with CRTP on contact constraints
333  getConstraintJacobian(model, data, clm_RF_LF_LOCAL, cld_RF_LF_LOCAL, J_clm_LOCAL_sparse);
334  BOOST_CHECK(J_clm_LOCAL.isApprox(J_clm_LOCAL_sparse));
335 
336  check_A1_and_A2(model, data, cm_RF_LOCAL, cd_RF_LOCAL);
337  check_A1_and_A2(model, data, cm_LF_LOCAL, cd_LF_LOCAL);
338  check_A1_and_A2(model, data, clm_RF_LF_LOCAL, cld_RF_LF_LOCAL);
339  }
340 
341  // 3D - LOCAL_WORLD_ALIGNED
342  {
343  RigidConstraintModel cm_RF_LWA(
344  CONTACT_3D, model, model.getJointId(RF), SE3::Random(), LOCAL_WORLD_ALIGNED);
345  RigidConstraintData cd_RF_LWA(cm_RF_LWA);
346  RigidConstraintModel cm_LF_LWA(
347  CONTACT_3D, model, model.getJointId(LF), SE3::Random(), LOCAL_WORLD_ALIGNED);
348  RigidConstraintData cd_LF_LWA(cm_LF_LWA);
349  RigidConstraintModel clm_RF_LF_LWA(
350  CONTACT_3D, model, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, cm_LF_LWA.joint1_id,
351  cm_LF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED);
352  RigidConstraintData cld_RF_LF_LWA(clm_RF_LF_LWA);
353 
354  Data::Matrix6x J_RF_LOCAL(6, model.nv);
355  J_RF_LOCAL.setZero();
356  getFrameJacobian(
357  model, data, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, LOCAL, J_RF_LOCAL);
358  Data::Matrix6x J_LF_LOCAL(6, model.nv);
359  J_LF_LOCAL.setZero();
360  getFrameJacobian(
361  model, data, cm_LF_LWA.joint1_id, cm_LF_LWA.joint1_placement, LOCAL, J_LF_LOCAL);
362 
363  Data::Matrix6x J_RF_LWA(6, model.nv);
364  J_RF_LWA.setZero();
365  getFrameJacobian(
366  model, data, cm_RF_LWA.joint1_id, cm_RF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED, J_RF_LWA);
367  Data::Matrix6x J_LF_LWA(6, model.nv);
368  J_LF_LWA.setZero();
369  getFrameJacobian(
370  model, data, cm_LF_LWA.joint1_id, cm_LF_LWA.joint1_placement, LOCAL_WORLD_ALIGNED, J_LF_LWA);
371 
372  for (DenseIndex k = 0; k < model.nv; ++k)
373  {
374  BOOST_CHECK(
375  J_RF_LWA.middleRows<3>(SE3::LINEAR).col(k).isZero()
376  != cm_RF_LWA.colwise_joint1_sparsity[k]);
377  BOOST_CHECK(
378  J_LF_LWA.middleRows<3>(SE3::LINEAR).col(k).isZero()
379  != cm_LF_LWA.colwise_joint1_sparsity[k]);
380  }
381  BOOST_CHECK(cm_RF_LWA.colwise_joint2_sparsity.isZero());
382  BOOST_CHECK(cm_LF_LWA.colwise_joint2_sparsity.isZero());
383 
384  const SE3 oMc1 = data.oMi[clm_RF_LF_LWA.joint1_id] * clm_RF_LF_LWA.joint1_placement;
385  const SE3 oMc2 = data.oMi[clm_RF_LF_LWA.joint2_id] * clm_RF_LF_LWA.joint2_placement;
386  const SE3 oMc1_lwa = SE3(oMc1.rotation(), SE3::Vector3::Zero());
387  const SE3 oMc2_lwa = SE3(oMc2.rotation(), SE3::Vector3::Zero());
388  const Data::Matrix3x J_clm_LWA =
389  (oMc1_lwa.toActionMatrix() * J_RF_LOCAL - oMc2_lwa.toActionMatrix() * J_LF_LOCAL)
390  .middleRows<3>(Motion::LINEAR);
391 
392  for (DenseIndex k = 0; k < model.nv; ++k)
393  {
394  BOOST_CHECK(J_clm_LWA.col(k).isZero(0) != within(k, clm_RF_LF_LWA.colwise_span_indexes));
395  }
396 
397  // Check Jacobian
398  Data::MatrixXs J_RF_LWA_sparse(3, model.nv);
399  J_RF_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized
400  // with CRTP on contact constraints
401  getConstraintJacobian(model, data, cm_RF_LWA, cd_RF_LWA, J_RF_LWA_sparse);
402  BOOST_CHECK(J_RF_LWA.middleRows<3>(SE3::LINEAR).isApprox(J_RF_LWA_sparse));
403 
404  Data::MatrixXs J_LF_LWA_sparse(3, model.nv);
405  J_LF_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized
406  // with CRTP on contact constraints
407  getConstraintJacobian(model, data, cm_LF_LWA, cd_LF_LWA, J_LF_LWA_sparse);
408  BOOST_CHECK(J_LF_LWA.middleRows<3>(SE3::LINEAR).isApprox(J_LF_LWA_sparse));
409 
410  Data::MatrixXs J_clm_LWA_sparse(3, model.nv);
411  J_clm_LWA_sparse.setZero(); // TODO: change input type when all the API would be refactorized
412  // with CRTP on contact constraints
413  getConstraintJacobian(model, data, clm_RF_LF_LWA, cld_RF_LF_LWA, J_clm_LWA_sparse);
414  BOOST_CHECK(J_clm_LWA.isApprox(J_clm_LWA_sparse));
415  }
416 }
417 
418 BOOST_AUTO_TEST_SUITE_END()
simulation-contact-dynamics.T
int T
Definition: simulation-contact-dynamics.py:94
pinocchio::DataTpl::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: multibody/data.hpp:74
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
frames.hpp
within
bool within(const T &elt, const std::vector< T > &vec)
Definition: contact-models.cpp:27
Eigen
test-cpp2pybind11.m
m
Definition: test-cpp2pybind11.py:22
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::getFrameJacobian
void getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame,...
pinocchio::SE3Tpl< context::Scalar, context::Options >
inverse-kinematics.J
J
Definition: inverse-kinematics.py:31
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:20
kinematics.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
vec
vec
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
contact-cholesky.cmodel
cmodel
Definition: contact-cholesky.py:28
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:325
pinocchio::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition: algorithm/contact-info.hpp:22
rnea.hpp
timer.hpp
aba.hpp
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
check_A1_and_A2
void check_A1_and_A2(const Model &model, const Data &data, const RigidConstraintModel &cmodel, RigidConstraintData &cdata)
Definition: contact-models.cpp:85
joint-configuration.hpp
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: multibody/data.hpp:94
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
mat
mat
pinocchio::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
pinocchio::getConstraintJacobian
void getConstraintJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const RigidConstraintModelTpl< Scalar, Options > &constraint_model, RigidConstraintDataTpl< Scalar, Options > &constraint_data, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the kinematic Jacobian associatied to a given constraint model.
M
M
contact-jacobian.hpp
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
fill
fill
pinocchio::SE3Tpl< context::Scalar, context::Options >::Random
static SE3Tpl Random()
Definition: spatial/se3-tpl.hpp:154
centroidal.hpp
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(contact_models)
Definition: contact-models.cpp:53
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:25
contact-info.hpp
sample-models.hpp
pinocchio::SE3
SE3Tpl< context::Scalar, context::Options > SE3
Definition: spatial/fwd.hpp:60
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
classic-acceleration.hpp
pinocchio::CONTACT_3D
@ CONTACT_3D
Definition: algorithm/contact-info.hpp:21
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
BOOST_CHECK
#define BOOST_CHECK(check)
Definition: overview-urdf.cpp:34
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

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