code-generator-base.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_utils_code_generator_base_hpp__
6 #define __pinocchio_utils_code_generator_base_hpp__
7 
8 #include "pinocchio/codegen/cppadcg.hpp"
9 
10 #include "pinocchio/multibody/model.hpp"
11 #include "pinocchio/multibody/data.hpp"
12 
13 namespace pinocchio
14 {
15 
16  template<typename _Scalar>
17  struct CodeGenBase
18  {
19  typedef _Scalar Scalar;
20  typedef CppAD::cg::CG<Scalar> CGScalar;
21  typedef CppAD::AD<CGScalar> ADScalar;
22 
23  enum { Options = 0 };
24 
31 
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;
37 
40 
43 
44  typedef CppAD::ADFun<CGScalar> ADFun;
45 
46  CodeGenBase(const Model & model,
47  const Eigen::DenseIndex dim_input,
48  const Eigen::DenseIndex dim_output,
49  const std::string & function_name,
50  const std::string & library_name)
51  : ad_model(model.template cast<ADScalar>())
52  , ad_data(ad_model)
53  , function_name(function_name)
54  , library_name(library_name + "_" + model.name)
55  , build_forward(true)
56  , build_jacobian(true)
57  {
58  ad_X = ADVectorXs(dim_input);
59  ad_Y = ADVectorXs(dim_output);
60 
61  y = VectorXs(ad_Y.size());
62 
63  jac = RowMatrixXs(ad_Y.size(),ad_X.size());
64  }
65 
67  virtual void buildMap() = 0;
68 
69  void initLib()
70  {
71  buildMap();
72 
73  // generates source code
74  cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun, function_name));
75  cgen_ptr->setCreateForwardZero(build_forward);
76  cgen_ptr->setCreateJacobian(build_jacobian);
77  libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> >(new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
78 
80  = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> >(new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr,library_name));
81  }
82 
83  CppAD::cg::ModelCSourceGen<Scalar> & codeGenerator()
84  { return *cgen_ptr; }
85 
86  void compileLib()
87  {
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);
93  }
94 
95  bool existLib() const
96  {
97  const std::string filename = dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION;
98  std::ifstream file(filename.c_str());
99  return file.good();
100  }
101 
102  void loadLib(const bool generate_if_not_exist = true)
103  {
104  if(!existLib() && generate_if_not_exist)
105  compileLib();
106 
107  const auto it = dynamicLibManager_ptr->getOptions().find("dlOpenMode");
108  if (it == dynamicLibManager_ptr->getOptions().end())
109  {
110  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
111  }
112  else
113  {
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));
116  }
117 
119  }
120 
121  template<typename Vector>
122  void evalFunction(const Eigen::MatrixBase<Vector> & x)
123  {
124  assert(build_forward);
125 
126  generatedFun_ptr->ForwardZero(PINOCCHIO_EIGEN_CONST_CAST(Vector,x),y);
127  }
128 
129  template<typename Vector>
130  void evalJacobian(const Eigen::MatrixBase<Vector> & x)
131  {
132  assert(build_jacobian);
133 
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_);
137  }
138 
140  Eigen::DenseIndex getInputDimension() const { return ad_X.size(); }
142  Eigen::DenseIndex getOutputDimension() const { return ad_Y.size(); }
143 
144  protected:
145 
146  ADModel ad_model;
147  ADData ad_data;
148 
150  const std::string function_name;
152  const std::string library_name;
153 
156 
159 
160  ADVectorXs ad_X, ad_Y;
161  ADFun ad_fun;
162 
163  ADConfigVectorType ad_q, ad_q_plus;
164  ADTangentVectorType ad_dq, ad_v, ad_a;
165 
166  VectorXs y;
167  RowMatrixXs jac;
168 
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;
174 
175  }; // struct CodeGenBase
176 
177 } // namespace pinocchio
178 
179 #endif // ifndef __pinocchio_utils_code_generator_base_hpp__
void evalJacobian(const Eigen::MatrixBase< Vector > &x)
CppAD::ADFun< CGScalar > ADFun
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Eigen::DenseIndex getInputDimension() const
Dimension of the input vector.
Model::ConfigVectorType ConfigVectorType
Model::TangentVectorType TangentVectorType
ADModel::ConfigVectorType ADConfigVectorType
NewScalar cast(const Scalar &value)
Definition: cast.hpp:13
std::unique_ptr< CppAD::cg::DynamicLib< Scalar > > dynamicLib_ptr
pinocchio::DataTpl< ADScalar, Options > ADData
const std::string library_name
Name of the library.
std::unique_ptr< CppAD::cg::GenericModel< Scalar > > generatedFun_ptr
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
const std::string function_name
Name of the function.
Eigen::Matrix< ADScalar, Eigen::Dynamic, 1, Options > ADVectorXs
bool build_forward
Options to generate or not the source code for the evaluation function.
pinocchio::ModelTpl< Scalar, Options > Model
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).
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
CppAD::AD< CGScalar > ADScalar
virtual void buildMap()=0
build the mapping Y = f(X)
CppAD::cg::ModelCSourceGen< Scalar > & codeGenerator()
void loadLib(const bool generate_if_not_exist=true)
pinocchio::DataTpl< Scalar, Options > Data
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
std::unique_ptr< CppAD::cg::ModelLibraryCSourceGen< Scalar > > libcgen_ptr
std::unique_ptr< CppAD::cg::ModelCSourceGen< Scalar > > cgen_ptr
pinocchio::ModelTpl< ADScalar, Options > ADModel
CodeGenBase(const Model &model, const Eigen::DenseIndex dim_input, const Eigen::DenseIndex dim_output, const std::string &function_name, const std::string &library_name)
void evalFunction(const Eigen::MatrixBase< Vector > &x)
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::ModelTpl< CGScalar, Options > CGModel
pinocchio::DataTpl< CGScalar, Options > CGData
filename
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
ADModel::TangentVectorType ADTangentVectorType
Eigen::Matrix< ADScalar, Eigen::Dynamic, Eigen::Dynamic, Options > ADMatrixXs
std::unique_ptr< CppAD::cg::DynamicModelLibraryProcessor< Scalar > > dynamicLibManager_ptr
Eigen::DenseIndex getOutputDimension() const
Dimension of the output vector.
CppAD::cg::CG< Scalar > CGScalar
JointCollectionTpl & model
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options|Eigen::RowMajor > RowMatrixXs
ADConfigVectorType ad_q_plus
bool build_jacobian
Options to build or not the Jacobian of he function.


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