rnea-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017-2020 CNRS INRIA
3 //
4 
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 #include "pinocchio/algorithm/jacobian.hpp"
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 #include "pinocchio/algorithm/kinematics.hpp"
10 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include "pinocchio/algorithm/rnea-derivatives.hpp"
13 #include "pinocchio/algorithm/crba.hpp"
14 #include "pinocchio/parsers/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_generalized_gravity_derivatives)
24 {
25  using namespace Eigen;
26  using namespace pinocchio;
27 
28  Model model;
29  buildModels::humanoidRandom(model);
30 
31  Data data(model), data_fd(model);
32 
33  model.lowerPositionLimit.head<3>().fill(-1.);
34  model.upperPositionLimit.head<3>().fill(1.);
35  VectorXd q = randomConfiguration(model);
36  VectorXd v(VectorXd::Zero(model.nv));
37  VectorXd a(VectorXd::Zero(model.nv));
38 
40  MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero();
41  computeGeneralizedGravityDerivatives(model,data,q,g_partial_dq);
42 
43  VectorXd g0 = computeGeneralizedGravity(model,data_fd,q);
44  BOOST_CHECK(data.g.isApprox(g0));
45 
46  MatrixXd g_partial_dq_fd(model.nv,model.nv); g_partial_dq_fd.setZero();
47 
48  VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
49  VectorXd q_plus(model.nq);
50  VectorXd g_plus(model.nv);
51  const double alpha = 1e-8;
52  for(int k = 0; k < model.nv; ++k)
53  {
54  v_eps[k] += alpha;
55  q_plus = integrate(model,q,v_eps);
56  g_plus = computeGeneralizedGravity(model,data_fd,q_plus);
57 
58  g_partial_dq_fd.col(k) = (g_plus - g0)/alpha;
59  v_eps[k] -= alpha;
60  }
61 
62  BOOST_CHECK(g_partial_dq.isApprox(g_partial_dq_fd,sqrt(alpha)));
63 }
64 
65 BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives_fext)
66 {
67  using namespace Eigen;
68  using namespace pinocchio;
69 
70  Model model;
71  buildModels::humanoidRandom(model);
72 
73  Data data(model), data_fd(model);
74 
75  model.lowerPositionLimit.head<3>().fill(-1.);
76  model.upperPositionLimit.head<3>().fill( 1.);
77  VectorXd q = randomConfiguration(model);
78 
79  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
80  ForceVector fext((size_t)model.njoints);
81  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
82  (*it).setRandom();
83 
84  // Check againt non-derivative algo
85  MatrixXd static_vec_partial_dq(model.nv,model.nv); static_vec_partial_dq.setZero();
86  computeStaticTorqueDerivatives(model,data,q,fext,static_vec_partial_dq);
87 
88  VectorXd tau0 = computeStaticTorque(model,data_fd,q,fext);
89  BOOST_CHECK(data.tau.isApprox(tau0));
90 
91  std::cout << "data.tau: " << data.tau.transpose() << std::endl;
92  std::cout << "tau0: " << tau0.transpose() << std::endl;
93 
94  MatrixXd static_vec_partial_dq_fd(model.nv,model.nv);
95 
96  VectorXd v_eps(Eigen::VectorXd::Zero(model.nv));
97  VectorXd q_plus(model.nq);
98  VectorXd tau_plus(model.nv);
99  const double alpha = 1e-8;
100  for(int k = 0; k < model.nv; ++k)
101  {
102  v_eps[k] += alpha;
103  q_plus = integrate(model,q,v_eps);
104  tau_plus = computeStaticTorque(model,data_fd,q_plus,fext);
105 
106  static_vec_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
107  v_eps[k] = 0.;
108  }
109 
110  BOOST_CHECK(static_vec_partial_dq.isApprox(static_vec_partial_dq_fd,sqrt(alpha)));
111 }
112 
113 BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
114 {
115  using namespace Eigen;
116  using namespace pinocchio;
117 
118  Model model;
119  buildModels::humanoidRandom(model);
120 
121  Data data(model), data_fd(model), data_ref(model);
122 
123  model.lowerPositionLimit.head<3>().fill(-1.);
124  model.upperPositionLimit.head<3>().fill(1.);
125  VectorXd q = randomConfiguration(model);
126  VectorXd v(VectorXd::Random(model.nv));
127  VectorXd a(VectorXd::Random(model.nv));
128 
130  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
131  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
132  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
133  computeRNEADerivatives(model,data,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv),rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
134  rnea(model,data_ref,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
135  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
136  {
137  BOOST_CHECK(data.of[k].isApprox(data.oMi[k].act(data_ref.f[k])));
138  }
139 
140  MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero();
141  computeGeneralizedGravityDerivatives(model,data_ref,q,g_partial_dq);
142 
143  BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq));
144  BOOST_CHECK(rnea_partial_dq.isApprox(g_partial_dq));
145  BOOST_CHECK(data.tau.isApprox(data_ref.g));
146 
147  VectorXd tau0 = rnea(model,data_fd,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
148  MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero();
149 
150  VectorXd v_eps(VectorXd::Zero(model.nv));
151  VectorXd q_plus(model.nq);
152  VectorXd tau_plus(model.nv);
153  const double alpha = 1e-8;
154  for(int k = 0; k < model.nv; ++k)
155  {
156  v_eps[k] += alpha;
157  q_plus = integrate(model,q,v_eps);
158  tau_plus = rnea(model,data_fd,q_plus,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
159 
160  rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
161  v_eps[k] -= alpha;
162  }
163  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
164 
165  // Check with q and a non zero
166  tau0 = rnea(model,data_fd,q,0*v,a);
167  rnea_partial_dq_fd.setZero();
168 
169  for(int k = 0; k < model.nv; ++k)
170  {
171  v_eps[k] += alpha;
172  q_plus = integrate(model,q,v_eps);
173  tau_plus = rnea(model,data_fd,q_plus,VectorXd::Zero(model.nv),a);
174 
175  rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
176  v_eps[k] -= alpha;
177  }
178 
179  rnea_partial_dq.setZero();
180  computeRNEADerivatives(model,data,q,VectorXd::Zero(model.nv),a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
181  forwardKinematics(model,data_ref,q,VectorXd::Zero(model.nv),a);
182 
183  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
184  {
185  BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
186  BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
187  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
188  BOOST_CHECK(data.oh[k].isApprox(Force::Zero()));
189  }
190 
191  BOOST_CHECK(data.tau.isApprox(tau0));
192  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
193 
194  // Check with q and v non zero
195  const Motion gravity(model.gravity);
196  model.gravity.setZero();
197  tau0 = rnea(model,data_fd,q,v,VectorXd::Zero(model.nv));
198  rnea_partial_dq_fd.setZero();
199 
200  for(int k = 0; k < model.nv; ++k)
201  {
202  v_eps[k] += alpha;
203  q_plus = integrate(model,q,v_eps);
204  tau_plus = rnea(model,data_fd,q_plus,v,VectorXd::Zero(model.nv));
205 
206  rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
207  v_eps[k] -= alpha;
208  }
209 
210  VectorXd v_plus(v);
211  MatrixXd rnea_partial_dv_fd(model.nv,model.nv); rnea_partial_dv_fd.setZero();
212 
213  for(int k = 0; k < model.nv; ++k)
214  {
215  v_plus[k] += alpha;
216  tau_plus = rnea(model,data_fd,q,v_plus,VectorXd::Zero(model.nv));
217 
218  rnea_partial_dv_fd.col(k) = (tau_plus - tau0)/alpha;
219  v_plus[k] -= alpha;
220  }
221 
222  rnea_partial_dq.setZero();
223  rnea_partial_dv.setZero();
224  computeRNEADerivatives(model,data,q,v,VectorXd::Zero(model.nv),rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
225  forwardKinematics(model,data_ref,q,v,VectorXd::Zero(model.nv));
226 
227  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
228  {
229  BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
230  BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
231  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
232  }
233 
234  BOOST_CHECK(data.tau.isApprox(tau0));
235  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
236  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha)));
237 
238 // std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.block<10,10>(0,0) << std::endl;
239 // std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.block<10,10>(0,0) << std::endl;
240 // std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.topRows<10>() << std::endl;
241 // std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.topRows<10>() << std::endl;
242  // Check with q, v and a non zero
243  model.gravity = gravity;
244  v_plus = v;
245  tau0 = rnea(model,data_fd,q,v,a);
246  rnea_partial_dq_fd.setZero();
247 
248  for(int k = 0; k < model.nv; ++k)
249  {
250  v_eps[k] += alpha;
251  q_plus = integrate(model,q,v_eps);
252  tau_plus = rnea(model,data_fd,q_plus,v,a);
253 
254  rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha;
255  v_eps[k] -= alpha;
256  }
257 
258  rnea_partial_dv_fd.setZero();
259  for(int k = 0; k < model.nv; ++k)
260  {
261  v_plus[k] += alpha;
262  tau_plus = rnea(model,data_fd,q,v_plus,a);
263 
264  rnea_partial_dv_fd.col(k) = (tau_plus - tau0)/alpha;
265  v_plus[k] -= alpha;
266  }
267 
268  rnea_partial_dq.setZero();
269  rnea_partial_dv.setZero();
270  computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
271  forwardKinematics(model,data_ref,q,v,a);
272 
273  for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k)
274  {
275  BOOST_CHECK(data.a[k].isApprox(data_ref.a[k]));
276  BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
277  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
278  }
279 
280  computeJointJacobiansTimeVariation(model,data_ref,q,v);
281  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
282  crba(model,data_ref,q);
283 
284  rnea_partial_da.triangularView<Eigen::StrictlyLower>()
285  = rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>();
286  data_ref.M.triangularView<Eigen::StrictlyLower>()
287  = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
288  BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M));
289 
290  BOOST_CHECK(data.tau.isApprox(tau0));
291  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha)));
292  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha)));
293 
294  Data data2(model);
295  computeRNEADerivatives(model,data2,q,v,a);
296  data2.M.triangularView<Eigen::StrictlyLower>()
297  = data2.M.transpose().triangularView<Eigen::StrictlyLower>();
298 
299  BOOST_CHECK(rnea_partial_dq.isApprox(data2.dtau_dq));
300  BOOST_CHECK(rnea_partial_dv.isApprox(data2.dtau_dv));
301  BOOST_CHECK(rnea_partial_da.isApprox(data2.M));
302 
303 }
304 
305 BOOST_AUTO_TEST_CASE(test_rnea_derivatives_fext)
306 {
307  using namespace Eigen;
308  using namespace pinocchio;
309 
310  Model model;
311  buildModels::humanoidRandom(model);
312  typedef Model::Force Force;
313 
314  Data data(model), data_fd(model), data_ref(model);
315 
316  model.lowerPositionLimit.head<3>().fill(-1.);
317  model.upperPositionLimit.head<3>().fill(1.);
318  VectorXd q = randomConfiguration(model);
319  VectorXd v(VectorXd::Random(model.nv));
320  VectorXd a(VectorXd::Random(model.nv));
321 
322  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
323  ForceVector fext((size_t)model.njoints);
324  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
325  (*it).setRandom();
326 
328  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
329  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
330  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
331 
332  computeRNEADerivatives(model,data,q,v,a,fext,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
333  rnea(model,data_ref,q,v,a,fext);
334 
335  BOOST_CHECK(data.tau.isApprox(data_ref.tau));
336 
337  computeRNEADerivatives(model,data_ref,q,v,a);
338  BOOST_CHECK(rnea_partial_dv.isApprox(data_ref.dtau_dv));
339  BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M));
340 
341  MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero();
342  MatrixXd rnea_partial_dv_fd(model.nv,model.nv); rnea_partial_dv_fd.setZero();
343  MatrixXd rnea_partial_da_fd(model.nv,model.nv); rnea_partial_da_fd.setZero();
344 
345  VectorXd v_eps(VectorXd::Zero(model.nv));
346  VectorXd q_plus(model.nq);
347  VectorXd tau_plus(model.nv);
348  const double eps = 1e-8;
349 
350  const VectorXd tau_ref = rnea(model,data_ref,q,v,a,fext);
351  for(int k = 0; k < model.nv; ++k)
352  {
353  v_eps[k] = eps;
354  q_plus = integrate(model,q,v_eps);
355  tau_plus = rnea(model,data_fd,q_plus,v,a,fext);
356 
357  rnea_partial_dq_fd.col(k) = (tau_plus - tau_ref) / eps;
358 
359  v_eps[k] = 0.;
360  }
361  BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(eps)));
362 
363  VectorXd v_plus(v);
364  for(int k = 0; k < model.nv; ++k)
365  {
366  v_plus[k] += eps;
367 
368  tau_plus = rnea(model,data_fd,q,v_plus,a,fext);
369 
370  rnea_partial_dv_fd.col(k) = (tau_plus - tau_ref) / eps;
371 
372  v_plus[k] -= eps;
373  }
374  BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(eps)));
375 
376  VectorXd a_plus(a);
377  for(int k = 0; k < model.nv; ++k)
378  {
379  a_plus[k] += eps;
380 
381  tau_plus = rnea(model,data_fd,q,v,a_plus,fext);
382 
383  rnea_partial_da_fd.col(k) = (tau_plus - tau_ref) / eps;
384 
385  a_plus[k] -= eps;
386  }
387 
388  rnea_partial_da.triangularView<Eigen::Lower>() = rnea_partial_da.transpose().triangularView<Eigen::Lower>();
389  BOOST_CHECK(rnea_partial_da.isApprox(rnea_partial_da_fd,sqrt(eps)));
390 
391  // test the shortcut
392  Data data_shortcut(model);
393  computeRNEADerivatives(model,data_shortcut,q,v,a,fext);
394  BOOST_CHECK(data_shortcut.dtau_dq.isApprox(rnea_partial_dq));
395  BOOST_CHECK(data_shortcut.dtau_dv.isApprox(rnea_partial_dv));
396  data_shortcut.M.triangularView<Eigen::Lower>() = data_shortcut.M.transpose().triangularView<Eigen::Lower>();
397  BOOST_CHECK(data_shortcut.M.isApprox(rnea_partial_da));
398 }
399 
400 BOOST_AUTO_TEST_CASE(test_rnea_derivatives_vs_kinematics_derivatives)
401 {
402  using namespace Eigen;
403  using namespace pinocchio;
404 
405  Model model;
406  buildModels::humanoidRandom(model);
407 
408  Data data(model), data_ref(model);
409 
410  model.lowerPositionLimit.head<3>().fill(-1.);
411  model.upperPositionLimit.head<3>().fill(1.);
412  VectorXd q = randomConfiguration(model);
413  VectorXd v(VectorXd::Random(model.nv));
414  VectorXd a(VectorXd::Random(model.nv));
415 
417  MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero();
418  MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero();
419  MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero();
420 
421  computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da);
422  computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
423 
424  BOOST_CHECK(data.J.isApprox(data_ref.J));
425  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
426 
427  for(size_t k = 1; k < (size_t)model.njoints; ++k)
428  {
429  BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
430  BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k]));
431  BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k]));
432  }
433 }
434 
435 BOOST_AUTO_TEST_CASE(test_multiple_calls)
436 {
437  using namespace Eigen;
438  using namespace pinocchio;
439 
440  Model model;
441  buildModels::humanoidRandom(model);
442 
443  Data data1(model), data2(model);
444 
445  model.lowerPositionLimit.head<3>().fill(-1.);
446  model.upperPositionLimit.head<3>().fill(1.);
447  VectorXd q = randomConfiguration(model);
448  VectorXd v(VectorXd::Random(model.nv));
449  VectorXd a(VectorXd::Random(model.nv));
450 
451  computeRNEADerivatives(model,data1,q,v,a);
452  data2 = data1;
453 
454  for(int k = 0; k < 20; ++k)
455  {
456  computeRNEADerivatives(model,data1,q,v,a);
457  }
458 
459  BOOST_CHECK(data1.J.isApprox(data2.J));
460  BOOST_CHECK(data1.dJ.isApprox(data2.dJ));
461  BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq));
462  BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq));
463  BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv));
464 
465  BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq));
466  BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv));
467  BOOST_CHECK(data1.dFda.isApprox(data2.dFda));
468 
469  BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq));
470  BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv));
471  BOOST_CHECK(data1.M.isApprox(data2.M));
472 }
473 
474 BOOST_AUTO_TEST_CASE(test_get_coriolis)
475 {
476  using namespace Eigen;
477  using namespace pinocchio;
478 
479  Model model;
480  buildModels::humanoidRandom(model);
481 
482  model.lowerPositionLimit.head<3>().fill(-1.);
483  model.upperPositionLimit.head<3>().fill( 1.);
484 
485  Data data_ref(model);
486  Data data(model);
487 
488  VectorXd q = randomConfiguration(model);
489  VectorXd v = VectorXd::Random(model.nv);
490  VectorXd tau = VectorXd::Random(model.nv);
491 
492  computeCoriolisMatrix(model,data_ref,q,v);
493 
494  computeRNEADerivatives(model,data,q,v,tau);
495  getCoriolisMatrix(model,data);
496 
497  BOOST_CHECK(data.J.isApprox(data_ref.J));
498  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
499  for(JointIndex k = 1; k < model.joints.size(); ++k)
500  {
501  BOOST_CHECK(data.B[k].isApprox(data_ref.B[k]));
502  BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
503  }
504 
505  BOOST_CHECK(data.C.isApprox(data_ref.C));
506 }
507 
508 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: src/multibody/data.hpp:286
pinocchio::getCoriolisMatrix
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & getCoriolisMatrix(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Retrives the Coriolis Matrix of the Lagrangian dynamics: after a call to the dynamics derivativ...
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: src/multibody/data.hpp:130
pinocchio::ModelTpl::joints
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
Definition: src/multibody/model.hpp:97
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::DataTpl::g
VectorXs g
Vector of generalized gravity (dim model.nv).
Definition: src/multibody/data.hpp:140
q
q
pinocchio::Force
ForceTpl< double, 0 > Force
Definition: src/spatial/fwd.hpp:58
pinocchio::computeStaticTorque
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & computeStaticTorque(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const container::aligned_vector< ForceTpl< Scalar, Options > > &fext)
Computes the generalized static torque contribution of the Lagrangian dynamics: ...
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: src/multibody/data.hpp:283
pinocchio::DataTpl::dtau_dv
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity. ...
Definition: src/multibody/data.hpp:312
pinocchio::DataTpl::dAdv
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Definition: src/multibody/data.hpp:306
pinocchio::rnea
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Definition: parallel/rnea.hpp:32
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:54
dcrba.gravity
gravity
Definition: dcrba.py:409
dcrba.eps
int eps
Definition: dcrba.py:384
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives)
Definition: rnea-derivatives.cpp:23
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: src/multibody/model.hpp:82
pinocchio::DataTpl::dFda
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
Definition: src/multibody/data.hpp:171
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: src/multibody/model.hpp:139
pinocchio::python::computeStaticTorqueDerivatives
Data::MatrixXs computeStaticTorqueDerivatives(const Model &model, Data &data, const Eigen::VectorXd &q, const ForceAlignedVector &fext)
Definition: expose-rnea-derivatives.cpp:26
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:224
pinocchio::DataTpl::dtau_dq
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: src/multibody/data.hpp:309
fill
fill
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::DataTpl::dFdq
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Definition: src/multibody/data.hpp:165
pinocchio::python::computeGeneralizedGravityDerivatives
Data::MatrixXs computeGeneralizedGravityDerivatives(const Model &model, Data &data, const Eigen::VectorXd &q)
Definition: expose-rnea-derivatives.cpp:17
pinocchio::DataTpl::dAdq
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Definition: src/multibody/data.hpp:303
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: src/multibody/data.hpp:153
pinocchio::python::computeRNEADerivatives
bp::tuple computeRNEADerivatives(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
Definition: expose-rnea-derivatives.cpp:36
pinocchio::DataTpl::C
MatrixXs C
The Coriolis matrix (a square matrix of dim model.nv).
Definition: src/multibody/data.hpp:159
PINOCCHIO_ALIGNED_STD_VECTOR
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
Definition: container/aligned-vector.hpp:11
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: src/multibody/model.hpp:49
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...
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::ModelTpl::gravity
Motion gravity
Spatial gravity of the model.
Definition: src/multibody/model.hpp:158
a
Vec3f a
pinocchio::computeGeneralizedGravity
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & computeGeneralizedGravity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the generalized gravity contribution of the Lagrangian dynamics:
pinocchio::DataTpl::dFdv
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Definition: src/multibody/data.hpp:168
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::JointIndex
Index JointIndex
Definition: src/multibody/fwd.hpp:28
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
pinocchio::computeCoriolisMatrix
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & computeCoriolisMatrix(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Coriolis Matrix of the Lagrangian dynamics:
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
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::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
pinocchio::DataTpl::dVdq
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: src/multibody/data.hpp:300

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32