timings-cppad-jit.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2022-2024 INRIA
3 //
4 #include "pinocchio/codegen/cppadcg.hpp"
5 #include "pinocchio/codegen/code-generator-algo.hpp"
6 #include "pinocchio/container/aligned-vector.hpp"
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 #include "pinocchio/algorithm/kinematics.hpp"
12 #include "pinocchio/algorithm/jacobian.hpp"
13 #include "pinocchio/algorithm/crba.hpp"
14 #include "pinocchio/algorithm/rnea.hpp"
15 #include "pinocchio/algorithm/joint-configuration.hpp"
16 
17 #include "pinocchio/multibody/sample-models.hpp"
18 
19 #include <iostream>
20 
21 #ifdef _WIN32
22  #define DLL_EXT ".dll"
23 #else
24  #define DLL_EXT ".so"
25 #endif
26 
27 #include "pinocchio/utils/timer.hpp"
28 
29 bool DELETE_GENERATED_LIBS_AFTER_TEST = true;
30 
31 int main()
32 {
33  using namespace Eigen;
34  using namespace pinocchio;
35 
36  PinocchioTicToc timer(PinocchioTicToc::US);
37 #ifdef NDEBUG
38  const int NBT = 1000 * 100;
39 #else
40  const int NBT = 1;
41  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
42 #endif
43 
44  using CppAD::AD;
45  using CppAD::NearEqual;
46 
47  enum
48  {
49  Options = 0
50  };
51  typedef double Scalar;
52  typedef AD<Scalar> ADScalar;
53  typedef CppAD::cg::CG<Scalar> CGScalar;
54  typedef CppAD::AD<CGScalar> ADCGScalar;
55  typedef CppAD::ADFun<CGScalar> ADCGFun;
56 
57  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ADVectorXs;
58  typedef Eigen::Matrix<ADCGScalar, Eigen::Dynamic, 1, Options> ADCGVectorXs;
59  typedef Eigen::Matrix<ADScalar, Eigen::Dynamic, Eigen::Dynamic> ADMatrixXs;
60  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> RowMatrixXs;
61 
62  typedef pinocchio::ModelTpl<Scalar> Model;
63  typedef Model::Data Data;
64 
65  typedef pinocchio::ModelTpl<ADScalar> ADModel;
66  typedef ADModel::Data ADData;
67 
68  typedef pinocchio::ModelTpl<ADCGScalar> ADCGModel;
69  typedef ADCGModel::Data ADCGData;
70 
71  typedef Model::ConfigVectorType ConfigVectorType;
72  typedef Model::TangentVectorType TangentVectorType;
73 
74  typedef ADModel::ConfigVectorType ADConfigVectorType;
75  typedef ADModel::TangentVectorType ADTangentVectorType;
76 
77  typedef ADCGModel::ConfigVectorType ADCGConfigVectorType;
78  typedef ADCGModel::TangentVectorType ADCGTangentVectorType;
79 
80  Model model;
81  pinocchio::buildModels::humanoidRandom(model);
82  int nq = model.nq;
83  int nv = model.nv;
84  int nx = nq + 2 * nv;
85  model.lowerPositionLimit.head<3>().fill(-1.);
86  model.upperPositionLimit.head<3>().fill(1.);
87 
88  ConfigVectorType q(nq);
89  q = pinocchio::randomConfiguration(model);
90  TangentVectorType v(TangentVectorType::Random(nv));
91  TangentVectorType a(TangentVectorType::Random(nv));
92 
93  {
94  Data data(model);
95 
96  std::vector<ConfigVectorType> qs;
97  for (size_t it = 0; it < NBT; ++it)
98  {
99  q = pinocchio::randomConfiguration(model);
100  qs.push_back(q);
101  }
102 
103  timer.tic();
104  SMOOTH(NBT)
105  {
106  pinocchio::crba(model, data, qs[_smooth], pinocchio::Convention::WORLD);
107  data.M.triangularView<Eigen::StrictlyLower>() =
108  data.M.transpose().triangularView<Eigen::StrictlyLower>();
109  }
110  std::cout << "Calculate data.M (JSIM) with CRBA =\t\t";
111  timer.toc(std::cout, NBT);
112  }
113 
114  {
115  ADModel ad_model = model.cast<ADScalar>();
116  ADData ad_data(ad_model);
117 
118  ADConfigVectorType ad_q = q.cast<ADScalar>();
119 
120  CppAD::Independent(ad_q);
121  pinocchio::crba(ad_model, ad_data, ad_q);
122  ad_data.M.triangularView<Eigen::StrictlyLower>() =
123  ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
124 
125  ADVectorXs M_vector =
126  Eigen::Map<ADVectorXs>(ad_data.M.data(), ad_data.M.cols() * ad_data.M.rows());
127 
128  CppAD::ADFun<Scalar> ad_fun(ad_q, M_vector);
129  ad_fun.function_name_set("ad_fun_crba");
130 
131  CPPAD_TESTVECTOR(Scalar) x((size_t)nq);
132  std::vector<CPPAD_TESTVECTOR(Scalar)> xs;
133  for (size_t it = 0; it < NBT; ++it)
134  {
135  q = pinocchio::randomConfiguration(model);
136  Eigen::Map<Data::ConfigVectorType>(x.data(), nq, 1) = q;
137  xs.push_back(x);
138  }
139 
140  timer.tic();
141  SMOOTH(NBT)
142  {
143  CPPAD_TESTVECTOR(Scalar) M_vector = ad_fun.Forward(0, xs[_smooth]);
144  }
145  std::cout << "Calculate data.M (JSIM) using CppAD =\t\t";
146  timer.toc(std::cout, NBT);
147  }
148 
149  {
150  ADModel ad_model = model.cast<ADScalar>();
151  ADData ad_data(ad_model);
152 
153  ADConfigVectorType ad_q = q.cast<ADScalar>();
154 
155  CppAD::Independent(ad_q);
156  pinocchio::crba(ad_model, ad_data, ad_q, pinocchio::Convention::WORLD);
157  ad_data.M.triangularView<Eigen::StrictlyLower>() =
158  ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
159 
160  ADVectorXs M_vector =
161  Eigen::Map<ADVectorXs>(ad_data.M.data(), ad_data.M.cols() * ad_data.M.rows());
162 
163  CppAD::ADFun<Scalar> ad_fun(ad_q, M_vector);
164  ad_fun.function_name_set("ad_fun_crba");
165 
166  // create csrc_file
167  std::string c_type = "double";
168  std::string csrc_file = "./jit_JSIM_crba.c";
169  std::ofstream ofs;
170  ofs.open(csrc_file, std::ofstream::out);
171  ad_fun.to_csrc(ofs, c_type);
172  ofs.close();
173 
174  // create dll_file
175  std::string dll_file = "./jit_JSIM_crba" DLL_EXT;
176  CPPAD_TESTVECTOR(std::string) csrc_files(1);
177  csrc_files[0] = csrc_file;
178  std::map<std::string, std::string> options;
179  std::string err_msg = CppAD::create_dll_lib(dll_file, csrc_files, options);
180  if (err_msg != "")
181  {
182  std::cerr << "jit_JSIM_crba: err_msg = " << err_msg << "\n";
183  }
184 
185  // dll_linker
186  CppAD::link_dll_lib dll_linker(dll_file, err_msg);
187  if (err_msg != "")
188  {
189  std::cerr << "jit_JSIM_crba: err_msg = " << err_msg << "\n";
190  }
191 
192  std::string function_name = "cppad_jit_ad_fun_crba";
193  void * void_ptr = dll_linker(function_name, err_msg);
194  if (err_msg != "")
195  {
196  std::cerr << "jit_JSIM_crba: err_msg = " << err_msg << "\n";
197  }
198 
199  using CppAD::jit_double;
200  jit_double ad_fun_ptr = reinterpret_cast<jit_double>(void_ptr);
201 
202  CPPAD_TESTVECTOR(Scalar) x((size_t)nq);
203  size_t compare_change = 0, nx = (size_t)nq, nM_vector = (size_t)nv * (size_t)nv;
204  CPPAD_TESTVECTOR(Scalar) M_vector_jit(nM_vector);
205 
206  std::vector<CPPAD_TESTVECTOR(Scalar)> xs;
207  for (size_t it = 0; it < NBT; ++it)
208  {
209  q = pinocchio::randomConfiguration(model);
210  Eigen::Map<Data::ConfigVectorType>(x.data(), nq, 1) = q;
211  xs.push_back(x);
212  }
213 
214  timer.tic();
215  SMOOTH(NBT)
216  {
217  ad_fun_ptr(nx, xs[_smooth].data(), nM_vector, M_vector_jit.data(), &compare_change);
218  }
219  std::cout << "Calculate data.M (JSIM) using CppAD-jit =\t";
220  timer.toc(std::cout, NBT);
221  }
222 
223  {
224  ADCGModel ad_model(model.template cast<ADCGScalar>());
225  ADCGData ad_data(ad_model);
226 
227  ADCGConfigVectorType ad_q = q.cast<ADCGScalar>();
228  ad_q = q.cast<ADCGScalar>();
229 
230  std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>> cgen_ptr;
231  std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>> libcgen_ptr;
232  std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>> dynamicLibManager_ptr;
233  std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib_ptr;
234  std::unique_ptr<CppAD::cg::GenericModel<Scalar>> generatedFun_ptr;
235 
236  const std::string & function_name = "crba";
237  const std::string & library_name = "cg_crba_eval";
238  const std::string & compile_options = "-Ofast";
239 
240  CppAD::Independent(ad_q);
241  pinocchio::crba(ad_model, ad_data, ad_q, pinocchio::Convention::WORLD);
242  ad_data.M.triangularView<Eigen::StrictlyLower>() =
243  ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
244 
245  ADCGVectorXs M_vector =
246  Eigen::Map<ADCGVectorXs>(ad_data.M.data(), ad_data.M.cols() * ad_data.M.rows());
247 
248  ADCGFun ad_fun;
249  ad_fun.Dependent(ad_q, M_vector);
250  ad_fun.optimize("no_compare_op");
251 
252  // generates source code
253  cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>>(
254  new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun, function_name));
255  cgen_ptr->setCreateForwardZero(true);
256  cgen_ptr->setCreateJacobian(false);
257  libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>>(
258  new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
259 
260  dynamicLibManager_ptr = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>>(
261  new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr, library_name));
262 
263  CppAD::cg::GccCompiler<Scalar> compiler(PINOCCHIO_CXX_COMPILER);
264  std::vector<std::string> compile_flags = compiler.getCompileFlags();
265  compile_flags[0] = compile_options;
266  compiler.setCompileFlags(compile_flags);
267  dynamicLibManager_ptr->createDynamicLibrary(compiler, false);
268 
269  const auto it = dynamicLibManager_ptr->getOptions().find("dlOpenMode");
270  if (it == dynamicLibManager_ptr->getOptions().end())
271  {
272  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
273  dynamicLibManager_ptr->getLibraryName()
274  + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
275  }
276  else
277  {
278  int dlOpenMode = std::stoi(it->second);
279  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
280  dynamicLibManager_ptr->getLibraryName()
281  + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION,
282  dlOpenMode));
283  }
284 
285  generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
286 
287  CPPAD_TESTVECTOR(Scalar) x((size_t)nq);
288  CPPAD_TESTVECTOR(Scalar) y((size_t)nv * (size_t)nv);
289 
290  std::vector<CPPAD_TESTVECTOR(Scalar)> xs;
291  for (size_t it = 0; it < NBT; ++it)
292  {
293  q = pinocchio::randomConfiguration(model);
294  Eigen::Map<Data::ConfigVectorType>(x.data(), nq, 1) = q;
295  xs.push_back(x);
296  }
297 
298  timer.tic();
299  SMOOTH(NBT)
300  {
301  generatedFun_ptr->ForwardZero(xs[_smooth], y);
302  }
303  std::cout << "Calculate data.M (JSIM) using CppADCodeGen =\t";
304  timer.toc(std::cout, NBT);
305  }
306 
307  std::cout << std::endl;
308 
309  {
310  Data data(model);
311 
312  std::vector<ConfigVectorType> qs, vs, as;
313  for (size_t it = 0; it < NBT; ++it)
314  {
315  q = pinocchio::randomConfiguration(model);
316  TangentVectorType v(TangentVectorType::Random(nv));
317  TangentVectorType a(TangentVectorType::Random(nv));
318  qs.push_back(q);
319  vs.push_back(v);
320  as.push_back(a);
321  }
322 
323  timer.tic();
324  SMOOTH(NBT)
325  {
326  computeRNEADerivatives(model, data, qs[_smooth], vs[_smooth], as[_smooth]);
327  }
328  std::cout << "Calculate dtau_dx with computeRNEADerivatives =\t";
329  timer.toc(std::cout, NBT);
330  }
331 
332  {
333  ADModel ad_model = model.cast<ADScalar>();
334  ADData ad_data(ad_model);
335 
336  ADConfigVectorType ad_q = q.cast<ADScalar>();
337  ADTangentVectorType ad_v = v.cast<ADScalar>();
338  ADTangentVectorType ad_a = a.cast<ADScalar>();
339 
340  ADConfigVectorType ad_X = ADConfigVectorType::Zero(nq + 2 * nv);
341  Eigen::DenseIndex i = 0;
342  ad_X.segment(i, nq) = ad_q;
343  i += nq;
344  ad_X.segment(i, nv) = ad_v;
345  i += nv;
346  ad_X.segment(i, nv) = ad_a;
347  i += nv;
348 
349  CppAD::Independent(ad_X);
350  pinocchio::rnea(
351  ad_model, ad_data, ad_X.segment(0, nq), ad_X.segment(nq, nv), ad_X.segment(nq + nv, nv));
352 
353  ADVectorXs ad_Y(nv);
354  Eigen::Map<ADData::TangentVectorType>(ad_Y.data(), nv, 1) = ad_data.tau;
355 
356  CppAD::ADFun<Scalar> ad_fun(ad_X, ad_Y);
357 
358  CPPAD_TESTVECTOR(Scalar) x((size_t)nx);
359  std::vector<CPPAD_TESTVECTOR(Scalar)> xs;
360  for (size_t it = 0; it < NBT; ++it)
361  {
362  q = pinocchio::randomConfiguration(model);
363  TangentVectorType v(TangentVectorType::Random(nv));
364  TangentVectorType a(TangentVectorType::Random(nv));
365  ConfigVectorType _x = ConfigVectorType::Zero(nq + 2 * nv);
366  Eigen::DenseIndex i = 0;
367  _x.segment(i, nq) = q;
368  i += nq;
369  _x.segment(i, nv) = v;
370  i += nv;
371  _x.segment(i, nv) = a;
372  i += nv;
373 
374  Eigen::Map<Data::TangentVectorType>(x.data(), nx, 1) = _x;
375  xs.push_back(x);
376  }
377  timer.tic();
378  SMOOTH(NBT)
379  {
380  CPPAD_TESTVECTOR(Scalar) dtau_da = ad_fun.Jacobian(xs[_smooth]);
381  }
382  std::cout << "Calculate dtau_dx (∂RNEA) using CppAD =\t\t";
383  timer.toc(std::cout, NBT);
384  }
385 
386  {
387  ADModel ad_model = model.cast<ADScalar>();
388  ADData ad_data(ad_model);
389 
390  ADConfigVectorType ad_q = q.cast<ADScalar>();
391  ADTangentVectorType ad_v = v.cast<ADScalar>();
392  ADTangentVectorType ad_a = a.cast<ADScalar>();
393  ADConfigVectorType ad_X = ADConfigVectorType::Zero(nx);
394  Eigen::DenseIndex i = 0;
395  ad_X.segment(i, nq) = ad_q;
396  i += nq;
397  ad_X.segment(i, nv) = ad_v;
398  i += nv;
399  ad_X.segment(i, nv) = ad_a;
400  i += nv;
401 
402  CppAD::Independent(ad_X);
403  pinocchio::rnea(
404  ad_model, ad_data, ad_X.segment(0, nq), ad_X.segment(nq, nv), ad_X.segment(nq + nv, nv));
405 
406  ADVectorXs ad_Y(nv);
407  Eigen::Map<ADData::TangentVectorType>(ad_Y.data(), nv, 1) = ad_data.tau;
408 
409  CppAD::ADFun<Scalar> f(ad_X, ad_Y);
410  CppAD::ADFun<ADScalar, Scalar> af = f.base2ad();
411 
412  CppAD::Independent(ad_X);
413  ADMatrixXs dtau_dx = af.Jacobian(ad_X);
414  ADVectorXs dtau_dx_vector(nv * nx);
415  dtau_dx_vector = Eigen::Map<ADVectorXs>(dtau_dx.data(), dtau_dx.cols() * dtau_dx.rows());
416  CppAD::ADFun<double> ad_fun(ad_X, dtau_dx_vector);
417 
418  ad_fun.function_name_set("ad_fun");
419 
420  // create csrc_file
421  std::string c_type = "double";
422  std::string csrc_file = "./jit_JSIM.c";
423  std::ofstream ofs;
424  ofs.open(csrc_file, std::ofstream::out);
425  ad_fun.to_csrc(ofs, c_type);
426  ofs.close();
427 
428  // create dll_file
429  std::string dll_file = "./jit_JSIM" DLL_EXT;
430  CPPAD_TESTVECTOR(std::string) csrc_files(1);
431  csrc_files[0] = csrc_file;
432  std::map<std::string, std::string> options;
433  std::string err_msg = CppAD::create_dll_lib(dll_file, csrc_files, options);
434  if (err_msg != "")
435  {
436  std::cerr << "jit_JSIM: err_msg = " << err_msg << "\n";
437  }
438 
439  // dll_linker
440  CppAD::link_dll_lib dll_linker(dll_file, err_msg);
441  if (err_msg != "")
442  {
443  std::cerr << "jit_JSIM: err_msg = " << err_msg << "\n";
444  }
445 
446  std::string function_name = "cppad_jit_ad_fun";
447  void * void_ptr = dll_linker(function_name, err_msg);
448  if (err_msg != "")
449  {
450  std::cerr << "jit_JSIM: err_msg = " << err_msg << "\n";
451  }
452 
453  using CppAD::jit_double;
454  jit_double ad_fun_ptr = reinterpret_cast<jit_double>(void_ptr);
455 
456  CPPAD_TESTVECTOR(Scalar) x((size_t)nx);
457  std::vector<CPPAD_TESTVECTOR(Scalar)> xs;
458  for (size_t it = 0; it < NBT; ++it)
459  {
460  q = pinocchio::randomConfiguration(model);
461  TangentVectorType v(TangentVectorType::Random(nv));
462  TangentVectorType a(TangentVectorType::Random(nv));
463  ConfigVectorType _x = ConfigVectorType::Zero(nx);
464  Eigen::DenseIndex i = 0;
465  _x.segment(i, nq) = q;
466  i += nq;
467  _x.segment(i, nv) = v;
468  i += nv;
469  _x.segment(i, nv) = a;
470  i += nv;
471 
472  Eigen::Map<Data::TangentVectorType>(x.data(), nx, 1) = _x;
473  xs.push_back(x);
474  }
475 
476  size_t compare_change = 0, nx_ = (size_t)nx, ndtau_dx = (size_t)nv * (size_t)nx;
477  std::vector<double> dtau_dx_jit(ndtau_dx);
478 
479  timer.tic();
480  SMOOTH(NBT)
481  {
482  ad_fun_ptr(nx_, xs[_smooth].data(), ndtau_dx, dtau_dx_jit.data(), &compare_change);
483  }
484  std::cout << "Calculate dtau_dx (∂RNEA) using CppAD-jit =\t";
485  timer.toc(std::cout, NBT);
486  }
487 
488  {
489  ADCGModel ad_model(model.template cast<ADCGScalar>());
490  ADCGData ad_data(ad_model);
491 
492  ADCGVectorXs ad_Y(nv);
493 
494  ADCGConfigVectorType ad_q = q.cast<ADCGScalar>();
495  ADCGTangentVectorType ad_v = v.cast<ADCGScalar>();
496  ADCGTangentVectorType ad_a = a.cast<ADCGScalar>();
497  ADCGConfigVectorType ad_X = ADCGConfigVectorType::Zero(nx);
498  Eigen::DenseIndex i = 0;
499  ad_X.segment(i, nq) = ad_q;
500  i += nq;
501  ad_X.segment(i, nv) = ad_v;
502  i += nv;
503  ad_X.segment(i, nv) = ad_a;
504  i += nv;
505 
506  ADCGFun ad_fun;
507 
508  std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>> cgen_ptr;
509  std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>> libcgen_ptr;
510  std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>> dynamicLibManager_ptr;
511  std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib_ptr;
512  std::unique_ptr<CppAD::cg::GenericModel<Scalar>> generatedFun_ptr;
513 
514  const std::string & function_name = "rnea";
515  const std::string & library_name = "cg_rnea_eval";
516  const std::string & compile_options = "-Ofast";
517 
518  CppAD::Independent(ad_X);
519  pinocchio::rnea(
520  ad_model, ad_data, ad_X.segment(0, nq), ad_X.segment(nq, nv), ad_X.segment(nq + nv, nv));
521  ad_Y = ad_data.tau;
522  ad_fun.Dependent(ad_X, ad_Y);
523  ad_fun.optimize("no_compare_op");
524  RowMatrixXs jac = RowMatrixXs::Zero(ad_Y.size(), ad_X.size());
525 
526  // generates source code
527  cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>>(
528  new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun, function_name));
529  cgen_ptr->setCreateForwardZero(true);
530  cgen_ptr->setCreateJacobian(true);
531  libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>>(
532  new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
533 
534  dynamicLibManager_ptr = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>>(
535  new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr, library_name));
536 
537  CppAD::cg::GccCompiler<Scalar> compiler(PINOCCHIO_CXX_COMPILER);
538  std::vector<std::string> compile_flags = compiler.getCompileFlags();
539  compile_flags[0] = compile_options;
540  compiler.setCompileFlags(compile_flags);
541  dynamicLibManager_ptr->createDynamicLibrary(compiler, false);
542 
543  const auto it = dynamicLibManager_ptr->getOptions().find("dlOpenMode");
544  if (it == dynamicLibManager_ptr->getOptions().end())
545  {
546  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
547  dynamicLibManager_ptr->getLibraryName()
548  + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
549  }
550  else
551  {
552  int dlOpenMode = std::stoi(it->second);
553  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
554  dynamicLibManager_ptr->getLibraryName()
555  + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION,
556  dlOpenMode));
557  }
558 
559  generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
560 
561  CPPAD_TESTVECTOR(Scalar) x((size_t)nv);
562  std::vector<size_t> xs;
563  for (size_t it = 0; it < NBT; ++it)
564  {
565  ConfigVectorType q = pinocchio::randomConfiguration(model);
566  TangentVectorType v(TangentVectorType::Random(nv));
567  TangentVectorType a(TangentVectorType::Random(nv));
568 
569  ConfigVectorType x = ConfigVectorType::Zero(nq + 2 * nv);
570  i = 0;
571  x.segment(i, nq) = q;
572  i += nq;
573  x.segment(i, nv) = v;
574  i += nv;
575  x.segment(i, nv) = a;
576  i += nv;
577 
578  xs.push_back(static_cast<size_t>(x.size()));
579  }
580 
581  CppAD::cg::ArrayView<Scalar> jac_(jac.data(), (size_t)jac.size());
582 
583  timer.tic();
584  SMOOTH(NBT)
585  {
586  generatedFun_ptr->Jacobian(CppAD::cg::ArrayView<const Scalar>(x.data(), xs[_smooth]), jac_);
587  }
588  std::cout << "Calculate dtau_dx (∂RNEA) using CppADCodeGen =\t";
589  timer.toc(std::cout, NBT);
590  }
591 
592  if (DELETE_GENERATED_LIBS_AFTER_TEST)
593  {
594  std::remove("cg_rnea_eval.dylib");
595  std::remove("jit_JSIM.c");
596  std::remove("jit_JSIM.so");
597  std::remove("jit_JSIM_crba.c");
598  std::remove("jit_JSIM_crba.so");
599  }
600 
601  std::cout << "--" << std::endl;
602  return 0;
603 }
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:88
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
cppadcg.hpp
codegen-rnea.jac
jac
Definition: codegen-rnea.py:41
aligned-vector.hpp
y
y
pinocchio::Convention::WORLD
@ WORLD
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
code-generator-algo.hpp
kinematics.hpp
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp: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::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::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::python::context::Model
ModelTpl< Scalar, Options > Model
Definition: bindings/python/context/generic.hpp:63
autodiff-rnea.f
f
Definition: autodiff-rnea.py:24
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
main
int main()
Definition: timings-cppad-jit.cpp:31
rnea.hpp
timer.hpp
pinocchio::context::RowMatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor|Options > RowMatrixXs
Definition: context/generic.hpp:51
collision-with-point-clouds.nx
int nx
Definition: collision-with-point-clouds.py:40
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::python::Options
@ Options
Definition: expose-contact-inverse-dynamics.cpp:22
codegen-rnea.nq
nq
Definition: codegen-rnea.py:18
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
joint-configuration.hpp
pinocchio::python::context::Data
DataTpl< Scalar, Options > Data
Definition: bindings/python/context/generic.hpp:64
DLL_EXT
#define DLL_EXT
Definition: timings-cppad-jit.cpp:24
pinocchio::ModelTpl::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: multibody/model.hpp:95
cartpole.v
v
Definition: cartpole.py:153
data.hpp
x
x
q
q
inverse-kinematics-3d.it
int it
Definition: inverse-kinematics-3d.py:17
autodiff-rnea.dtau_da
dtau_da
Definition: autodiff-rnea.py:29
a
Vec3f a
PinocchioTicToc::US
@ US
Definition: timer.hpp:53
fill
fill
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...
DELETE_GENERATED_LIBS_AFTER_TEST
bool DELETE_GENERATED_LIBS_AFTER_TEST
Definition: timings-cppad-jit.cpp:29
codegen-rnea.nv
nv
Definition: codegen-rnea.py:19
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: multibody/model.hpp:87
sample-models.hpp
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
crba.hpp
PinocchioTicToc
Definition: timer.hpp:47
meshcat-viewer.qs
qs
Definition: meshcat-viewer.py:128
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:82

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