unittest/kinematics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4 
5 #include <iostream>
6 
14 
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17 
18 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19 
20 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_all)
21 {
22  using namespace Eigen;
23  using namespace pinocchio;
24 
25  Model model;
27 
28  Data data(model), data_ref(model);
29 
30  model.lowerPositionLimit.head<3>().fill(-1.);
31  model.upperPositionLimit.head<3>().fill(1.);
32  VectorXd q = randomConfiguration(model);
33  VectorXd v(VectorXd::Random(model.nv));
34  VectorXd a(VectorXd::Random(model.nv));
35 
36  forwardKinematics(model, data_ref, q, v, a);
38 
39  for (size_t i = 1; i < (size_t)model.njoints; ++i)
40  {
41  BOOST_CHECK(data.oMi[i].isApprox(data_ref.oMi[i]));
42  BOOST_CHECK(data.v[i].isApprox(data_ref.v[i]));
43  BOOST_CHECK(data.ov[i].isApprox(data_ref.oMi[i].act(data_ref.v[i])));
44  BOOST_CHECK(data.a[i].isApprox(data_ref.a[i]));
45  BOOST_CHECK(data.oa[i].isApprox(data_ref.oMi[i].act(data_ref.a[i])));
46  }
47 
48  computeJointJacobians(model, data_ref, q);
49  BOOST_CHECK(data.J.isApprox(data_ref.J));
50 
52  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
53 }
54 
55 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_velocity)
56 {
57  using namespace Eigen;
58  using namespace pinocchio;
59 
60  Model model;
62 
63  Data data(model), data_ref(model);
64 
65  model.lowerPositionLimit.head<3>().fill(-1.);
66  model.upperPositionLimit.head<3>().fill(1.);
67  VectorXd q = randomConfiguration(model);
68  VectorXd v(VectorXd::Random(model.nv));
69  VectorXd a(VectorXd::Random(model.nv));
70 
72 
73  const Model::JointIndex jointId = model.existJointName("rarm2_joint")
74  ? model.getJointId("rarm2_joint")
75  : (Model::Index)(model.njoints - 1);
76  Data::Matrix6x partial_dq(6, model.nv);
77  partial_dq.setZero();
78  Data::Matrix6x partial_dq_local_world_aligned(6, model.nv);
79  partial_dq_local_world_aligned.setZero();
80  Data::Matrix6x partial_dq_local(6, model.nv);
81  partial_dq_local.setZero();
82  Data::Matrix6x partial_dv(6, model.nv);
83  partial_dv.setZero();
84  Data::Matrix6x partial_dv_local_world_aligned(6, model.nv);
85  partial_dv_local_world_aligned.setZero();
86  Data::Matrix6x partial_dv_local(6, model.nv);
87  partial_dv_local.setZero();
88 
89  getJointVelocityDerivatives(model, data, jointId, WORLD, partial_dq, partial_dv);
90 
92  model, data, jointId, LOCAL_WORLD_ALIGNED, partial_dq_local_world_aligned,
93  partial_dv_local_world_aligned);
94 
95  getJointVelocityDerivatives(model, data, jointId, LOCAL, partial_dq_local, partial_dv_local);
96 
97  Data::Matrix6x J_ref(6, model.nv);
98  J_ref.setZero();
99  Data::Matrix6x J_ref_local_world_aligned(6, model.nv);
100  J_ref_local_world_aligned.setZero();
101  Data::Matrix6x J_ref_local(6, model.nv);
102  J_ref_local.setZero();
103  computeJointJacobians(model, data_ref, q);
104  getJointJacobian(model, data_ref, jointId, WORLD, J_ref);
105  getJointJacobian(model, data_ref, jointId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned);
106  getJointJacobian(model, data_ref, jointId, LOCAL, J_ref_local);
107 
108  BOOST_CHECK(partial_dv.isApprox(J_ref));
109  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned));
110  BOOST_CHECK(partial_dv_local.isApprox(J_ref_local));
111 
112  // Check against finite differences
113  Data::Matrix6x partial_dq_fd(6, model.nv);
114  partial_dq_fd.setZero();
115  Data::Matrix6x partial_dq_fd_local_world_aligned(6, model.nv);
116  partial_dq_fd_local_world_aligned.setZero();
117  Data::Matrix6x partial_dq_fd_local(6, model.nv);
118  partial_dq_fd_local.setZero();
119  Data::Matrix6x partial_dv_fd(6, model.nv);
120  partial_dv_fd.setZero();
121  Data::Matrix6x partial_dv_fd_local_world_aligned(6, model.nv);
122  partial_dv_fd_local_world_aligned.setZero();
123  Data::Matrix6x partial_dv_fd_local(6, model.nv);
124  partial_dv_fd_local.setZero();
125  const double alpha = 1e-8;
126 
127  // dvel/dv
128  Eigen::VectorXd v_plus(v);
129  Data data_plus(model);
130  forwardKinematics(model, data_ref, q, v);
131  SE3 oMi_rot(SE3::Identity());
132  oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
133  Motion v0(data_ref.oMi[jointId].act(data_ref.v[jointId]));
134  Motion v0_local_world_aligned(oMi_rot.act(data_ref.v[jointId]));
135  Motion v0_local(data_ref.v[jointId]);
136  for (int k = 0; k < model.nv; ++k)
137  {
138  v_plus[k] += alpha;
139  forwardKinematics(model, data_plus, q, v_plus);
140 
141  partial_dv_fd.col(k) =
142  (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector() / alpha;
143  partial_dv_fd_local_world_aligned.col(k) =
144  (oMi_rot.act(data_plus.v[jointId]) - v0_local_world_aligned).toVector() / alpha;
145  partial_dv_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector() / alpha;
146  v_plus[k] -= alpha;
147  }
148 
149  BOOST_CHECK(partial_dv.isApprox(partial_dv_fd, sqrt(alpha)));
150  BOOST_CHECK(
151  partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned, sqrt(alpha)));
152  BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local, sqrt(alpha)));
153 
154  // dvel/dq
155  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
156  forwardKinematics(model, data_ref, q, v);
157  v0 = data_ref.oMi[jointId].act(data_ref.v[jointId]);
158 
159  for (int k = 0; k < model.nv; ++k)
160  {
161  v_eps[k] += alpha;
162  q_plus = integrate(model, q, v_eps);
163  forwardKinematics(model, data_plus, q_plus, v);
164 
165  SE3 oMi_plus_rot = data_plus.oMi[jointId];
166  oMi_plus_rot.translation().setZero();
167 
168  Motion v_plus_local_world_aligned = oMi_plus_rot.act(data_plus.v[jointId]);
169  SE3::Vector3 trans = data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation();
170  v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
171  partial_dq_fd.col(k) =
172  (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector() / alpha;
173  partial_dq_fd_local_world_aligned.col(k) =
174  (v_plus_local_world_aligned - v0_local_world_aligned).toVector() / alpha;
175  partial_dq_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector() / alpha;
176  v_eps[k] -= alpha;
177  }
178 
179  BOOST_CHECK(partial_dq.isApprox(partial_dq_fd, sqrt(alpha)));
180  BOOST_CHECK(
181  partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned, sqrt(alpha)));
182  BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local, sqrt(alpha)));
183 }
184 
185 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
186 {
187  using namespace Eigen;
188  using namespace pinocchio;
189 
190  Model model;
192 
193  Data data(model), data_ref(model);
194 
195  model.lowerPositionLimit.head<3>().fill(-1.);
196  model.upperPositionLimit.head<3>().fill(1.);
197  VectorXd q = randomConfiguration(model);
198  VectorXd v(VectorXd::Random(model.nv));
199  VectorXd a(VectorXd::Random(model.nv));
200 
202 
203  const Model::JointIndex jointId = model.existJointName("rarm2_joint")
204  ? model.getJointId("rarm2_joint")
205  : (Model::Index)(model.njoints - 1);
206 
207  Data::Matrix6x v_partial_dq(6, model.nv);
208  v_partial_dq.setZero();
209  Data::Matrix6x v_partial_dq_local(6, model.nv);
210  v_partial_dq_local.setZero();
211  Data::Matrix6x v_partial_dq_local_world_aligned(6, model.nv);
212  v_partial_dq_local_world_aligned.setZero();
213  Data::Matrix6x a_partial_dq(6, model.nv);
214  a_partial_dq.setZero();
215  Data::Matrix6x a_partial_dq_local_world_aligned(6, model.nv);
216  a_partial_dq_local_world_aligned.setZero();
217  Data::Matrix6x a_partial_dq_local(6, model.nv);
218  a_partial_dq_local.setZero();
219  Data::Matrix6x a_partial_dv(6, model.nv);
220  a_partial_dv.setZero();
221  Data::Matrix6x a_partial_dv_local_world_aligned(6, model.nv);
222  a_partial_dv_local_world_aligned.setZero();
223  Data::Matrix6x a_partial_dv_local(6, model.nv);
224  a_partial_dv_local.setZero();
225  Data::Matrix6x a_partial_da(6, model.nv);
226  a_partial_da.setZero();
227  Data::Matrix6x a_partial_da_local_world_aligned(6, model.nv);
228  a_partial_da_local_world_aligned.setZero();
229  Data::Matrix6x a_partial_da_local(6, model.nv);
230  a_partial_da_local.setZero();
231 
233  model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
234 
236  model, data, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned,
237  a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned,
238  a_partial_da_local_world_aligned);
239 
241  model, data, jointId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local,
242  a_partial_da_local);
243 
244  // Check v_partial_dq against getJointVelocityDerivatives
245  {
246  Data data_v(model);
248 
249  Data::Matrix6x v_partial_dq_ref(6, model.nv);
250  v_partial_dq_ref.setZero();
251  Data::Matrix6x v_partial_dq_ref_local_world_aligned(6, model.nv);
252  v_partial_dq_ref_local_world_aligned.setZero();
253  Data::Matrix6x v_partial_dq_ref_local(6, model.nv);
254  v_partial_dq_ref_local.setZero();
255  Data::Matrix6x v_partial_dv_ref(6, model.nv);
256  v_partial_dv_ref.setZero();
257  Data::Matrix6x v_partial_dv_ref_local_world_aligned(6, model.nv);
258  v_partial_dv_ref_local_world_aligned.setZero();
259  Data::Matrix6x v_partial_dv_ref_local(6, model.nv);
260  v_partial_dv_ref_local.setZero();
261 
262  getJointVelocityDerivatives(model, data_v, jointId, WORLD, v_partial_dq_ref, v_partial_dv_ref);
263 
264  BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
265  BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref));
266 
268  model, data_v, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_ref_local_world_aligned,
269  v_partial_dv_ref_local_world_aligned);
270 
271  BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned));
272  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned));
273 
275  model, data_v, jointId, LOCAL, v_partial_dq_ref_local, v_partial_dv_ref_local);
276 
277  BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
278  BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local));
279  }
280 
281  Data::Matrix6x J_ref(6, model.nv);
282  J_ref.setZero();
283  Data::Matrix6x J_ref_local(6, model.nv);
284  J_ref_local.setZero();
285  Data::Matrix6x J_ref_local_world_aligned(6, model.nv);
286  J_ref_local_world_aligned.setZero();
287  computeJointJacobians(model, data_ref, q);
288  getJointJacobian(model, data_ref, jointId, WORLD, J_ref);
289  getJointJacobian(model, data_ref, jointId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned);
290  getJointJacobian(model, data_ref, jointId, LOCAL, J_ref_local);
291 
292  BOOST_CHECK(a_partial_da.isApprox(J_ref));
293  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned));
294  BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local));
295 
296  // Check against finite differences
297  Data::Matrix6x a_partial_da_fd(6, model.nv);
298  a_partial_da_fd.setZero();
299  Data::Matrix6x a_partial_da_fd_local_world_aligned(6, model.nv);
300  a_partial_da_fd_local_world_aligned.setZero();
301  Data::Matrix6x a_partial_da_fd_local(6, model.nv);
302  a_partial_da_fd_local.setZero();
303  const double alpha = 1e-8;
304 
305  Eigen::VectorXd v_plus(v), a_plus(a);
306  Data data_plus(model);
307  forwardKinematics(model, data_ref, q, v, a);
308  SE3 oMi_rot(SE3::Identity());
309  oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
310 
311  // dacc/da
312  Motion a0(data_ref.oMi[jointId].act(data_ref.a[jointId]));
313  Motion a0_local_world_aligned(oMi_rot.act(data_ref.a[jointId]));
314  Motion a0_local(data_ref.a[jointId]);
315  for (int k = 0; k < model.nv; ++k)
316  {
317  a_plus[k] += alpha;
318  forwardKinematics(model, data_plus, q, v, a_plus);
319 
320  a_partial_da_fd.col(k) =
321  (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha;
322  a_partial_da_fd_local_world_aligned.col(k) =
323  (oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector() / alpha;
324  a_partial_da_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha;
325  a_plus[k] -= alpha;
326  }
327  BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd, sqrt(alpha)));
328  BOOST_CHECK(
329  a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned, sqrt(alpha)));
330  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local, sqrt(alpha)));
331  motionSet::se3Action(data_ref.oMi[jointId].inverse(), a_partial_da, a_partial_da_local);
332  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local, sqrt(alpha)));
333 
334  // dacc/dv
335  Data::Matrix6x a_partial_dv_fd(6, model.nv);
336  a_partial_dv_fd.setZero();
337  Data::Matrix6x a_partial_dv_fd_local_world_aligned(6, model.nv);
338  a_partial_dv_fd_local_world_aligned.setZero();
339  Data::Matrix6x a_partial_dv_fd_local(6, model.nv);
340  a_partial_dv_fd_local.setZero();
341  for (int k = 0; k < model.nv; ++k)
342  {
343  v_plus[k] += alpha;
344  forwardKinematics(model, data_plus, q, v_plus, a);
345 
346  a_partial_dv_fd.col(k) =
347  (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha;
348  a_partial_dv_fd_local_world_aligned.col(k) =
349  (oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector() / alpha;
350  a_partial_dv_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha;
351  v_plus[k] -= alpha;
352  }
353 
354  BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd, sqrt(alpha)));
355  BOOST_CHECK(
356  a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned, sqrt(alpha)));
357  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local, sqrt(alpha)));
358  motionSet::se3Action(data_ref.oMi[jointId].inverse(), a_partial_dv, a_partial_dv_local);
359  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local, sqrt(alpha)));
360 
361  // dacc/dq
362  a_partial_dq.setZero();
363  a_partial_dv.setZero();
364  a_partial_da.setZero();
365 
366  a_partial_dq_local_world_aligned.setZero();
367  a_partial_dv_local_world_aligned.setZero();
368  a_partial_da_local_world_aligned.setZero();
369 
370  a_partial_dq_local.setZero();
371  a_partial_dv_local.setZero();
372  a_partial_da_local.setZero();
373 
374  Data::Matrix6x a_partial_dq_fd(6, model.nv);
375  a_partial_dq_fd.setZero();
376  Data::Matrix6x a_partial_dq_fd_local_world_aligned(6, model.nv);
377  a_partial_dq_fd_local_world_aligned.setZero();
378  Data::Matrix6x a_partial_dq_fd_local(6, model.nv);
379  a_partial_dq_fd_local.setZero();
380 
383  model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
384 
386  model, data, jointId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned,
387  a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned,
388  a_partial_da_local_world_aligned);
389 
391  model, data, jointId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local,
392  a_partial_da_local);
393 
394  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
395  forwardKinematics(model, data_ref, q, v, a);
396  a0 = data_ref.oMi[jointId].act(data_ref.a[jointId]);
397  oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
398  a0_local_world_aligned = oMi_rot.act(data_ref.a[jointId]);
399  a0_local = data_ref.a[jointId];
400 
401  for (int k = 0; k < model.nv; ++k)
402  {
403  v_eps[k] += alpha;
404  q_plus = integrate(model, q, v_eps);
405  forwardKinematics(model, data_plus, q_plus, v, a);
406 
407  SE3 oMi_plus_rot = data_plus.oMi[jointId];
408  oMi_plus_rot.translation().setZero();
409 
410  Motion a_plus_local_world_aligned = oMi_plus_rot.act(data_plus.a[jointId]);
411  const SE3::Vector3 trans =
412  data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation();
413  a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans);
414  a_partial_dq_fd.col(k) =
415  (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector() / alpha;
416  a_partial_dq_fd_local_world_aligned.col(k) =
417  (a_plus_local_world_aligned - a0_local_world_aligned).toVector() / alpha;
418  a_partial_dq_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector() / alpha;
419  v_eps[k] -= alpha;
420  }
421 
422  BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd, sqrt(alpha)));
423  BOOST_CHECK(
424  a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned, sqrt(alpha)));
425  BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local, sqrt(alpha)));
426 }
427 
428 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_against_classic_formula)
429 {
430  using namespace Eigen;
431  using namespace pinocchio;
432 
433  Model model;
435 
436  Data data(model), data_ref(model);
437 
438  model.lowerPositionLimit.head<3>().fill(-1.);
439  model.upperPositionLimit.head<3>().fill(1.);
440  VectorXd q = randomConfiguration(model);
441  VectorXd v(VectorXd::Random(model.nv));
442  VectorXd a(VectorXd::Random(model.nv));
443 
444  const Model::JointIndex jointId = model.existJointName("rarm4_joint")
445  ? model.getJointId("rarm4_joint")
446  : (Model::Index)(model.njoints - 1);
447  Data::Matrix6x v_partial_dq(6, model.nv);
448  v_partial_dq.setZero();
449  Data::Matrix6x v_partial_dq_ref(6, model.nv);
450  v_partial_dq_ref.setZero();
451  Data::Matrix6x v_partial_dv_ref(6, model.nv);
452  v_partial_dv_ref.setZero();
453  Data::Matrix6x a_partial_dq(6, model.nv);
454  a_partial_dq.setZero();
455  Data::Matrix6x a_partial_dv(6, model.nv);
456  a_partial_dv.setZero();
457  Data::Matrix6x a_partial_da(6, model.nv);
458  a_partial_da.setZero();
459 
460  // LOCAL: da/dv == dJ/dt + dv/dq
461  // {
462  // Data::Matrix6x rhs(6,model.nv); rhs.setZero();
463  //
464  // v_partial_dq.setZero();
465  // a_partial_dq.setZero();
466  // a_partial_dv.setZero();
467  // a_partial_da.setZero();
468  //
469  // computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
470  // computeForwardKinematicsDerivatives(model,data,q,v,a);
471  //
472  // getJointAccelerationDerivatives<LOCAL>(model,data,jointId,
473  // v_partial_dq,
474  // a_partial_dq,a_partial_dv,a_partial_da);
475  //
476  // getJointJacobianTimeVariation<LOCAL>(model,data_ref,jointId,rhs);
477  //
478  // v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
479  // getJointVelocityDerivatives<LOCAL>(model,data_ref,jointId,
480  // v_partial_dq_ref,v_partial_dv_ref);
481  // rhs += v_partial_dq_ref;
482  // BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12));
483  //
484  // std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
485  // std::cout << "rhs\n" << rhs << std::endl;
486  // }
487 
488  // WORLD: da/dv == dJ/dt + dv/dq
489  {
490  Data::Matrix6x rhs(6, model.nv);
491  rhs.setZero();
492 
493  v_partial_dq.setZero();
494  a_partial_dq.setZero();
495  a_partial_dv.setZero();
496  a_partial_da.setZero();
497 
500 
502  model, data, jointId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
503 
504  getJointJacobianTimeVariation(model, data_ref, jointId, WORLD, rhs);
505 
506  v_partial_dq_ref.setZero();
507  v_partial_dv_ref.setZero();
509  model, data_ref, jointId, WORLD, v_partial_dq_ref, v_partial_dv_ref);
510  rhs += v_partial_dq_ref;
511  BOOST_CHECK(a_partial_dv.isApprox(rhs, 1e-12));
512 
513  // std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
514  // std::cout << "rhs\n" << rhs << std::endl;
515  }
516 
517  // // WORLD: da/dq == d/dt(dv/dq)
518  // {
519  // const double alpha = 1e-8;
520  // Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);
521  //
522  // Data data_plus(model);
523  // v_plus = v + alpha * a;
524  // q_plus = integrate(model,q,alpha*v);
525  //
526  // computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a);
527  // computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
528  //
529  // Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero();
530  // Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero();
531  //
532  // v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
533  //
534  // v_partial_dq.setZero();
535  // a_partial_dq.setZero();
536  // a_partial_dv.setZero();
537  // a_partial_da.setZero();
538  //
539  // getJointVelocityDerivatives<WORLD>(model,data_ref,jointId,
540  // v_partial_dq_ref,v_partial_dv_ref);
541  // getJointVelocityDerivatives<WORLD>(model,data_plus,jointId,
542  // v_partial_dq_plus,v_partial_dv_plus);
543  //
544  // getJointAccelerationDerivatives<WORLD>(model,data_ref,jointId,
545  // v_partial_dq,
546  // a_partial_dq,a_partial_dv,a_partial_da);
547  //
548  // Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
549  // {
550  // Data data_fd(model);
551  // VectorXd q_fd(model.nq), v_eps(model.nv); v_eps.setZero();
552  // for(int k = 0; k < model.nv; ++k)
553  // {
554  // v_eps[k] += alpha;
555  // q_fd = integrate(model,q,v_eps);
556  // forwardKinematics(model,data_fd,q_fd,v,a);
557  // a_partial_dq_fd.col(k) = (data_fd.oMi[jointId].act(data_fd.a[jointId]) -
558  // data_ref.oa[jointId]).toVector()/alpha; v_eps[k] = 0.;
559  // }
560  // }
561  //
562  // Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha;
563  // BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha)));
564  //
565  // std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
566  // std::cout << "a_partial_dq_fd\n" << a_partial_dq_fd << std::endl;
567  // std::cout << "rhs\n" << rhs << std::endl;
568  // }
569 
570  // LOCAL: da/dq == d/dt(dv/dq)
571  {
572  const double alpha = 1e-8;
573  Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);
574 
575  Data data_plus(model);
576  v_plus = v + alpha * a;
577  q_plus = integrate(model, q, alpha * v);
578 
579  computeForwardKinematicsDerivatives(model, data_plus, q_plus, v_plus, a);
581 
582  Data::Matrix6x v_partial_dq_plus(6, model.nv);
583  v_partial_dq_plus.setZero();
584  Data::Matrix6x v_partial_dv_plus(6, model.nv);
585  v_partial_dv_plus.setZero();
586 
587  v_partial_dq_ref.setZero();
588  v_partial_dv_ref.setZero();
589 
590  v_partial_dq.setZero();
591  a_partial_dq.setZero();
592  a_partial_dv.setZero();
593  a_partial_da.setZero();
594 
596  model, data_ref, jointId, LOCAL, v_partial_dq_ref, v_partial_dv_ref);
598  model, data_plus, jointId, LOCAL, v_partial_dq_plus, v_partial_dv_plus);
599 
601  model, data_ref, jointId, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
602 
603  Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref) / alpha;
604  BOOST_CHECK(a_partial_dq.isApprox(rhs, sqrt(alpha)));
605 
606  // std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
607  // std::cout << "rhs\n" << rhs << std::endl;
608  }
609 }
610 
611 BOOST_AUTO_TEST_CASE(test_classic_acceleration_derivatives)
612 {
613  using namespace Eigen;
614  using namespace pinocchio;
615 
616  Model model;
618 
619  Data data(model), data_ref(model), data_plus(model);
620 
621  model.lowerPositionLimit.head<3>().fill(-1.);
622  model.upperPositionLimit.head<3>().fill(1.);
623  VectorXd q = randomConfiguration(model);
624  VectorXd v = VectorXd::Random(model.nv);
625  VectorXd a = VectorXd::Random(model.nv);
626 
627  const Model::JointIndex joint_id = model.existJointName("rarm4_joint")
628  ? model.getJointId("rarm4_joint")
629  : (Model::Index)(model.njoints - 1);
630  Data::Matrix3x v3_partial_dq(3, model.nv);
631  v3_partial_dq.setZero();
632  Data::Matrix3x a3_partial_dq(3, model.nv);
633  a3_partial_dq.setZero();
634  Data::Matrix3x a3_partial_dv(3, model.nv);
635  a3_partial_dv.setZero();
636  Data::Matrix3x a3_partial_da(3, model.nv);
637  a3_partial_da.setZero();
638 
639  Data::Matrix6x v_partial_dq_ref(6, model.nv);
640  v_partial_dq_ref.setZero();
641  Data::Matrix6x v_partial_dv_ref(6, model.nv);
642  v_partial_dv_ref.setZero();
643  Data::Matrix6x a_partial_dq_ref(6, model.nv);
644  a_partial_dq_ref.setZero();
645  Data::Matrix6x a_partial_dv_ref(6, model.nv);
646  a_partial_dv_ref.setZero();
647  Data::Matrix6x a_partial_da_ref(6, model.nv);
648  a_partial_da_ref.setZero();
649 
650  computeForwardKinematicsDerivatives(model, data_ref, q, 0 * v, a);
652 
653  // LOCAL
655  model, data_ref, joint_id, LOCAL, v_partial_dq_ref, v_partial_dv_ref, a_partial_dq_ref,
656  a_partial_dv_ref, a_partial_da_ref);
657 
659  model, data, joint_id, SE3::Identity(), LOCAL, v3_partial_dq, a3_partial_dq, a3_partial_dv,
660  a3_partial_da);
661 
662  BOOST_CHECK(v3_partial_dq.isApprox(v_partial_dq_ref.middleRows<3>(Motion::LINEAR)));
663  BOOST_CHECK(a3_partial_dq.isApprox(a_partial_dq_ref.middleRows<3>(Motion::LINEAR)));
664  BOOST_CHECK(a3_partial_dv.isApprox(a_partial_dv_ref.middleRows<3>(Motion::LINEAR)));
665  BOOST_CHECK(a3_partial_da.isApprox(a_partial_da_ref.middleRows<3>(Motion::LINEAR)));
666 
667  // LOCAL_WORLD_ALIGNED
668  v_partial_dq_ref.setZero();
669  v_partial_dv_ref.setZero();
670  a_partial_dq_ref.setZero();
671  a_partial_dv_ref.setZero();
672  a_partial_da_ref.setZero();
674  model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, v_partial_dq_ref, v_partial_dv_ref,
675  a_partial_dq_ref, a_partial_dv_ref, a_partial_da_ref);
676 
677  v3_partial_dq.setZero();
678  a3_partial_dq.setZero();
679  a3_partial_dv.setZero();
680  a3_partial_da.setZero();
682  model, data, joint_id, SE3::Identity(), LOCAL_WORLD_ALIGNED, v3_partial_dq, a3_partial_dq,
683  a3_partial_dv, a3_partial_da);
684 
685  BOOST_CHECK(v3_partial_dq.isApprox(v_partial_dq_ref.middleRows<3>(Motion::LINEAR)));
686  // BOOST_CHECK(a3_partial_dq.isApprox(a_partial_dq_ref.middleRows<3>(Motion::LINEAR)));
687  BOOST_CHECK(a3_partial_dv.isApprox(a_partial_dv_ref.middleRows<3>(Motion::LINEAR)));
688  BOOST_CHECK(a3_partial_da.isApprox(a_partial_da_ref.middleRows<3>(Motion::LINEAR)));
689 
690  // std::cout << "a3_partial_dq:\n" << a3_partial_dq << std::endl;
691  // std::cout << "a3_partial_dq_ref:\n" << a_partial_dq_ref.middleRows<3>(Motion::LINEAR) <<
692  // std::endl;
693 
694  const SE3 iMpoint = SE3::Random();
696 
697  v3_partial_dq.setZero();
698  a3_partial_dq.setZero();
699  a3_partial_dv.setZero();
700  a3_partial_da.setZero();
702  model, data, joint_id, iMpoint, LOCAL, v3_partial_dq, a3_partial_dq, a3_partial_dv,
703  a3_partial_da);
704 
705  Data::Matrix3x v3_partial_dq_LWA(3, model.nv);
706  v3_partial_dq_LWA.setZero();
707  Data::Matrix3x a3_partial_dq_LWA(3, model.nv);
708  a3_partial_dq_LWA.setZero();
709  Data::Matrix3x a3_partial_LWA_dv(3, model.nv);
710  a3_partial_LWA_dv.setZero();
711  Data::Matrix3x a3_partial_LWA_da(3, model.nv);
712  a3_partial_LWA_da.setZero();
713 
715  model, data, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v3_partial_dq_LWA, a3_partial_dq_LWA,
716  a3_partial_LWA_dv, a3_partial_LWA_da);
717 
718  const double eps = 1e-8;
719  Eigen::VectorXd v_plus = Eigen::VectorXd::Zero(model.nv);
720 
721  Data::Matrix3x v3_partial_dq_fd(3, model.nv);
722  Data::Matrix3x v3_partial_dv_fd(3, model.nv);
723  Data::Matrix3x a3_partial_dq_fd(3, model.nv);
724  Data::Matrix3x a3_partial_dv_fd(3, model.nv);
725  Data::Matrix3x a3_partial_da_fd(3, model.nv);
726 
727  Data::Matrix3x v3_partial_dq_LWA_fd(3, model.nv);
728  Data::Matrix3x v3_partial_dv_LWA_fd(3, model.nv);
729  Data::Matrix3x a3_partial_dq_LWA_fd(3, model.nv);
730  Data::Matrix3x a3_partial_dv_LWA_fd(3, model.nv);
731  Data::Matrix3x a3_partial_da_LWA_fd(3, model.nv);
732 
733  const SE3 oMpoint = data.oMi[joint_id] * iMpoint;
734  const Motion::Vector3 point_vec_L = iMpoint.actInv(data.v[joint_id]).linear(); // LOCAL
735  const Motion::Vector3 point_acc_L =
736  classicAcceleration(data.v[joint_id], data.a[joint_id], iMpoint); // LOCAL
737  const Motion::Vector3 point_vec_LWA = oMpoint.rotation() * point_vec_L; // LOCAL_WORLD_ALIGNED
738  const Motion::Vector3 point_acc_LWA = oMpoint.rotation() * point_acc_L; // LOCAL_WORLD_ALIGNED
739 
740  // Derivatives w.r.t q
741  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
742  {
743  v_plus[k] = eps;
744  const VectorXd q_plus = integrate(model, q, v_plus);
745  forwardKinematics(model, data_plus, q_plus, v, a);
746 
747  const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint;
748  const Motion::Vector3 point_vec_L_plus = iMpoint.actInv(data_plus.v[joint_id]).linear();
749  const Motion::Vector3 point_acc_L_plus =
750  classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint);
751 
752  const Motion::Vector3 point_vec_LWA_plus = oMpoint_plus.rotation() * point_vec_L_plus;
753  const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus;
754 
755  v3_partial_dq_fd.col(k) = (point_vec_L_plus - point_vec_L) / eps;
756  a3_partial_dq_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps;
757 
758  v3_partial_dq_LWA_fd.col(k) = (point_vec_LWA_plus - point_vec_LWA) / eps;
759  a3_partial_dq_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps;
760 
761  v_plus[k] = 0.;
762  }
763 
764  BOOST_CHECK(v3_partial_dq_fd.isApprox(v3_partial_dq, sqrt(eps)));
765  BOOST_CHECK(a3_partial_dq_fd.isApprox(a3_partial_dq, sqrt(eps)));
766 
767  BOOST_CHECK(v3_partial_dq_LWA_fd.isApprox(v3_partial_dq_LWA, sqrt(eps)));
768  BOOST_CHECK(a3_partial_dq_LWA_fd.isApprox(a3_partial_dq_LWA, sqrt(eps)));
769 
770  // Derivatives w.r.t v
771  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
772  {
773  v_plus = v;
774  v_plus[k] += eps;
775  forwardKinematics(model, data_plus, q, v_plus, a);
776 
777  const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint;
778  const Motion::Vector3 point_vec_L_plus = iMpoint.actInv(data_plus.v[joint_id]).linear();
779  const Motion::Vector3 point_acc_L_plus =
780  classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint);
781 
782  const Motion::Vector3 point_vec_LWA_plus = oMpoint_plus.rotation() * point_vec_L_plus;
783  const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus;
784 
785  v3_partial_dv_fd.col(k) = (point_vec_L_plus - point_vec_L) / eps;
786  a3_partial_dv_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps;
787 
788  v3_partial_dv_LWA_fd.col(k) = (point_vec_LWA_plus - point_vec_LWA) / eps;
789  a3_partial_dv_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps;
790  }
791 
792  BOOST_CHECK(v3_partial_dv_fd.isApprox(a3_partial_da, sqrt(eps)));
793  BOOST_CHECK(a3_partial_dv_fd.isApprox(a3_partial_dv, sqrt(eps)));
794 
795  // Derivatives w.r.t v
796  Eigen::VectorXd a_plus = Eigen::VectorXd::Zero(model.nv);
797  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
798  {
799  a_plus = a;
800  a_plus[k] += eps;
801  forwardKinematics(model, data_plus, q, v, a_plus);
802 
803  const SE3 oMpoint_plus = data_plus.oMi[joint_id] * iMpoint;
804  const Motion::Vector3 point_acc_L_plus =
805  classicAcceleration(data_plus.v[joint_id], data_plus.a[joint_id], iMpoint);
806 
807  const Motion::Vector3 point_acc_LWA_plus = oMpoint_plus.rotation() * point_acc_L_plus;
808 
809  a3_partial_da_fd.col(k) = (point_acc_L_plus - point_acc_L) / eps;
810  a3_partial_da_LWA_fd.col(k) = (point_acc_LWA_plus - point_acc_LWA) / eps;
811  }
812 
813  BOOST_CHECK(a3_partial_da_fd.isApprox(a3_partial_da, sqrt(eps)));
814 
815  // Test other signature
816  Data data_other(model);
817  Data::Matrix3x v3_partial_dq_other(3, model.nv);
818  v3_partial_dq_other.setZero();
819  Data::Matrix3x v3_partial_dv_other(3, model.nv);
820  v3_partial_dv_other.setZero();
821  Data::Matrix3x a3_partial_dq_other(3, model.nv);
822  a3_partial_dq_other.setZero();
823  Data::Matrix3x a3_partial_dv_other(3, model.nv);
824  a3_partial_dv_other.setZero();
825  Data::Matrix3x a3_partial_da_other(3, model.nv);
826  a3_partial_da_other.setZero();
827 
830  model, data_other, joint_id, iMpoint, LOCAL, v3_partial_dq_other, v3_partial_dv_other,
831  a3_partial_dq_other, a3_partial_dv_other, a3_partial_da_other);
832 
833  BOOST_CHECK(v3_partial_dq_other.isApprox(v3_partial_dq));
834  BOOST_CHECK(v3_partial_dv_other.isApprox(a3_partial_da));
835  BOOST_CHECK(a3_partial_dq_other.isApprox(a3_partial_dq));
836  BOOST_CHECK(a3_partial_dv_other.isApprox(a3_partial_dv));
837  BOOST_CHECK(a3_partial_da_other.isApprox(a3_partial_da));
838 }
839 
840 BOOST_AUTO_TEST_CASE(test_classic_velocity_derivatives)
841 {
842  using namespace Eigen;
843  using namespace pinocchio;
844 
845  Model model;
847 
848  Data data(model), data_ref(model);
849 
850  model.lowerPositionLimit.head<3>().fill(-1.);
851  model.upperPositionLimit.head<3>().fill(1.);
852  VectorXd q = randomConfiguration(model);
853  VectorXd v = VectorXd::Random(model.nv);
854  VectorXd a = VectorXd::Random(model.nv);
855 
856  const SE3 iMpoint = SE3::Random();
857  const Model::JointIndex joint_id = model.existJointName("rarm4_joint")
858  ? model.getJointId("rarm4_joint")
859  : (Model::Index)(model.njoints - 1);
860  Data::Matrix3x v3_partial_dq_L(3, model.nv);
861  v3_partial_dq_L.setZero();
862  Data::Matrix3x v3_partial_dv_L(3, model.nv);
863  v3_partial_dv_L.setZero();
864 
867  model, data, joint_id, iMpoint, LOCAL, v3_partial_dq_L, v3_partial_dv_L);
868 
869  Data::Matrix3x v_partial_dq_ref_L(3, model.nv);
870  v_partial_dq_ref_L.setZero();
871  Data::Matrix3x v_partial_dv_ref_L(3, model.nv);
872  v_partial_dv_ref_L.setZero();
873  Data::Matrix3x a_partial_dq_ref_L(3, model.nv);
874  a_partial_dq_ref_L.setZero();
875  Data::Matrix3x a_partial_dv_ref_L(3, model.nv);
876  a_partial_dv_ref_L.setZero();
877  Data::Matrix3x a_partial_da_ref_L(3, model.nv);
878  a_partial_da_ref_L.setZero();
879 
882  model, data_ref, joint_id, iMpoint, LOCAL, v_partial_dq_ref_L, v_partial_dv_ref_L,
883  a_partial_dq_ref_L, a_partial_dv_ref_L, a_partial_da_ref_L);
884 
885  BOOST_CHECK(v3_partial_dq_L.isApprox(v_partial_dq_ref_L));
886  BOOST_CHECK(v3_partial_dv_L.isApprox(v_partial_dv_ref_L));
887 
888  // LOCAL_WORLD_ALIGNED
889  Data::Matrix3x v3_partial_dq_LWA(3, model.nv);
890  v3_partial_dq_LWA.setZero();
891  Data::Matrix3x v3_partial_dv_LWA(3, model.nv);
892  v3_partial_dv_LWA.setZero();
893 
895  model, data, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v3_partial_dq_LWA, v3_partial_dv_LWA);
896 
897  Data::Matrix3x v_partial_dq_ref_LWA(3, model.nv);
898  v_partial_dq_ref_LWA.setZero();
899  Data::Matrix3x v_partial_dv_ref_LWA(3, model.nv);
900  v_partial_dv_ref_LWA.setZero();
901  Data::Matrix3x a_partial_dq_ref_LWA(3, model.nv);
902  a_partial_dq_ref_LWA.setZero();
903  Data::Matrix3x a_partial_dv_ref_LWA(3, model.nv);
904  a_partial_dv_ref_LWA.setZero();
905  Data::Matrix3x a_partial_da_ref_LWA(3, model.nv);
906  a_partial_da_ref_LWA.setZero();
907 
909  model, data_ref, joint_id, iMpoint, LOCAL_WORLD_ALIGNED, v_partial_dq_ref_LWA,
910  v_partial_dv_ref_LWA, a_partial_dq_ref_LWA, a_partial_dv_ref_LWA, a_partial_da_ref_LWA);
911 
912  BOOST_CHECK(v3_partial_dq_LWA.isApprox(v_partial_dq_ref_LWA));
913  BOOST_CHECK(v3_partial_dv_LWA.isApprox(v_partial_dv_ref_LWA));
914 }
915 
916 BOOST_AUTO_TEST_CASE(test_kinematics_hessians)
917 {
918  using namespace Eigen;
919  using namespace pinocchio;
920 
921  Model model;
923 
924  Data data(model), data_ref(model), data_plus(model);
925 
926  model.lowerPositionLimit.head<3>().fill(-1.);
927  model.upperPositionLimit.head<3>().fill(1.);
928  VectorXd q = randomConfiguration(model);
929 
930  const Model::JointIndex joint_id = model.existJointName("rarm2_joint")
931  ? model.getJointId("rarm2_joint")
932  : (Model::Index)(model.njoints - 1);
933 
936 
937  Data data2(model);
939  BOOST_CHECK(data2.J.isApprox(data.J));
940 
941  const Eigen::DenseIndex matrix_offset = 6 * model.nv;
942 
943  for (int k = 0; k < model.nv; ++k)
944  {
945  Eigen::Map<Data::Matrix6x> dJ(data.kinematic_hessians.data() + k * matrix_offset, 6, model.nv);
946  Eigen::Map<Data::Matrix6x> dJ2(
947  data2.kinematic_hessians.data() + k * matrix_offset, 6, model.nv);
948 
949  BOOST_CHECK(dJ2.isApprox(dJ));
950  }
951 
952  for (int i = 0; i < model.nv; ++i)
953  {
954  for (int j = i; j < model.nv; ++j)
955  {
956  bool j_is_children_of_i = false;
957  for (int parent = j; parent >= 0; parent = data.parents_fromRow[(size_t)parent])
958  {
959  if (parent == i)
960  {
961  j_is_children_of_i = true;
962  break;
963  }
964  }
965 
966  if (j_is_children_of_i)
967  {
968  if (i == j)
969  {
970  Eigen::Map<Data::Motion::Vector6> SixSi(
971  data.kinematic_hessians.data() + i * matrix_offset + i * 6);
972  BOOST_CHECK(SixSi.isZero());
973  }
974  else
975  {
976  Eigen::Map<Data::Motion::Vector6> SixSj(
977  data.kinematic_hessians.data() + i * matrix_offset + j * 6);
978 
979  Eigen::Map<Data::Motion::Vector6> SjxSi(
980  data.kinematic_hessians.data() + j * matrix_offset + i * 6);
981 
982  BOOST_CHECK(SixSj.isApprox(-SjxSi));
983  }
984  }
985  else
986  {
987  Eigen::Map<Data::Motion::Vector6> SixSj(
988  data.kinematic_hessians.data() + i * matrix_offset + j * 6);
989 
990  Eigen::Map<Data::Motion::Vector6> SjxSi(
991  data.kinematic_hessians.data() + j * matrix_offset + i * 6);
992 
993  BOOST_CHECK(SixSj.isZero());
994  BOOST_CHECK(SjxSi.isZero());
995  }
996  }
997  }
998 
999  const double eps = 1e-8;
1000  Data::Matrix6x J_ref(6, model.nv), J_plus(6, model.nv);
1001  J_ref.setZero();
1002  J_plus.setZero();
1003 
1004  computeJointJacobians(model, data_ref, q);
1005  VectorXd v_plus(VectorXd::Zero(model.nv));
1006 
1007  const Eigen::DenseIndex outer_offset = model.nv * 6;
1008 
1009  // WORLD
1010  getJointJacobian(model, data_ref, joint_id, WORLD, J_ref);
1011  Data::Tensor3x kinematic_hessian_world = getJointKinematicHessian(model, data, joint_id, WORLD);
1012  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
1013  {
1014  v_plus[k] = eps;
1015  const VectorXd q_plus = integrate(model, q, v_plus);
1016  computeJointJacobians(model, data_plus, q_plus);
1017  J_plus.setZero();
1018  getJointJacobian(model, data_plus, joint_id, WORLD, J_plus);
1019 
1020  Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps;
1021  Eigen::Map<Data::Matrix6x> dJ_dq(
1022  kinematic_hessian_world.data() + k * outer_offset, 6, model.nv);
1023 
1024  // std::cout << "k: " << k << std::endl;
1025  // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
1026  // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
1027  BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps)));
1028  v_plus[k] = 0.;
1029  }
1030 
1031  // LOCAL_WORLD_ALIGNED
1032  computeJointJacobians(model, data_ref, q);
1033  getJointJacobian(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, J_ref);
1034  Data::Tensor3x kinematic_hessian_local_world_aligned =
1036  Data::Matrix3x dt_last_fd(3, model.nv);
1037  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
1038  {
1039  v_plus[k] = eps;
1040  const VectorXd q_plus = integrate(model, q, v_plus);
1041  computeJointJacobians(model, data_plus, q_plus);
1042  J_plus.setZero();
1043  getJointJacobian(model, data_plus, joint_id, LOCAL_WORLD_ALIGNED, J_plus);
1044 
1045  SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id];
1046  tMt_plus.rotation().setIdentity();
1047 
1048  dt_last_fd.col(k) =
1049  (data_plus.oMi[joint_id].translation() - data_ref.oMi[joint_id].translation()) / eps;
1050 
1051  Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps;
1052  Eigen::Map<Data::Matrix6x> dJ_dq(
1053  kinematic_hessian_local_world_aligned.data() + k * outer_offset, 6, model.nv);
1054 
1055  // std::cout << "k: " << k << std::endl;
1056  // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
1057  // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
1058  BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps)));
1059  v_plus[k] = 0.;
1060  }
1061 
1062  Data::Matrix6x J_world(6, model.nv);
1063  J_world.setZero();
1064  getJointJacobian(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, J_world);
1065 
1066  BOOST_CHECK(dt_last_fd.isApprox(J_world.topRows<3>(), sqrt(eps)));
1067 
1068  // LOCAL
1069  computeJointJacobians(model, data_ref, q);
1070  getJointJacobian(model, data_ref, joint_id, LOCAL, J_ref);
1071  Data::Tensor3x kinematic_hessian_local = getJointKinematicHessian(model, data, joint_id, LOCAL);
1072  for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
1073  {
1074  v_plus[k] = eps;
1075  const VectorXd q_plus = integrate(model, q, v_plus);
1076  computeJointJacobians(model, data_plus, q_plus);
1077  J_plus.setZero();
1078  getJointJacobian(model, data_plus, joint_id, LOCAL, J_plus);
1079 
1080  // const SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id];
1081 
1082  Data::Matrix6x dJ_dq_ref = (J_plus - J_ref) / eps;
1083  Eigen::Map<Data::Matrix6x> dJ_dq(
1084  kinematic_hessian_local.data() + k * outer_offset, 6, model.nv);
1085 
1086  // std::cout << "k: " << k << std::endl;
1087  // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
1088  // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
1089  BOOST_CHECK((dJ_dq_ref - dJ_dq).isZero(sqrt(eps)));
1090  v_plus[k] = 0.;
1091  }
1092 }
1093 
1094 BOOST_AUTO_TEST_SUITE_END()
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
Eigen
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
pinocchio::getJointJacobianTimeVariation
void getJointJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &dJ)
Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (r...
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:149
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::isZero
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Definition: math/matrix.hpp:59
meshcat-viewer.v0
int v0
Definition: meshcat-viewer.py:88
lambdas.parent
parent
Definition: lambdas.py:21
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: multibody/data.hpp:363
pinocchio::SE3Tpl< context::Scalar, context::Options >
pinocchio::getPointClassicAccelerationDerivatives
void getPointClassicAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix3xOut1 > &v_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut2 > &a_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut3 > &a_point_partial_dv, const Eigen::MatrixBase< Matrix3xOut4 > &a_point_partial_da)
Computes the partial derivatives of the classic acceleration of a point given by its placement inform...
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
kinematics.hpp
pinocchio::DataTpl::kinematic_hessians
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
Definition: multibody/data.hpp:525
model.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::computeJointJacobiansTimeVariation
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobiansTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depen...
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::computeJointKinematicHessians
void computeJointKinematicHessians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes all the terms required to compute the second order derivatives of the placement information,...
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
pinocchio::getJointAccelerationDerivatives
void getJointAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, 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 derivaties of the spatial acceleration of a given with respect to the joint conf...
pinocchio::Index
PINOCCHIO_COMPILER_DIAGNOSTIC_POP typedef std::size_t Index
Definition: multibody/fwd.hpp:22
anymal-simulation.model
model
Definition: anymal-simulation.py:8
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_all)
Definition: unittest/kinematics-derivatives.cpp:20
pinocchio::SE3Tpl::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:52
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
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
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: multibody/data.hpp:94
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:70
pinocchio::getJointVelocityDerivatives
void getJointVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivaties of the spatial velocity of a given with respect to the joint configur...
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...
cartpole.v
v
Definition: cartpole.py:153
data.hpp
pinocchio::SE3Tpl::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:48
pinocchio::forceSet::se3Action
static void se3Action(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
SE3 action on a set of forces, represented by a 6xN matrix whose each column represent a spatial forc...
pinocchio::context::Vector3
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Definition: context/generic.hpp:53
q
q
pinocchio::Tensor::data
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar * data()
Definition: tensor.hpp:150
a
Vec3f a
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:360
fill
fill
kinematics-derivatives.hpp
dcrba.eps
int eps
Definition: dcrba.py:458
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
pinocchio::getJointKinematicHessian
void getJointKinematicHessian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const ReferenceFrame rf, Tensor< Scalar, 3, Options > &kinematic_hessian)
Retrieves the kinematic Hessian of a given joint according to the values aleardy computed by computeJ...
pinocchio::Tensor< Scalar, 3, Options >
pinocchio::getPointVelocityDerivatives
void getPointVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix3xOut1 > &v_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut2 > &v_point_partial_dv)
Computes the partial derivatives of the velocity of a point given by its placement information w....
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
cartpole.a0
a0
Definition: cartpole.py:155
simulation-closed-kinematic-chains.rhs
rhs
Definition: simulation-closed-kinematic-chains.py:138
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:11