unittest/aba-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2020 CNRS INRIA
3 //
4 
5 #include "pinocchio/algorithm/jacobian.hpp"
6 #include "pinocchio/algorithm/joint-configuration.hpp"
7 #include "pinocchio/algorithm/kinematics.hpp"
8 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
9 #include "pinocchio/algorithm/rnea.hpp"
10 #include "pinocchio/algorithm/rnea-derivatives.hpp"
11 #include "pinocchio/algorithm/aba.hpp"
12 #include "pinocchio/algorithm/aba-derivatives.hpp"
13 #include "pinocchio/algorithm/crba.hpp"
14 #include "pinocchio/multibody/sample-models.hpp"
15 
16 #include <iostream>
17 
18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
20 
21 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
22 
23 BOOST_AUTO_TEST_CASE(test_aba_derivatives)
24 {
25  using namespace Eigen;
26  using namespace pinocchio;
27 
28  Model model;
29  buildModels::humanoidRandom(model);
30 
31  Data data(model), data_ref(model);
32  model.lowerPositionLimit.head<3>().fill(-1.);
33  model.upperPositionLimit.head<3>().fill(1.);
34  VectorXd q = randomConfiguration(model);
35  VectorXd v(VectorXd::Random(model.nv));
36  VectorXd tau(VectorXd::Random(model.nv));
37  VectorXd a(aba(model, data_ref, q, v, tau, Convention::LOCAL));
38 
39  VectorXd tau_from_a(rnea(model, data_ref, q, v, a));
40  BOOST_CHECK(tau_from_a.isApprox(tau));
41 
42  MatrixXd aba_partial_dq(model.nv, model.nv);
43  aba_partial_dq.setZero();
44  MatrixXd aba_partial_dv(model.nv, model.nv);
45  aba_partial_dv.setZero();
46  Data::RowMatrixXs aba_partial_dtau(model.nv, model.nv);
47  aba_partial_dtau.setZero();
48 
49  const double prec = Eigen::NumTraits<double>::dummy_precision();
50 
51  computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
52  computeRNEADerivatives(model, data_ref, q, v, a);
53  BOOST_CHECK(data.J.isApprox(data_ref.J));
54  for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
55  {
56  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
57  BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
58  BOOST_CHECK(data.oh[k].isApprox(data_ref.oh[k]));
59  BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa[k] - model.gravity));
60  BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k]));
61  BOOST_CHECK(data.of[k].isApprox(data_ref.of[k], 1e1 * prec));
62  BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
63  BOOST_CHECK(data.doYcrb[k].isApprox(data_ref.doYcrb[k]));
64  }
65 
66  aba(model, data_ref, q, v, tau, Convention::LOCAL);
67  for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
68  {
69  BOOST_CHECK(data.oYaba[k].isApprox(
70  data_ref.oMi[k].toDualActionMatrix() * data_ref.Yaba[k]
71  * data_ref.oMi[k].inverse().toActionMatrix()));
72  // BOOST_CHECK(data.of[k].isApprox(data.oMi[k].act(data_ref.f[k])));
73  }
74  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
75 
76  aba(model, data_ref, q, v, tau, Convention::WORLD);
77  BOOST_CHECK(data.J.isApprox(data_ref.J));
78  BOOST_CHECK(data.u.isApprox(data_ref.u));
79  for (Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
80  {
81  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
82  BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
83  BOOST_CHECK(data.oYaba[k].isApprox(data_ref.oYaba[k]));
84  // BOOST_CHECK(data.of[k].isApprox(data_ref.of[k]));
85  BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k]));
86  BOOST_CHECK(data.joints[k].U().isApprox(data_ref.joints[k].U()));
87  BOOST_CHECK(data.joints[k].StU().isApprox(data_ref.joints[k].StU()));
88  BOOST_CHECK(data.joints[k].Dinv().isApprox(data_ref.joints[k].Dinv()));
89  BOOST_CHECK(data.joints[k].UDinv().isApprox(data_ref.joints[k].UDinv()));
90  }
91  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
92 
93  computeJointJacobians(model, data_ref, q);
94  BOOST_CHECK(data.J.isApprox(data_ref.J));
95 
96  computeMinverse(model, data_ref, q);
97  data_ref.Minv.triangularView<Eigen::StrictlyLower>() =
98  data_ref.Minv.transpose().triangularView<Eigen::StrictlyLower>();
99 
100  BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv));
101 
102  BOOST_CHECK(data.J.isApprox(data_ref.J));
103  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
104  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
105  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
106  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
107  BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq));
108  BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv));
109 
110  MatrixXd aba_partial_dq_fd(model.nv, model.nv);
111  aba_partial_dq_fd.setZero();
112  MatrixXd aba_partial_dv_fd(model.nv, model.nv);
113  aba_partial_dv_fd.setZero();
114  MatrixXd aba_partial_dtau_fd(model.nv, model.nv);
115  aba_partial_dtau_fd.setZero();
116 
117  Data data_fd(model);
118  VectorXd a0 = aba(model, data_fd, q, v, tau, Convention::LOCAL);
119  VectorXd v_eps(VectorXd::Zero(model.nv));
120  VectorXd q_plus(model.nq);
121  VectorXd a_plus(model.nv);
122  const double alpha = 1e-8;
123  for (int k = 0; k < model.nv; ++k)
124  {
125  v_eps[k] += alpha;
126  q_plus = integrate(model, q, v_eps);
127  a_plus = aba(model, data_fd, q_plus, v, tau, Convention::LOCAL);
128 
129  aba_partial_dq_fd.col(k) = (a_plus - a0) / alpha;
130  v_eps[k] -= alpha;
131  }
132  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd, sqrt(alpha)));
133 
134  VectorXd v_plus(v);
135  for (int k = 0; k < model.nv; ++k)
136  {
137  v_plus[k] += alpha;
138  a_plus = aba(model, data_fd, q, v_plus, tau, Convention::LOCAL);
139 
140  aba_partial_dv_fd.col(k) = (a_plus - a0) / alpha;
141  v_plus[k] -= alpha;
142  }
143  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd, sqrt(alpha)));
144 
145  VectorXd tau_plus(tau);
146  for (int k = 0; k < model.nv; ++k)
147  {
148  tau_plus[k] += alpha;
149  a_plus = aba(model, data_fd, q, v, tau_plus, Convention::LOCAL);
150 
151  aba_partial_dtau_fd.col(k) = (a_plus - a0) / alpha;
152  tau_plus[k] -= alpha;
153  }
154  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd, sqrt(alpha)));
155 }
156 
157 BOOST_AUTO_TEST_CASE(test_aba_minimal_argument)
158 {
159  using namespace Eigen;
160  using namespace pinocchio;
161 
162  Model model;
163  buildModels::humanoidRandom(model);
164 
165  Data data(model), data_ref(model);
166 
167  model.lowerPositionLimit.head<3>().fill(-1.);
168  model.upperPositionLimit.head<3>().fill(1.);
169  VectorXd q = randomConfiguration(model);
170  VectorXd v(VectorXd::Random(model.nv));
171  VectorXd tau(VectorXd::Random(model.nv));
172  VectorXd a(aba(model, data_ref, q, v, tau, Convention::LOCAL));
173 
174  MatrixXd aba_partial_dq(model.nv, model.nv);
175  aba_partial_dq.setZero();
176  MatrixXd aba_partial_dv(model.nv, model.nv);
177  aba_partial_dv.setZero();
178  Data::RowMatrixXs aba_partial_dtau(model.nv, model.nv);
179  aba_partial_dtau.setZero();
180 
181  computeABADerivatives(
182  model, data_ref, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
183 
184  computeABADerivatives(model, data, q, v, tau);
185 
186  BOOST_CHECK(data.J.isApprox(data_ref.J));
187  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
188  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
189  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
190  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
191  BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq));
192  BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv));
193  BOOST_CHECK(data.Minv.isApprox(aba_partial_dtau));
194  BOOST_CHECK(data.ddq_dq.isApprox(aba_partial_dq));
195  BOOST_CHECK(data.ddq_dv.isApprox(aba_partial_dv));
196 }
197 
198 BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext)
199 {
200  using namespace Eigen;
201  using namespace pinocchio;
202 
203  Model model;
204  buildModels::humanoidRandom(model);
205 
206  Data data(model), data_ref(model);
207 
208  model.lowerPositionLimit.head<3>().fill(-1.);
209  model.upperPositionLimit.head<3>().fill(1.);
210  VectorXd q = randomConfiguration(model);
211  VectorXd v(VectorXd::Random(model.nv));
212  VectorXd tau(VectorXd::Random(model.nv));
213  VectorXd a(aba(model, data_ref, q, v, tau, Convention::LOCAL));
214 
215  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
216  ForceVector fext((size_t)model.njoints);
217  for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
218  (*it).setRandom();
219 
220  MatrixXd aba_partial_dq(model.nv, model.nv);
221  aba_partial_dq.setZero();
222  MatrixXd aba_partial_dv(model.nv, model.nv);
223  aba_partial_dv.setZero();
224  Data::RowMatrixXs aba_partial_dtau(model.nv, model.nv);
225  aba_partial_dtau.setZero();
226 
227  computeABADerivatives(
228  model, data, q, v, tau, fext, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
229 
230  aba(model, data_ref, q, v, tau, fext, Convention::LOCAL);
231  // updateGlobalPlacements(model, data_ref);
232  // for(size_t k =1; k < (size_t)model.njoints; ++k)
233  // {
234  // BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
235  // BOOST_CHECK(daita.of[k].isApprox(data_ref.oMi[k].act(data.f[k])));
236  //
237  // }
238  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
239 
240  computeABADerivatives(model, data_ref, q, v, tau);
241  BOOST_CHECK(aba_partial_dv.isApprox(data_ref.ddq_dv));
242  BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv));
243 
244  MatrixXd aba_partial_dq_fd(model.nv, model.nv);
245  aba_partial_dq_fd.setZero();
246  MatrixXd aba_partial_dv_fd(model.nv, model.nv);
247  aba_partial_dv_fd.setZero();
248  MatrixXd aba_partial_dtau_fd(model.nv, model.nv);
249  aba_partial_dtau_fd.setZero();
250 
251  Data data_fd(model);
252  const VectorXd a0 = aba(model, data_fd, q, v, tau, fext, Convention::LOCAL);
253  VectorXd v_eps(VectorXd::Zero(model.nv));
254  VectorXd q_plus(model.nq);
255  VectorXd a_plus(model.nv);
256  const double alpha = 1e-8;
257  for (int k = 0; k < model.nv; ++k)
258  {
259  v_eps[k] += alpha;
260  q_plus = integrate(model, q, v_eps);
261  a_plus = aba(model, data_fd, q_plus, v, tau, fext, Convention::LOCAL);
262 
263  aba_partial_dq_fd.col(k) = (a_plus - a0) / alpha;
264  v_eps[k] -= alpha;
265  }
266  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd, sqrt(alpha)));
267 
268  VectorXd v_plus(v);
269  for (int k = 0; k < model.nv; ++k)
270  {
271  v_plus[k] += alpha;
272  a_plus = aba(model, data_fd, q, v_plus, tau, fext, Convention::LOCAL);
273 
274  aba_partial_dv_fd.col(k) = (a_plus - a0) / alpha;
275  v_plus[k] -= alpha;
276  }
277  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd, sqrt(alpha)));
278 
279  VectorXd tau_plus(tau);
280  for (int k = 0; k < model.nv; ++k)
281  {
282  tau_plus[k] += alpha;
283  a_plus = aba(model, data_fd, q, v, tau_plus, fext, Convention::LOCAL);
284 
285  aba_partial_dtau_fd.col(k) = (a_plus - a0) / alpha;
286  tau_plus[k] -= alpha;
287  }
288  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd, sqrt(alpha)));
289 
290  // test the shortcut
291  Data data_shortcut(model);
292  computeABADerivatives(model, data_shortcut, q, v, tau, fext);
293  BOOST_CHECK(data_shortcut.ddq_dq.isApprox(aba_partial_dq));
294  BOOST_CHECK(data_shortcut.ddq_dv.isApprox(aba_partial_dv));
295  BOOST_CHECK(data_shortcut.Minv.isApprox(aba_partial_dtau));
296 }
297 
298 BOOST_AUTO_TEST_CASE(test_multiple_calls)
299 {
300  using namespace Eigen;
301  using namespace pinocchio;
302 
303  Model model;
304  buildModels::humanoidRandom(model);
305 
306  Data data1(model), data2(model);
307 
308  model.lowerPositionLimit.head<3>().fill(-1.);
309  model.upperPositionLimit.head<3>().fill(1.);
310  VectorXd q = randomConfiguration(model);
311  VectorXd v(VectorXd::Random(model.nv));
312  VectorXd tau(VectorXd::Random(model.nv));
313 
314  computeABADerivatives(model, data1, q, v, tau);
315  data2 = data1;
316 
317  for (int k = 0; k < 20; ++k)
318  {
319  computeABADerivatives(model, data1, q, v, tau);
320  }
321 
322  BOOST_CHECK(data1.J.isApprox(data2.J));
323  BOOST_CHECK(data1.dJ.isApprox(data2.dJ));
324  BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq));
325  BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq));
326  BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv));
327 
328  BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
329  BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv));
330 
331  BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq));
332  BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv));
333 
334  BOOST_CHECK(data1.ddq_dq.isApprox(data2.ddq_dq));
335  BOOST_CHECK(data1.ddq_dv.isApprox(data2.ddq_dv));
336  BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
337 }
338 
339 BOOST_AUTO_TEST_CASE(test_aba_derivatives_vs_kinematics_derivatives)
340 {
341  using namespace Eigen;
342  using namespace pinocchio;
343 
344  Model model;
345  buildModels::humanoidRandom(model);
346 
347  Data data(model), data_ref(model);
348 
349  model.lowerPositionLimit.head<3>().fill(-1.);
350  model.upperPositionLimit.head<3>().fill(1.);
351  VectorXd q = randomConfiguration(model);
352  VectorXd v(VectorXd::Random(model.nv));
353  VectorXd a(VectorXd::Random(model.nv));
354 
355  VectorXd tau = rnea(model, data_ref, q, v, a);
356 
357  MatrixXd aba_partial_dq(model.nv, model.nv);
358  aba_partial_dq.setZero();
359  MatrixXd aba_partial_dv(model.nv, model.nv);
360  aba_partial_dv.setZero();
361  MatrixXd aba_partial_dtau(model.nv, model.nv);
362  aba_partial_dtau.setZero();
363 
364  computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
365  computeForwardKinematicsDerivatives(model, data_ref, q, v, a);
366 
367  BOOST_CHECK(data.J.isApprox(data_ref.J));
368  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
369 
370  for (size_t k = 1; k < (size_t)model.njoints; ++k)
371  {
372  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
373  BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
374  BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k]));
375  }
376 }
377 
378 BOOST_AUTO_TEST_CASE(test_optimized_aba_derivatives)
379 {
380  using namespace Eigen;
381  using namespace pinocchio;
382 
383  Model model;
384  buildModels::humanoidRandom(model);
385 
386  Data data(model), data_ref(model);
387 
388  model.lowerPositionLimit.head<3>().fill(-1.);
389  model.upperPositionLimit.head<3>().fill(1.);
390  VectorXd q = randomConfiguration(model);
391  VectorXd v(VectorXd::Random(model.nv));
392  VectorXd tau(VectorXd::Random(model.nv));
393 
394  MatrixXd aba_partial_dq(model.nv, model.nv);
395  aba_partial_dq.setZero();
396  MatrixXd aba_partial_dv(model.nv, model.nv);
397  aba_partial_dv.setZero();
398  MatrixXd aba_partial_dtau(model.nv, model.nv);
399  aba_partial_dtau.setZero();
400 
401  aba(model, data, q, v, tau, Convention::WORLD);
402  computeABADerivatives(model, data, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
403 
404  MatrixXd aba_partial_dq_ref(model.nv, model.nv);
405  aba_partial_dq_ref.setZero();
406  MatrixXd aba_partial_dv_ref(model.nv, model.nv);
407  aba_partial_dv_ref.setZero();
408  MatrixXd aba_partial_dtau_ref(model.nv, model.nv);
409  aba_partial_dtau_ref.setZero();
410 
411  computeABADerivatives(
412  model, data_ref, q, v, tau, aba_partial_dq_ref, aba_partial_dv_ref, aba_partial_dtau_ref);
413 
414  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
415  BOOST_CHECK(data.Minv.isApprox(data_ref.Minv));
416 
417  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref));
418  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref));
419  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref));
420 
421  // Test multiple calls
422  const int num_calls = 20;
423  aba(model, data, q, v, tau, Convention::WORLD);
424  for (int it = 0; it < num_calls; ++it)
425  {
426  computeABADerivatives(model, data, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
427 
428  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
429  BOOST_CHECK(data.Minv.isApprox(data_ref.Minv));
430 
431  for (size_t joint_id = 1; joint_id < model.joints.size(); ++joint_id)
432  {
433  BOOST_CHECK(data.oMi[joint_id].isApprox(data_ref.oMi[joint_id]));
434  BOOST_CHECK(data.ov[joint_id].isApprox(data_ref.ov[joint_id]));
435  BOOST_CHECK(data.oa[joint_id].isApprox(data_ref.oa[joint_id]));
436  BOOST_CHECK(data.oa_gf[joint_id].isApprox(data_ref.oa_gf[joint_id]));
437  BOOST_CHECK(data.of[joint_id].isApprox(data_ref.of[joint_id]));
438  BOOST_CHECK(data.oh[joint_id].isApprox(data_ref.oh[joint_id]));
439  BOOST_CHECK(data.oYaba[joint_id].isApprox(data_ref.oYaba[joint_id]));
440  BOOST_CHECK(data.oYcrb[joint_id].isApprox(data_ref.oYcrb[joint_id]));
441  }
442 
443  BOOST_CHECK(data.J.isApprox(data_ref.J));
444  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
445  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
446  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
447  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
448  BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq));
449  BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv));
450  BOOST_CHECK(data.dFda.isApprox(data_ref.dFda));
451 
452  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref));
453  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref));
454  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref));
455  }
456 }
457 
458 BOOST_AUTO_TEST_CASE(test_optimized_aba_derivatives_fext)
459 {
460  using namespace Eigen;
461  using namespace pinocchio;
462 
463  Model model;
464  buildModels::humanoidRandom(model);
465 
466  Data data(model), data_ref(model);
467 
468  model.lowerPositionLimit.head<3>().fill(-1.);
469  model.upperPositionLimit.head<3>().fill(1.);
470  VectorXd q = randomConfiguration(model);
471  VectorXd v(VectorXd::Random(model.nv));
472  VectorXd tau(VectorXd::Random(model.nv));
473 
474  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
475  ForceVector fext((size_t)model.njoints);
476  for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
477  (*it).setRandom();
478 
479  MatrixXd aba_partial_dq(model.nv, model.nv);
480  aba_partial_dq.setZero();
481  MatrixXd aba_partial_dv(model.nv, model.nv);
482  aba_partial_dv.setZero();
483  MatrixXd aba_partial_dtau(model.nv, model.nv);
484  aba_partial_dtau.setZero();
485 
486  aba(model, data, q, v, tau, fext, Convention::WORLD);
487  computeABADerivatives(model, data, fext, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
488 
489  MatrixXd aba_partial_dq_ref(model.nv, model.nv);
490  aba_partial_dq_ref.setZero();
491  MatrixXd aba_partial_dv_ref(model.nv, model.nv);
492  aba_partial_dv_ref.setZero();
493  MatrixXd aba_partial_dtau_ref(model.nv, model.nv);
494  aba_partial_dtau_ref.setZero();
495 
496  computeABADerivatives(
497  model, data_ref, q, v, tau, fext, aba_partial_dq_ref, aba_partial_dv_ref, aba_partial_dtau_ref);
498 
499  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
500  BOOST_CHECK(data.Minv.isApprox(data_ref.Minv));
501 
502  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref));
503  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref));
504  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref));
505 
506  // Test multiple calls
507  const int num_calls = 20;
508  aba(model, data, q, v, tau, fext, Convention::WORLD);
509  for (int it = 0; it < num_calls; ++it)
510  {
511  computeABADerivatives(model, data, fext, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
512 
513  BOOST_CHECK(data.ddq.isApprox(data_ref.ddq));
514  BOOST_CHECK(data.Minv.isApprox(data_ref.Minv));
515 
516  for (size_t joint_id = 1; joint_id < model.joints.size(); ++joint_id)
517  {
518  BOOST_CHECK(data.oMi[joint_id].isApprox(data_ref.oMi[joint_id]));
519  BOOST_CHECK(data.ov[joint_id].isApprox(data_ref.ov[joint_id]));
520  BOOST_CHECK(data.oa[joint_id].isApprox(data_ref.oa[joint_id]));
521  BOOST_CHECK(data.oa_gf[joint_id].isApprox(data_ref.oa_gf[joint_id]));
522  BOOST_CHECK(data.of[joint_id].isApprox(data_ref.of[joint_id]));
523  BOOST_CHECK(data.oh[joint_id].isApprox(data_ref.oh[joint_id]));
524  BOOST_CHECK(data.oYaba[joint_id].isApprox(data_ref.oYaba[joint_id]));
525  BOOST_CHECK(data.oYcrb[joint_id].isApprox(data_ref.oYcrb[joint_id]));
526  }
527 
528  BOOST_CHECK(data.J.isApprox(data_ref.J));
529  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
530  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
531  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
532  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
533  BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq));
534  BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv));
535  BOOST_CHECK(data.dFda.isApprox(data_ref.dFda));
536 
537  BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref));
538  BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref));
539  BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref));
540  }
541 }
542 
543 BOOST_AUTO_TEST_SUITE_END()
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
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
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
Eigen
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::DataTpl::RowMatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor|Options > RowMatrixXs
Definition: multibody/data.hpp:99
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: multibody/data.hpp:363
pinocchio::DataTpl::dFdv
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Definition: multibody/data.hpp:214
kinematics.hpp
rnea-derivatives.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
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::python::computeRNEADerivatives
bp::tuple computeRNEADerivatives(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &a)
Definition: expose-rnea-derivatives.cpp:38
pinocchio::DataTpl::u
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
Definition: multibody/data.hpp:279
pinocchio::aba
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(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 > &tau, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
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::DataTpl::dFda
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
Definition: multibody/data.hpp:217
pinocchio::DataTpl::joints
PINOCCHIO_COMPILER_DIAGNOSTIC_POP JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model,...
Definition: multibody/data.hpp:115
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
rnea.hpp
aba-derivatives.hpp
pinocchio::computeMinverse
const DataTpl< Scalar, Options, JointCollectionTpl >::RowMatrixXs & computeMinverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the inverse of the joint space inertia matrix using Articulated Body formulation.
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
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::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:70
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_aba_derivatives)
Definition: unittest/aba-derivatives.cpp:23
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
cartpole.v
v
Definition: cartpole.py:153
pinocchio::ForceTpl
Definition: context/casadi.hpp:25
q
q
inverse-kinematics-3d.it
int it
Definition: inverse-kinematics-3d.py:17
a
Vec3f a
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:360
fill
fill
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::rnea
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(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)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
kinematics-derivatives.hpp
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
PINOCCHIO_ALIGNED_STD_VECTOR
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
Definition: container/aligned-vector.hpp:11
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
sample-models.hpp
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
crba.hpp
pinocchio::DataTpl::dVdq
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: multibody/data.hpp:377
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::DataTpl::dAdq
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Definition: multibody/data.hpp:380
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

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