22 #define DLL_EXT ".dll"
33 using namespace Eigen;
38 const int NBT = 1000 * 100;
41 std::cout <<
"(the time score in debug mode is not relevant) " << std::endl;
45 using CppAD::NearEqual;
52 typedef AD<Scalar> ADScalar;
53 typedef CppAD::cg::CG<Scalar> CGScalar;
54 typedef CppAD::AD<CGScalar> ADCGScalar;
55 typedef CppAD::ADFun<CGScalar> ADCGFun;
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;
74 typedef ADModel::ConfigVectorType ADConfigVectorType;
75 typedef ADModel::TangentVectorType ADTangentVectorType;
77 typedef ADCGModel::ConfigVectorType ADCGConfigVectorType;
78 typedef ADCGModel::TangentVectorType ADCGTangentVectorType;
85 model.lowerPositionLimit.head<3>().
fill(-1.);
86 model.upperPositionLimit.head<3>().
fill(1.);
88 ConfigVectorType
q(
nq);
90 TangentVectorType
v(TangentVectorType::Random(
nv));
91 TangentVectorType
a(TangentVectorType::Random(
nv));
96 std::vector<ConfigVectorType>
qs;
97 for (
size_t it = 0; it < NBT; ++it)
107 data.M.triangularView<Eigen::StrictlyLower>() =
108 data.M.transpose().triangularView<Eigen::StrictlyLower>();
110 std::cout <<
"Calculate data.M (JSIM) with crba = \t\t";
111 timer.
toc(std::cout, NBT);
115 ADModel ad_model =
model.cast<ADScalar>();
116 ADData ad_data(ad_model);
118 ADConfigVectorType ad_q =
q.cast<ADScalar>();
120 CppAD::Independent(ad_q);
122 ad_data.M.triangularView<Eigen::StrictlyLower>() =
123 ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
125 ADVectorXs M_vector =
126 Eigen::Map<ADVectorXs>(ad_data.M.data(), ad_data.M.cols() * ad_data.M.rows());
128 CppAD::ADFun<Scalar> ad_fun(ad_q, M_vector);
129 ad_fun.function_name_set(
"ad_fun_crba");
132 std::string c_type =
"double";
133 std::string csrc_file =
"./jit_JSIM_crba.c";
135 ofs.open(csrc_file, std::ofstream::out);
136 ad_fun.to_csrc(ofs, c_type);
140 std::string dll_file =
"./jit_JSIM_crba" DLL_EXT;
141 CPPAD_TESTVECTOR(std::string) csrc_files(1);
142 csrc_files[0] = csrc_file;
143 std::map<std::string, std::string> options;
144 std::string err_msg = CppAD::create_dll_lib(dll_file, csrc_files, options);
147 std::cerr <<
"jit_JSIM_crba: err_msg = " << err_msg <<
"\n";
151 CppAD::link_dll_lib dll_linker(dll_file, err_msg);
154 std::cerr <<
"jit_JSIM_crba: err_msg = " << err_msg <<
"\n";
157 std::string function_name =
"cppad_jit_ad_fun_crba";
158 void * void_ptr = dll_linker(function_name, err_msg);
161 std::cerr <<
"jit_JSIM_crba: err_msg = " << err_msg <<
"\n";
164 using CppAD::jit_double;
165 jit_double ad_fun_ptr =
reinterpret_cast<jit_double
>(void_ptr);
168 size_t compare_change = 0,
nx = (size_t)
nq, nM_vector = (
size_t)
nv * (size_t)
nv;
169 CPPAD_TESTVECTOR(
Scalar) M_vector_jit(nM_vector);
171 std::vector<CPPAD_TESTVECTOR(
Scalar)> xs;
172 for (
size_t it = 0; it < NBT; ++it)
175 Eigen::Map<Data::ConfigVectorType>(
x.data(),
nq, 1) =
q;
182 ad_fun_ptr(
nx, xs[_smooth].
data(), nM_vector, M_vector_jit.data(), &compare_change);
184 std::cout <<
"Calculate data.M (JSIM) with crba using cppad-jit = \t\t";
185 timer.
toc(std::cout, NBT);
189 ADCGModel ad_model(
model.template cast<ADCGScalar>());
190 ADCGData ad_data(ad_model);
192 ADCGConfigVectorType ad_q =
q.cast<ADCGScalar>();
193 ad_q =
q.cast<ADCGScalar>();
195 std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>> cgen_ptr;
196 std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>> libcgen_ptr;
197 std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>> dynamicLibManager_ptr;
198 std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib_ptr;
199 std::unique_ptr<CppAD::cg::GenericModel<Scalar>> generatedFun_ptr;
201 const std::string & function_name =
"crba";
202 const std::string & library_name =
"cg_crba_eval";
203 const std::string & compile_options =
"-Ofast";
205 CppAD::Independent(ad_q);
207 ad_data.M.triangularView<Eigen::StrictlyLower>() =
208 ad_data.M.transpose().triangularView<Eigen::StrictlyLower>();
210 ADCGVectorXs M_vector =
211 Eigen::Map<ADCGVectorXs>(ad_data.M.data(), ad_data.M.cols() * ad_data.M.rows());
214 ad_fun.Dependent(ad_q, M_vector);
215 ad_fun.optimize(
"no_compare_op");
218 cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>>(
219 new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun, function_name));
220 cgen_ptr->setCreateForwardZero(
true);
221 cgen_ptr->setCreateJacobian(
false);
222 libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>>(
223 new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
225 dynamicLibManager_ptr = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>>(
226 new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr, library_name));
228 CppAD::cg::GccCompiler<Scalar> compiler(PINOCCHIO_CXX_COMPILER);
229 std::vector<std::string> compile_flags = compiler.getCompileFlags();
230 compile_flags[0] = compile_options;
231 compiler.setCompileFlags(compile_flags);
232 dynamicLibManager_ptr->createDynamicLibrary(compiler,
false);
234 const auto it = dynamicLibManager_ptr->getOptions().find(
"dlOpenMode");
235 if (it == dynamicLibManager_ptr->getOptions().end())
237 dynamicLib_ptr.reset(
new CppAD::cg::LinuxDynamicLib<Scalar>(
238 dynamicLibManager_ptr->getLibraryName()
239 + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
243 int dlOpenMode = std::stoi(it->second);
244 dynamicLib_ptr.reset(
new CppAD::cg::LinuxDynamicLib<Scalar>(
245 dynamicLibManager_ptr->getLibraryName()
246 + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION,
250 generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
253 CPPAD_TESTVECTOR(
Scalar)
y((
size_t)
nv * (
size_t)
nv);
255 std::vector<CPPAD_TESTVECTOR(
Scalar)> xs;
256 for (
size_t it = 0; it < NBT; ++it)
259 Eigen::Map<Data::ConfigVectorType>(
x.data(),
nq, 1) =
q;
266 generatedFun_ptr->ForwardZero(xs[_smooth],
y);
268 std::cout <<
"Calculate data.M (JSIM) with crba using cppadcg = \t\t";
269 timer.
toc(std::cout, NBT);
275 std::vector<ConfigVectorType>
qs, vs, as;
276 for (
size_t it = 0; it < NBT; ++it)
279 TangentVectorType
v(TangentVectorType::Random(
nv));
280 TangentVectorType
a(TangentVectorType::Random(
nv));
291 std::cout <<
"Calculate dtau_dx using computeRNEADerivatives = \t\t";
292 timer.
toc(std::cout, NBT);
296 ADModel ad_model =
model.cast<ADScalar>();
297 ADData ad_data(ad_model);
299 ADConfigVectorType ad_q =
q.cast<ADScalar>();
300 ADTangentVectorType ad_v =
v.cast<ADScalar>();
301 ADTangentVectorType ad_a =
a.cast<ADScalar>();
302 ADConfigVectorType ad_X = ADConfigVectorType::Zero(
nx);
303 Eigen::DenseIndex
i = 0;
304 ad_X.segment(
i,
nq) = ad_q;
306 ad_X.segment(
i,
nv) = ad_v;
308 ad_X.segment(
i,
nv) = ad_a;
311 CppAD::Independent(ad_X);
313 ad_model, ad_data, ad_X.segment(0,
nq), ad_X.segment(
nq,
nv), ad_X.segment(
nq +
nv,
nv));
316 Eigen::Map<ADData::TangentVectorType>(ad_Y.data(),
nv, 1) = ad_data.tau;
318 CppAD::ADFun<Scalar>
f(ad_X, ad_Y);
319 CppAD::ADFun<ADScalar, Scalar> af =
f.base2ad();
321 CppAD::Independent(ad_X);
322 ADMatrixXs dtau_dx = af.Jacobian(ad_X);
323 ADVectorXs dtau_dx_vector(
nv *
nx);
324 dtau_dx_vector = Eigen::Map<ADVectorXs>(dtau_dx.data(), dtau_dx.cols() * dtau_dx.rows());
325 CppAD::ADFun<double> ad_fun(ad_X, dtau_dx_vector);
327 ad_fun.function_name_set(
"ad_fun");
330 std::string c_type =
"double";
331 std::string csrc_file =
"./jit_JSIM.c";
333 ofs.open(csrc_file, std::ofstream::out);
334 ad_fun.to_csrc(ofs, c_type);
338 std::string dll_file =
"./jit_JSIM" DLL_EXT;
339 CPPAD_TESTVECTOR(std::string) csrc_files(1);
340 csrc_files[0] = csrc_file;
341 std::map<std::string, std::string> options;
342 std::string err_msg = CppAD::create_dll_lib(dll_file, csrc_files, options);
345 std::cerr <<
"jit_JSIM: err_msg = " << err_msg <<
"\n";
349 CppAD::link_dll_lib dll_linker(dll_file, err_msg);
352 std::cerr <<
"jit_JSIM: err_msg = " << err_msg <<
"\n";
355 std::string function_name =
"cppad_jit_ad_fun";
356 void * void_ptr = dll_linker(function_name, err_msg);
359 std::cerr <<
"jit_JSIM: err_msg = " << err_msg <<
"\n";
362 using CppAD::jit_double;
363 jit_double ad_fun_ptr =
reinterpret_cast<jit_double
>(void_ptr);
366 std::vector<CPPAD_TESTVECTOR(
Scalar)> xs;
367 for (
size_t it = 0; it < NBT; ++it)
370 TangentVectorType
v(TangentVectorType::Random(
nv));
371 TangentVectorType
a(TangentVectorType::Random(
nv));
372 ConfigVectorType _x = ConfigVectorType::Zero(
nx);
373 Eigen::DenseIndex
i = 0;
374 _x.segment(
i,
nq) =
q;
376 _x.segment(
i,
nv) =
v;
378 _x.segment(
i,
nv) =
a;
381 Eigen::Map<Data::TangentVectorType>(
x.data(),
nx, 1) = _x;
385 size_t compare_change = 0, nx_ = (size_t)
nx, ndtau_dx = (
size_t)
nv * (size_t)
nx;
386 std::vector<double> dtau_dx_jit(ndtau_dx);
391 ad_fun_ptr(nx_, xs[_smooth].
data(), ndtau_dx, dtau_dx_jit.data(), &compare_change);
393 std::cout <<
"Calculate dtau_dx (rnea) with cppad using jit = \t\t";
394 timer.
toc(std::cout, NBT);
398 ADModel ad_model =
model.cast<ADScalar>();
399 ADData ad_data(ad_model);
401 ADConfigVectorType ad_q =
q.cast<ADScalar>();
402 ADTangentVectorType ad_v =
v.cast<ADScalar>();
403 ADTangentVectorType ad_a =
a.cast<ADScalar>();
405 ADConfigVectorType ad_X = ADConfigVectorType::Zero(
nq + 2 *
nv);
406 Eigen::DenseIndex
i = 0;
407 ad_X.segment(
i,
nq) = ad_q;
409 ad_X.segment(
i,
nv) = ad_v;
411 ad_X.segment(
i,
nv) = ad_a;
414 CppAD::Independent(ad_X);
416 ad_model, ad_data, ad_X.segment(0,
nq), ad_X.segment(
nq,
nv), ad_X.segment(
nq +
nv,
nv));
419 Eigen::Map<ADData::TangentVectorType>(ad_Y.data(),
nv, 1) = ad_data.tau;
421 CppAD::ADFun<Scalar> ad_fun(ad_X, ad_Y);
424 std::vector<CPPAD_TESTVECTOR(
Scalar)> xs;
425 for (
size_t it = 0; it < NBT; ++it)
428 TangentVectorType
v(TangentVectorType::Random(
nv));
429 TangentVectorType
a(TangentVectorType::Random(
nv));
430 ConfigVectorType _x = ConfigVectorType::Zero(
nq + 2 *
nv);
431 Eigen::DenseIndex
i = 0;
432 _x.segment(
i,
nq) =
q;
434 _x.segment(
i,
nv) =
v;
436 _x.segment(
i,
nv) =
a;
439 Eigen::Map<Data::TangentVectorType>(
x.data(),
nx, 1) = _x;
445 CPPAD_TESTVECTOR(
Scalar)
dtau_da = ad_fun.Jacobian(xs[_smooth]);
447 std::cout <<
"Calculate dtau_dx (rnea) with cppad = \t\t";
448 timer.
toc(std::cout, NBT);
452 ADCGModel ad_model(
model.template cast<ADCGScalar>());
453 ADCGData ad_data(ad_model);
455 ADCGVectorXs ad_Y(
nv);
457 ADCGConfigVectorType ad_q =
q.cast<ADCGScalar>();
458 ADCGTangentVectorType ad_v =
v.cast<ADCGScalar>();
459 ADCGTangentVectorType ad_a =
a.cast<ADCGScalar>();
460 ADCGConfigVectorType ad_X = ADCGConfigVectorType::Zero(
nx);
461 Eigen::DenseIndex
i = 0;
462 ad_X.segment(
i,
nq) = ad_q;
464 ad_X.segment(
i,
nv) = ad_v;
466 ad_X.segment(
i,
nv) = ad_a;
471 std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>> cgen_ptr;
472 std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>> libcgen_ptr;
473 std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>> dynamicLibManager_ptr;
474 std::unique_ptr<CppAD::cg::DynamicLib<Scalar>> dynamicLib_ptr;
475 std::unique_ptr<CppAD::cg::GenericModel<Scalar>> generatedFun_ptr;
477 const std::string & function_name =
"rnea";
478 const std::string & library_name =
"cg_rnea_eval";
479 const std::string & compile_options =
"-Ofast";
481 CppAD::Independent(ad_X);
483 ad_model, ad_data, ad_X.segment(0,
nq), ad_X.segment(
nq,
nv), ad_X.segment(
nq +
nv,
nv));
485 ad_fun.Dependent(ad_X, ad_Y);
486 ad_fun.optimize(
"no_compare_op");
490 cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar>>(
491 new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun, function_name));
492 cgen_ptr->setCreateForwardZero(
true);
493 cgen_ptr->setCreateJacobian(
true);
494 libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar>>(
495 new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
497 dynamicLibManager_ptr = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar>>(
498 new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr, library_name));
500 CppAD::cg::GccCompiler<Scalar> compiler(PINOCCHIO_CXX_COMPILER);
501 std::vector<std::string> compile_flags = compiler.getCompileFlags();
502 compile_flags[0] = compile_options;
503 compiler.setCompileFlags(compile_flags);
504 dynamicLibManager_ptr->createDynamicLibrary(compiler,
false);
506 const auto it = dynamicLibManager_ptr->getOptions().find(
"dlOpenMode");
507 if (it == dynamicLibManager_ptr->getOptions().end())
509 dynamicLib_ptr.reset(
new CppAD::cg::LinuxDynamicLib<Scalar>(
510 dynamicLibManager_ptr->getLibraryName()
511 + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
515 int dlOpenMode = std::stoi(it->second);
516 dynamicLib_ptr.reset(
new CppAD::cg::LinuxDynamicLib<Scalar>(
517 dynamicLibManager_ptr->getLibraryName()
518 + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION,
522 generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
525 std::vector<size_t> xs;
526 for (
size_t it = 0; it < NBT; ++it)
529 TangentVectorType
v(TangentVectorType::Random(
nv));
530 TangentVectorType
a(TangentVectorType::Random(
nv));
532 ConfigVectorType
x = ConfigVectorType::Zero(
nq + 2 *
nv);
534 x.segment(
i,
nq) =
q;
536 x.segment(
i,
nv) =
v;
538 x.segment(
i,
nv) =
a;
541 xs.push_back(
static_cast<size_t>(
x.size()));
544 CppAD::cg::ArrayView<Scalar> jac_(
jac.data(), (
size_t)
jac.size());
549 generatedFun_ptr->Jacobian(CppAD::cg::ArrayView<const Scalar>(
x.data(), xs[_smooth]), jac_);
551 std::cout <<
"Calculate dtau_dx (rnea) with cppad code gen = \t\t";
552 timer.
toc(std::cout, NBT);
557 std::remove(
"cg_rnea_eval.dylib");
558 std::remove(
"jit_JSIM.c");
559 std::remove(
"jit_JSIM.so");
560 std::remove(
"jit_JSIM_crba.c");
561 std::remove(
"jit_JSIM_crba.so");
564 std::cout <<
"--" << std::endl;