pinocchio  2.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
code-generator-base.hpp
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 
38  typedef typename Model::ConfigVectorType ConfigVectorType;
39  typedef typename Model::TangentVectorType TangentVectorType;
40 
41  typedef typename ADModel::ConfigVectorType ADConfigVectorType;
42  typedef typename ADModel::TangentVectorType ADTangentVectorType;
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 
79  dynamicLibManager_ptr
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(not 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 
118  generatedFun_ptr = dynamicLib_ptr->model(function_name.c_str());
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__
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.
Definition: model.hpp:66
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;.
Definition: model.hpp:157
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).
Definition: model.hpp:72