pinocchio  2.3.1-dirty
code-generator-base.hpp
1 //
2 // Copyright (c) 2018 CNRS
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 #ifdef PINOCCHIO_WITH_CPPADCG_SUPPORT
14 
15 namespace pinocchio
16 {
17 
18  template<typename _Scalar>
19  struct CodeGenBase
20  {
21  typedef _Scalar Scalar;
22  typedef CppAD::cg::CG<Scalar> CGScalar;
23  typedef CppAD::AD<CGScalar> ADScalar;
24 
25  enum { Options = 0 };
26 
33 
34  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
35  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
36  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options|Eigen::RowMajor> RowMatrixXs;
37  typedef Eigen::Matrix<ADScalar,Eigen::Dynamic,1,Options> ADVectorXs;
38 
39  typedef typename Model::ConfigVectorType ConfigVectorType;
40  typedef typename Model::TangentVectorType TangentVectorType;
41 
42  typedef typename ADModel::ConfigVectorType ADConfigVectorType;
43  typedef typename ADModel::TangentVectorType ADTangentVectorType;
44 
45  typedef CppAD::ADFun<CGScalar> ADFun;
46 
47  CodeGenBase(const Model & model,
48  const Eigen::DenseIndex dim_input,
49  const Eigen::DenseIndex dim_output,
50  const std::string & function_name,
51  const std::string & library_name)
52  : ad_model(model.template cast<ADScalar>())
53  , ad_data(ad_model)
54  , function_name(function_name)
55  , library_name(library_name + "_" + model.name)
56  , build_forward(true)
57  , build_jacobian(true)
58  {
59  ad_X = ADVectorXs(dim_input);
60  ad_Y = ADVectorXs(dim_output);
61 
62  y = VectorXs(ad_Y.size());
63 
64  jac = RowMatrixXs(ad_Y.size(),ad_X.size());
65  }
66 
68  virtual void buildMap() = 0;
69 
70  void initLib()
71  {
72  buildMap();
73 
74  // generates source code
75  cgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(new CppAD::cg::ModelCSourceGen<Scalar>(ad_fun, function_name));
76  cgen_ptr->setCreateForwardZero(build_forward);
77  cgen_ptr->setCreateJacobian(build_jacobian);
78  libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> >(new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*cgen_ptr));
79 
80  dynamicLibManager_ptr
81  = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> >(new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr,library_name));
82  }
83 
84  CppAD::cg::ModelCSourceGen<Scalar> & codeGenerator()
85  { return *cgen_ptr; }
86 
87  void compileLib()
88  {
89  CppAD::cg::GccCompiler<Scalar> compiler;
90  std::vector<std::string> compile_options = compiler.getCompileFlags();
91  compile_options[0] = "-Ofast";
92  compiler.setCompileFlags(compile_options);
93  dynamicLibManager_ptr->createDynamicLibrary(compiler,false);
94  }
95 
96  bool existLib() const
97  {
98  const std::string filename = dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION;
99  std::ifstream file(filename.c_str());
100  return file.good();
101  }
102 
103  void loadLib(const bool generate_if_not_exist = true)
104  {
105  if(not existLib() && generate_if_not_exist)
106  compileLib();
107 
108  const auto it = dynamicLibManager_ptr->getOptions().find("dlOpenMode");
109  if (it == dynamicLibManager_ptr->getOptions().end())
110  {
111  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
112  }
113  else
114  {
115  int dlOpenMode = std::stoi(it->second);
116  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION, dlOpenMode));
117  }
118 
119  generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
120  }
121 
122  template<typename Vector>
123  void evalFunction(const Eigen::MatrixBase<Vector> & x)
124  {
125  assert(build_forward);
126 
127  generatedFun_ptr->ForwardZero(PINOCCHIO_EIGEN_CONST_CAST(Vector,x),y);
128  }
129 
130  template<typename Vector>
131  void evalJacobian(const Eigen::MatrixBase<Vector> & x)
132  {
133  assert(build_jacobian);
134 
135  CppAD::cg::ArrayView<const Scalar> x_(PINOCCHIO_EIGEN_CONST_CAST(Vector,x).data(),(size_t)x.size());
136  CppAD::cg::ArrayView<Scalar> jac_(jac.data(),(size_t)jac.size());
137  generatedFun_ptr->Jacobian(x_,jac_);
138  }
139 
141  Eigen::DenseIndex getInputDimension() const { return ad_X.size(); }
143  Eigen::DenseIndex getOutputDimension() const { return ad_Y.size(); }
144 
145  protected:
146 
147  ADModel ad_model;
148  ADData ad_data;
149 
151  const std::string function_name;
153  const std::string library_name;
154 
157 
160 
161  ADVectorXs ad_X, ad_Y;
162  ADFun ad_fun;
163 
164  ADConfigVectorType ad_q, ad_q_plus;
165  ADTangentVectorType ad_dq, ad_v, ad_a;
166 
167  VectorXs y;
168  RowMatrixXs jac;
169 
170  std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> > cgen_ptr;
171  std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> > libcgen_ptr;
172  std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> > dynamicLibManager_ptr;
173  std::unique_ptr<CppAD::cg::DynamicLib<Scalar> > dynamicLib_ptr;
174  std::unique_ptr<CppAD::cg::GenericModel<Scalar> > generatedFun_ptr;
175 
176  }; // struct CodeGenBase
177 
178 } // namespace pinocchio
179 
180 #endif // PINOCCHIO_WITH_CPPADCG_SUPPORT
181 
182 #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.
Definition: treeview.dox:24
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).