5 #ifndef __pinocchio_utils_code_generator_base_hpp__ 6 #define __pinocchio_utils_code_generator_base_hpp__ 8 #include "pinocchio/codegen/cppadcg.hpp" 10 #include "pinocchio/multibody/model.hpp" 11 #include "pinocchio/multibody/data.hpp" 16 template<
typename _Scalar>
19 typedef _Scalar Scalar;
20 typedef CppAD::cg::CG<Scalar> CGScalar;
21 typedef CppAD::AD<CGScalar> ADScalar;
32 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
33 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
34 typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options|Eigen::RowMajor> RowMatrixXs;
35 typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1,Options> ADVectorXs;
36 typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,Eigen::Dynamic,Options> ADMatrixXs;
44 typedef CppAD::ADFun<CGScalar> ADFun;
47 const Eigen::DenseIndex dim_input,
48 const Eigen::DenseIndex dim_output,
51 : ad_model(model.template cast<ADScalar>())
58 ad_X = ADVectorXs(dim_input);
59 ad_Y = ADVectorXs(dim_output);
61 y = VectorXs(ad_Y.size());
63 jac = RowMatrixXs(ad_Y.size(),ad_X.size());
74 cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(
new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun,
function_name));
77 libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> >(
new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
80 = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> >(
new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr,
library_name));
83 CppAD::cg::ModelCSourceGen<Scalar> & codeGenerator()
88 CppAD::cg::GccCompiler<Scalar> compiler;
89 std::vector<std::string> compile_options = compiler.getCompileFlags();
90 compile_options[0] =
"-Ofast";
91 compiler.setCompileFlags(compile_options);
92 dynamicLibManager_ptr->createDynamicLibrary(compiler,
false);
97 const std::string filename = dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION;
98 std::ifstream file(filename.c_str());
102 void loadLib(
const bool generate_if_not_exist =
true)
104 if(not existLib() && generate_if_not_exist)
107 const auto it = dynamicLibManager_ptr->getOptions().find(
"dlOpenMode");
108 if (it == dynamicLibManager_ptr->getOptions().end())
110 dynamicLib_ptr.reset(
new CppAD::cg::LinuxDynamicLib<Scalar>(dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
114 int dlOpenMode = std::stoi(it->second);
115 dynamicLib_ptr.reset(
new CppAD::cg::LinuxDynamicLib<Scalar>(dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION, dlOpenMode));
118 generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
121 template<
typename Vector>
122 void evalFunction(
const Eigen::MatrixBase<Vector> & x)
126 generatedFun_ptr->ForwardZero(PINOCCHIO_EIGEN_CONST_CAST(Vector,x),y);
129 template<
typename Vector>
130 void evalJacobian(
const Eigen::MatrixBase<Vector> & x)
134 CppAD::cg::ArrayView<const Scalar> x_(PINOCCHIO_EIGEN_CONST_CAST(Vector,x).data(),(
size_t)x.size());
135 CppAD::cg::ArrayView<Scalar> jac_(jac.data(),(size_t)jac.size());
136 generatedFun_ptr->Jacobian(x_,jac_);
160 ADVectorXs ad_X, ad_Y;
163 ADConfigVectorType ad_q, ad_q_plus;
164 ADTangentVectorType ad_dq, ad_v, ad_a;
169 std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> > cgen_ptr;
170 std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> > libcgen_ptr;
171 std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> > dynamicLibManager_ptr;
172 std::unique_ptr<CppAD::cg::DynamicLib<Scalar> > dynamicLib_ptr;
173 std::unique_ptr<CppAD::cg::GenericModel<Scalar> > generatedFun_ptr;
179 #endif // ifndef __pinocchio_utils_code_generator_base_hpp__ bool build_forward
Options to generate or not the source code for the evaluation function.
const std::string function_name
Name of the function.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
const std::string library_name
Name of the library.
bool build_jacobian
Options to build or not the Jacobian of he function.
Eigen::DenseIndex getOutputDimension() const
Dimension of the output vector.
virtual void buildMap()=0
build the mapping Y = f(X)
Eigen::DenseIndex getInputDimension() const
Dimension of the input vector.
Main pinocchio namespace.
std::string name
Model name;.
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). It also handles the notion of co-tangent vector (e.g. torque, etc).