casadi-algo-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #include "pinocchio/autodiff/casadi.hpp"
6 
7 #include "pinocchio/algorithm/rnea.hpp"
8 #include "pinocchio/algorithm/rnea-derivatives.hpp"
9 #include "pinocchio/algorithm/aba.hpp"
10 #include "pinocchio/algorithm/aba-derivatives.hpp"
11 #include "pinocchio/algorithm/joint-configuration.hpp"
12 
13 #include "pinocchio/parsers/sample-models.hpp"
14 
15 #include <casadi/casadi.hpp>
16 
17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
19 
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21 
22 BOOST_AUTO_TEST_CASE(test_integrate)
23 {
24  typedef double Scalar;
25  typedef casadi::SX ADScalar;
26 
27  typedef pinocchio::ModelTpl<Scalar> Model;
28  typedef Model::Data Data;
29 
30  typedef pinocchio::ModelTpl<ADScalar> ADModel;
31  typedef ADModel::Data ADData;
32 
33  Model model;
34  pinocchio::buildModels::humanoidRandom(model);
35  model.lowerPositionLimit.head<3>().fill(-1.);
36  model.upperPositionLimit.head<3>().fill(1.);
37  Data data(model);
38 
39  typedef Model::ConfigVectorType ConfigVector;
40  typedef Model::TangentVectorType TangentVector;
41  ConfigVector q(model.nq);
42  q = pinocchio::randomConfiguration(model);
43  TangentVector v(TangentVector::Random(model.nv));
44  TangentVector a(TangentVector::Random(model.nv));
45 
46  typedef ADModel::ConfigVectorType ConfigVectorAD;
47  ADModel ad_model = model.cast<ADScalar>();
48  ADData ad_data(ad_model);
49 
50  pinocchio::rnea(model,data,q,v,a);
51 
52  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
53  casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);
54 
55  ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
56  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
57  v_int_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_v_int).data(),model.nv,1);
58 
59  pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad);
60  casadi::SX cs_q_int(model.nq,1);
61  pinocchio::casadi::copy(q_int_ad,cs_q_int);
62 
63  std::cout << "cs_q_int:" << cs_q_int << std::endl;
64  casadi::Function eval_integrate("eval_integrate",
65  casadi::SXVector {cs_q,cs_v_int},
66  casadi::SXVector {cs_q_int});
67  std::vector<double> q_vec((size_t)model.nq);
68  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;
69 
70  std::vector<double> v_int_vec((size_t)model.nv);
71  Eigen::Map<TangentVector>(v_int_vec.data(),model.nv,1).setZero();
72  casadi::DM q_int_res = eval_integrate(casadi::DMVector {q_vec,v_int_vec})[0];
73 
74  Data::ConfigVectorType q_int_vec = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(q_int_res).data(),model.nq,1);
75 
76  ConfigVector q_plus(model.nq);
77  pinocchio::integrate(model,q,TangentVector::Zero(model.nv),q_plus);
78 
79  std::cout << "q_int_vec: " << q_int_vec.transpose() << std::endl;
80  BOOST_CHECK(q_plus.isApprox(q_int_vec));
81 }
82 
83 BOOST_AUTO_TEST_CASE(test_rnea_derivatives)
84 {
85  typedef double Scalar;
86  typedef casadi::SX ADScalar;
87 
88  typedef pinocchio::ModelTpl<Scalar> Model;
89  typedef Model::Data Data;
90 
91  typedef pinocchio::ModelTpl<ADScalar> ADModel;
92  typedef ADModel::Data ADData;
93 
94  Model model;
95  pinocchio::buildModels::humanoidRandom(model);
96  model.lowerPositionLimit.head<3>().fill(-1.);
97  model.upperPositionLimit.head<3>().fill(1.);
98  Data data(model);
99 
100  typedef Model::ConfigVectorType ConfigVector;
101  typedef Model::TangentVectorType TangentVector;
102  ConfigVector q(model.nq);
103  q = pinocchio::randomConfiguration(model);
104  TangentVector v(TangentVector::Random(model.nv));
105  TangentVector a(TangentVector::Random(model.nv));
106 
107  typedef ADModel::ConfigVectorType ConfigVectorAD;
108  typedef ADModel::TangentVectorType TangentVectorAD;
109  ADModel ad_model = model.cast<ADScalar>();
110  ADData ad_data(ad_model);
111 
112  pinocchio::rnea(model,data,q,v,a);
113 
114  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
115  casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);
116 
117  ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
118  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
119  v_int_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_v_int).data(),model.nv,1);
120 
121  pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad);
122  casadi::SX cs_q_int(model.nq,1);
123  pinocchio::casadi::copy(q_int_ad,cs_q_int);
124  std::vector<double> q_vec((size_t)model.nq);
125  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;
126 
127  std::vector<double> v_int_vec((size_t)model.nv);
128  Eigen::Map<TangentVector>(v_int_vec.data(),model.nv,1).setZero();
129 
130  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
131  TangentVectorAD v_ad(model.nv);
132  v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);
133 
134  casadi::SX cs_a = casadi::SX::sym("a", model.nv);
135  TangentVectorAD a_ad(model.nv);
136  a_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_a).data(),model.nv,1);
137 
138  rnea(ad_model,ad_data,q_int_ad,v_ad,a_ad);
139  casadi::SX cs_tau(model.nv,1);
140  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
141  {
142  cs_tau(k) = ad_data.tau[k];
143  }
144  casadi::Function eval_rnea("eval_rnea",
145  casadi::SXVector {cs_q,cs_v_int, cs_v, cs_a},
146  casadi::SXVector {cs_tau});
147 
148  std::vector<double> v_vec((size_t)model.nv);
149  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;
150 
151  std::vector<double> a_vec((size_t)model.nv);
152  Eigen::Map<TangentVector>(a_vec.data(),model.nv,1) = a;
153 
154  // check return value
155  casadi::DM tau_res = eval_rnea(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0];
156  std::cout << "tau_res = " << tau_res << std::endl;
157  Data::TangentVectorType tau_vec = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(tau_res).data(),model.nv,1);
158 
159  BOOST_CHECK(data.tau.isApprox(tau_vec));
160 
161  // compute references
162  Data::MatrixXs dtau_dq_ref(model.nv,model.nv), dtau_dv_ref(model.nv,model.nv), dtau_da_ref(model.nv,model.nv);
163  dtau_dq_ref.setZero(); dtau_dv_ref.setZero(); dtau_da_ref.setZero();
164 
165  pinocchio::computeRNEADerivatives(model,data,q,v,a,dtau_dq_ref,dtau_dv_ref,dtau_da_ref);
166  dtau_da_ref.triangularView<Eigen::StrictlyLower>() = dtau_da_ref.transpose().triangularView<Eigen::StrictlyLower>();
167 
168  // check with respect to q+dq
169  casadi::SX dtau_dq = jacobian(cs_tau, cs_v_int);
170  casadi::Function eval_dtau_dq("eval_dtau_dq",
171  casadi::SXVector {cs_q,cs_v_int, cs_v, cs_a},
172  casadi::SXVector {dtau_dq});
173 
174  casadi::DM dtau_dq_res = eval_dtau_dq(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0];
175  std::vector<double> dtau_dq_vec(static_cast< std::vector<double> >(dtau_dq_res));
176  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(dtau_dq_vec.data(),model.nv,model.nv).isApprox(dtau_dq_ref));
177 
178  // check with respect to v+dv
179  casadi::SX dtau_dv = jacobian(cs_tau, cs_v);
180  casadi::Function eval_dtau_dv("eval_dtau_dv",
181  casadi::SXVector {cs_q,cs_v_int, cs_v, cs_a},
182  casadi::SXVector {dtau_dv});
183 
184  casadi::DM dtau_dv_res = eval_dtau_dv(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0];
185  std::vector<double> dtau_dv_vec(static_cast< std::vector<double> >(dtau_dv_res));
186  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(dtau_dv_vec.data(),model.nv,model.nv).isApprox(dtau_dv_ref));
187 
188  // check with respect to a+da
189  casadi::SX dtau_da = jacobian(cs_tau, cs_a);
190  casadi::Function eval_dtau_da("eval_dtau_da",
191  casadi::SXVector {cs_q,cs_v_int, cs_v, cs_a},
192  casadi::SXVector {dtau_da});
193 
194  casadi::DM dtau_da_res = eval_dtau_da(casadi::DMVector {q_vec,v_int_vec,v_vec,a_vec})[0];
195  std::vector<double> dtau_da_vec(static_cast< std::vector<double> >(dtau_da_res));
196  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(dtau_da_vec.data(),model.nv,model.nv).isApprox(dtau_da_ref));
197 
198  // call RNEA derivatives in Casadi
199  casadi::SX cs_dtau_dq(model.nv,model.nv);
200  casadi::SX cs_dtau_dv(model.nv,model.nv);
201  casadi::SX cs_dtau_da(model.nv,model.nv);
202 
203  computeRNEADerivatives(ad_model,ad_data,q_ad,v_ad,a_ad);
204  ad_data.M.triangularView<Eigen::StrictlyLower>()
205  = ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
206 
207  pinocchio::casadi::copy(ad_data.dtau_dq,cs_dtau_dq);
208  pinocchio::casadi::copy(ad_data.dtau_dv,cs_dtau_dv);
209  pinocchio::casadi::copy(ad_data.M,cs_dtau_da);
210 
211  casadi::Function eval_rnea_derivatives_dq("eval_rnea_derivatives_dq",
212  casadi::SXVector {cs_q, cs_v, cs_a},
213  casadi::SXVector {cs_dtau_dq});
214 
215  casadi::DM dtau_dq_res_direct = eval_rnea_derivatives_dq(casadi::DMVector {q_vec,v_vec,a_vec})[0];
216  Data::MatrixXs dtau_dq_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(dtau_dq_res_direct).data(),model.nv,model.nv);
217  BOOST_CHECK(dtau_dq_ref.isApprox(dtau_dq_res_direct_map));
218 
219  casadi::Function eval_rnea_derivatives_dv("eval_rnea_derivatives_dv",
220  casadi::SXVector {cs_q, cs_v, cs_a},
221  casadi::SXVector {cs_dtau_dv});
222 
223  casadi::DM dtau_dv_res_direct = eval_rnea_derivatives_dv(casadi::DMVector {q_vec,v_vec,a_vec})[0];
224  Data::MatrixXs dtau_dv_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(dtau_dv_res_direct).data(),model.nv,model.nv);
225  BOOST_CHECK(dtau_dv_ref.isApprox(dtau_dv_res_direct_map));
226 
227  casadi::Function eval_rnea_derivatives_da("eval_rnea_derivatives_da",
228  casadi::SXVector {cs_q, cs_v, cs_a},
229  casadi::SXVector {cs_dtau_da});
230 
231  casadi::DM dtau_da_res_direct = eval_rnea_derivatives_da(casadi::DMVector {q_vec,v_vec,a_vec})[0];
232  Data::MatrixXs dtau_da_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(dtau_da_res_direct).data(),model.nv,model.nv);
233  BOOST_CHECK(dtau_da_ref.isApprox(dtau_da_res_direct_map));
234 }
235 
236  BOOST_AUTO_TEST_CASE(test_aba)
237  {
238  typedef double Scalar;
239  typedef casadi::SX ADScalar;
240 
241  typedef pinocchio::ModelTpl<Scalar> Model;
242  typedef Model::Data Data;
243 
244  typedef pinocchio::ModelTpl<ADScalar> ADModel;
245  typedef ADModel::Data ADData;
246 
247  Model model;
248  pinocchio::buildModels::humanoidRandom(model);
249  model.lowerPositionLimit.head<3>().fill(-1.);
250  model.upperPositionLimit.head<3>().fill(1.);
251  Data data(model);
252 
253  typedef Model::ConfigVectorType ConfigVector;
254  typedef Model::TangentVectorType TangentVector;
255  ConfigVector q(model.nq);
256  q = pinocchio::randomConfiguration(model);
257  TangentVector v(TangentVector::Random(model.nv));
258  TangentVector tau(TangentVector::Random(model.nv));
259 
260  typedef ADModel::ConfigVectorType ConfigVectorAD;
261  typedef ADModel::TangentVectorType TangentVectorAD;
262  ADModel ad_model = model.cast<ADScalar>();
263  ADData ad_data(ad_model);
264 
265  pinocchio::aba(model,data,q,v,tau);
266 
267  casadi::SX cs_q = casadi::SX::sym("q", model.nq);
268  casadi::SX cs_v_int = casadi::SX::sym("v_inc", model.nv);
269  ConfigVectorAD q_ad(model.nq), v_int_ad(model.nv), q_int_ad(model.nq);
270  q_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_q).data(),model.nq,1);
271  v_int_ad = Eigen::Map<ConfigVectorAD>(static_cast< std::vector<ADScalar> >(cs_v_int).data(),model.nv,1);
272 
273  pinocchio::integrate(ad_model,q_ad,v_int_ad,q_int_ad);
274  casadi::SX cs_q_int(model.nq,1);
275  pinocchio::casadi::copy(q_int_ad,cs_q_int);
276  std::vector<double> q_vec((size_t)model.nq);
277  Eigen::Map<ConfigVector>(q_vec.data(),model.nq,1) = q;
278 
279  std::vector<double> v_int_vec((size_t)model.nv);
280  Eigen::Map<TangentVector>(v_int_vec.data(),model.nv,1).setZero();
281 
282  casadi::SX cs_v = casadi::SX::sym("v", model.nv);
283  TangentVectorAD v_ad(model.nv);
284  v_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_v).data(),model.nv,1);
285 
286  casadi::SX cs_tau = casadi::SX::sym("tau", model.nv);
287  TangentVectorAD tau_ad(model.nv);
288  tau_ad = Eigen::Map<TangentVectorAD>(static_cast< std::vector<ADScalar> >(cs_tau).data(),model.nv,1);
289 
290  // ABA
291  aba(ad_model,ad_data,q_int_ad,v_ad,tau_ad);
292  casadi::SX cs_ddq(model.nv,1);
293  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
294  cs_ddq(k) = ad_data.ddq[k];
295  casadi::Function eval_aba("eval_aba",
296  casadi::SXVector {cs_q, cs_v_int, cs_v, cs_tau},
297  casadi::SXVector {cs_ddq});
298 
299  std::vector<double> v_vec((size_t)model.nv);
300  Eigen::Map<TangentVector>(v_vec.data(),model.nv,1) = v;
301 
302  std::vector<double> tau_vec((size_t)model.nv);
303  Eigen::Map<TangentVector>(tau_vec.data(),model.nv,1) = tau;
304 
305  casadi::DM ddq_res = eval_aba(casadi::DMVector {q_vec, v_int_vec, v_vec, tau_vec})[0];
306  Data::TangentVectorType ddq_mat = Eigen::Map<Data::TangentVectorType>(static_cast< std::vector<double> >(ddq_res).data(),
307  model.nv,1);
308 
309  BOOST_CHECK(ddq_mat.isApprox(data.ddq));
310 
311  // compute references
312  Data::MatrixXs ddq_dq_ref(model.nv,model.nv), ddq_dv_ref(model.nv,model.nv), ddq_dtau_ref(model.nv,model.nv);
313  ddq_dq_ref.setZero(); ddq_dv_ref.setZero(); ddq_dtau_ref.setZero();
314 
315  pinocchio::computeABADerivatives(model,data,q,v,tau,ddq_dq_ref,ddq_dv_ref,ddq_dtau_ref);
316  ddq_dtau_ref.triangularView<Eigen::StrictlyLower>()
317  = ddq_dtau_ref.transpose().triangularView<Eigen::StrictlyLower>();
318 
319  // check with respect to q+dq
320  casadi::SX ddq_dq = jacobian(cs_ddq, cs_v_int);
321  casadi::Function eval_ddq_dq("eval_ddq_dq",
322  casadi::SXVector {cs_q,cs_v_int,cs_v,cs_tau},
323  casadi::SXVector {ddq_dq});
324 
325  casadi::DM ddq_dq_res = eval_ddq_dq(casadi::DMVector {q_vec,v_int_vec,v_vec,tau_vec})[0];
326  std::vector<double> ddq_dq_vec(static_cast< std::vector<double> >(ddq_dq_res));
327  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(ddq_dq_vec.data(),model.nv,model.nv).isApprox(ddq_dq_ref));
328 
329  // check with respect to v+dv
330  casadi::SX ddq_dv = jacobian(cs_ddq, cs_v);
331  casadi::Function eval_ddq_dv("eval_ddq_dv",
332  casadi::SXVector {cs_q,cs_v_int, cs_v, cs_tau},
333  casadi::SXVector {ddq_dv});
334 
335  casadi::DM ddq_dv_res = eval_ddq_dv(casadi::DMVector {q_vec,v_int_vec,v_vec,tau_vec})[0];
336  std::vector<double> ddq_dv_vec(static_cast< std::vector<double> >(ddq_dv_res));
337  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(ddq_dv_vec.data(),model.nv,model.nv).isApprox(ddq_dv_ref));
338 
339  // check with respect to a+da
340  casadi::SX ddq_dtau = jacobian(cs_ddq, cs_tau);
341  casadi::Function eval_ddq_da("eval_ddq_da",
342  casadi::SXVector {cs_q,cs_v_int, cs_v, cs_tau},
343  casadi::SXVector {ddq_dtau});
344 
345  casadi::DM ddq_dtau_res = eval_ddq_da(casadi::DMVector {q_vec,v_int_vec,v_vec,tau_vec})[0];
346  std::vector<double> ddq_dtau_vec(static_cast< std::vector<double> >(ddq_dtau_res));
347  BOOST_CHECK(Eigen::Map<Data::MatrixXs>(ddq_dtau_vec.data(),model.nv,model.nv).isApprox(ddq_dtau_ref));
348 
349  // call ABA derivatives in Casadi
350  casadi::SX cs_ddq_dq(model.nv,model.nv);
351  casadi::SX cs_ddq_dv(model.nv,model.nv);
352  casadi::SX cs_ddq_dtau(model.nv,model.nv);
353 
354  computeABADerivatives(ad_model,ad_data,q_ad,v_ad,tau_ad);
355  ad_data.Minv.triangularView<Eigen::StrictlyLower>()
356  = ad_data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
357 
358  pinocchio::casadi::copy(ad_data.ddq_dq,cs_ddq_dq);
359  pinocchio::casadi::copy(ad_data.ddq_dv,cs_ddq_dv);
360  pinocchio::casadi::copy(ad_data.Minv,cs_ddq_dtau);
361 
362  casadi::Function eval_aba_derivatives_dq("eval_aba_derivatives_dq",
363  casadi::SXVector {cs_q, cs_v, cs_tau},
364  casadi::SXVector {cs_ddq_dq});
365 
366  casadi::DM ddq_dq_res_direct = eval_aba_derivatives_dq(casadi::DMVector {q_vec,v_vec,tau_vec})[0];
367  Data::MatrixXs ddq_dq_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(ddq_dq_res_direct).data(),model.nv,model.nv);
368  BOOST_CHECK(ddq_dq_ref.isApprox(ddq_dq_res_direct_map));
369 
370  casadi::Function eval_aba_derivatives_dv("eval_aba_derivatives_dv",
371  casadi::SXVector {cs_q, cs_v, cs_tau},
372  casadi::SXVector {cs_ddq_dv});
373 
374  casadi::DM ddq_dv_res_direct = eval_aba_derivatives_dv(casadi::DMVector {q_vec,v_vec,tau_vec})[0];
375  Data::MatrixXs ddq_dv_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(ddq_dv_res_direct).data(),model.nv,model.nv);
376  BOOST_CHECK(ddq_dv_ref.isApprox(ddq_dv_res_direct_map));
377 
378  casadi::Function eval_aba_derivatives_dtau("eval_aba_derivatives_dtau",
379  casadi::SXVector {cs_q, cs_v, cs_tau},
380  casadi::SXVector {cs_ddq_dtau});
381 
382  casadi::DM ddq_dtau_res_direct = eval_aba_derivatives_dtau(casadi::DMVector {q_vec,v_vec,tau_vec})[0];
383  Data::MatrixXs ddq_dtau_res_direct_map = Eigen::Map<Data::MatrixXs>(static_cast< std::vector<double> >(ddq_dtau_res_direct).data(),model.nv,model.nv);
384  BOOST_CHECK(ddq_dtau_ref.isApprox(ddq_dtau_res_direct_map));
385  }
386 
387 BOOST_AUTO_TEST_SUITE_END()
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)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::Model
ModelTpl< double > Model
Definition: src/multibody/fwd.hpp:35
q
q
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
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
fill
fill
pinocchio::casadi::sym
void sym(const Eigen::MatrixBase< MatrixDerived > &eig_mat, std::string const &name)
Definition: autodiff/casadi.hpp:170
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_integrate)
Definition: casadi-algo-derivatives.cpp:22
pinocchio::python::Scalar
SE3::Scalar Scalar
Definition: conversions.cpp:13
pinocchio::computeRNEADerivatives
void computeRNEADerivatives(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, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint c...
isApprox
bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol)
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
a
Vec3f a
pinocchio::Data
DataTpl< double > Data
Definition: src/multibody/fwd.hpp:36
setZero
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
pinocchio::computeABADerivatives
void computeABADerivatives(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 Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.
pinocchio::casadi::copy
void copy(::casadi::Matrix< Scalar > const &src, Eigen::MatrixBase< MT > &dst)
Definition: autodiff/casadi.hpp:139
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
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28