crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
action-base.hpp
1 
3 // BSD 3-Clause License
4 //
5 // Copyright (C) 2019-2020, LAAS-CNRS, INRIA, University of Edinburgh
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_CORE_CODEGEN_ACTION_BASE_HPP_
11 #define CROCODDYL_CORE_CODEGEN_ACTION_BASE_HPP_
12 
13 #include <functional>
14 #include "pinocchio/codegen/cppadcg.hpp"
15 
16 #include "crocoddyl/core/action-base.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename Scalar>
21 struct ActionDataCodeGenTpl;
22 
23 template <typename _Scalar>
25  public:
26  typedef _Scalar Scalar;
30  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
31  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
32 
33  typedef CppAD::cg::CG<Scalar> CGScalar;
34  typedef CppAD::AD<CGScalar> ADScalar;
38  typedef typename MathBaseTpl<ADScalar>::VectorXs ADVectorXs;
39  typedef typename MathBaseTpl<ADScalar>::MatrixXs ADMatrixXs;
40  typedef typename MathBaseTpl<ADScalar>::Vector3s ADVector3s;
41  typedef typename MathBaseTpl<ADScalar>::Matrix3s ADMatrix3s;
42 
43  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
44 
45  typedef CppAD::ADFun<CGScalar> ADFun;
46 
47  ActionModelCodeGenTpl(boost::shared_ptr<ADBase> admodel, boost::shared_ptr<Base> model,
48  const std::string& library_name, const std::size_t n_env = 0,
49  std::function<void(boost::shared_ptr<ADBase>, const Eigen::Ref<const ADVectorXs>&)>
50  fn_record_env = empty_record_env,
51  const std::string& function_name_calc = "calc",
52  const std::string& function_name_calcDiff = "calcDiff")
53  : Base(model->get_state(), model->get_nu()),
54  model(model),
55  ad_model(admodel),
56  ad_data(ad_model->createData()),
58  function_name_calcDiff(function_name_calcDiff),
60  n_env(n_env),
62  ad_X(ad_model->get_state()->get_nx() + ad_model->get_nu() + n_env),
63  ad_X2(ad_model->get_state()->get_nx() + ad_model->get_nu() + n_env),
64  ad_calcout(ad_model->get_state()->get_nx() + 1) {
65  const std::size_t ndx = ad_model->get_state()->get_ndx();
66  const std::size_t nu = ad_model->get_nu();
67  ad_calcDiffout.resize(2 * ndx * ndx + 2 * ndx * nu + nu * nu + ndx + nu);
68  initLib();
69  loadLib();
70  }
71 
72  static void empty_record_env(boost::shared_ptr<ADBase>, const Eigen::Ref<const ADVectorXs>&) {}
73 
74  void recordCalc() {
75  CppAD::Independent(ad_X);
76  const std::size_t nx = ad_model->get_state()->get_nx();
77  const std::size_t nu = ad_model->get_nu();
78 
79  fn_record_env(ad_model, ad_X.tail(n_env));
80 
81  ad_model->calc(ad_data, ad_X.head(nx), ad_X.segment(nx, nu));
82  collect_calcout();
83  // ad_calcout.template head<1>()[0] = ad_data->cost;
84  // ad_calcout.tail(ad_model->get_state()->get_nx()) = ad_data->xnext;
85  ad_calc.Dependent(ad_X, ad_calcout);
86  ad_calc.optimize("no_compare_op");
87  }
88 
89  void collect_calcout() {
90  ad_calcout[0] = ad_data->cost;
91  ad_calcout.tail(ad_model->get_state()->get_nx()) = ad_data->xnext;
92  }
93 
94  void collect_calcDiffout() {
95  ADVectorXs& ad_Y = ad_calcDiffout;
96 
97  const std::size_t ndx = ad_model->get_state()->get_ndx();
98  const std::size_t nu = ad_model->get_nu();
99  Eigen::DenseIndex it_Y = 0;
100  Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, ndx) = ad_data->Fx;
101  it_Y += ndx * ndx;
102  Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, nu) = ad_data->Fu;
103  it_Y += ndx * nu;
104  Eigen::Map<ADVectorXs>(ad_Y.data() + it_Y, ndx) = ad_data->Lx;
105  it_Y += ndx;
106  Eigen::Map<ADVectorXs>(ad_Y.data() + it_Y, nu) = ad_data->Lu;
107  it_Y += nu;
108  Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, ndx) = ad_data->Lxx;
109  it_Y += ndx * ndx;
110  Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, nu) = ad_data->Lxu;
111  it_Y += ndx * nu;
112  Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, nu, nu) = ad_data->Luu;
113  }
114 
115  void recordCalcDiff() {
116  CppAD::Independent(ad_X2);
117  const std::size_t nx = ad_model->get_state()->get_nx();
118  const std::size_t nu = ad_model->get_nu();
119 
120  fn_record_env(ad_model, ad_X2.tail(n_env));
121 
122  ad_model->calc(ad_data, ad_X2.head(nx), ad_X2.segment(nx, nu));
123  ad_model->calcDiff(ad_data, ad_X2.head(nx), ad_X2.segment(nx, nu));
124 
125  collect_calcDiffout();
126  ad_calcDiff.Dependent(ad_X2, ad_calcDiffout);
127  ad_calcDiff.optimize("no_compare_op");
128  }
129 
130  void initLib() {
131  recordCalc();
132 
133  // generates source code
134  calcgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(
135  new CppAD::cg::ModelCSourceGen<Scalar>(ad_calc, function_name_calc));
136  calcgen_ptr->setCreateForwardZero(true);
137  calcgen_ptr->setCreateJacobian(false);
138 
139  // generates source code
140  recordCalcDiff();
141  calcDiffgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(
142  new CppAD::cg::ModelCSourceGen<Scalar>(ad_calcDiff, function_name_calcDiff));
143  calcDiffgen_ptr->setCreateForwardZero(true);
144  calcDiffgen_ptr->setCreateJacobian(false);
145 
146  libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> >(
147  new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*calcgen_ptr, *calcDiffgen_ptr));
148 
149  dynamicLibManager_ptr = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> >(
150  new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr, library_name));
151  }
152 
153  void compileLib() {
154  CppAD::cg::GccCompiler<Scalar> compiler;
155  std::vector<std::string> compile_options = compiler.getCompileFlags();
156  compile_options[0] = "-O3";
157  compiler.setCompileFlags(compile_options);
158  dynamicLibManager_ptr->createDynamicLibrary(compiler, false);
159  }
160 
161  bool existLib() const {
162  const std::string filename =
163  dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION;
164  std::ifstream file(filename.c_str());
165  return file.good();
166  }
167 
168  void loadLib(const bool generate_if_not_exist = true) {
169  if (not existLib() && generate_if_not_exist) compileLib();
170 
171  const auto it = dynamicLibManager_ptr->getOptions().find("dlOpenMode");
172  if (it == dynamicLibManager_ptr->getOptions().end()) {
173  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
174  dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
175  } else {
176  int dlOpenMode = std::stoi(it->second);
177  dynamicLib_ptr.reset(new CppAD::cg::LinuxDynamicLib<Scalar>(
178  dynamicLibManager_ptr->getLibraryName() + CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION,
179  dlOpenMode));
180  }
181 
182  calcFun_ptr = dynamicLib_ptr->model(function_name_calc.c_str());
183  calcDiffFun_ptr = dynamicLib_ptr->model(function_name_calcDiff.c_str());
184  }
185 
186  void set_env(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& env_val) const {
187  Data* d = static_cast<Data*>(data.get());
188  d->xu.tail(n_env) = env_val;
189  }
190 
191  void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
192  const Eigen::Ref<const VectorXs>& u) {
193  Data* d = static_cast<Data*>(data.get());
194  const std::size_t nx = ad_model->get_state()->get_nx();
195  const std::size_t nu = ad_model->get_nu();
196 
197  d->xu.head(nx) = x;
198  d->xu.segment(nx, nu) = u;
199 
200  calcFun_ptr->ForwardZero(d->xu, d->calcout);
201  d->distribute_calcout();
202  }
203 
204  void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
205  const Eigen::Ref<const VectorXs>& u) {
206  Data* d = static_cast<Data*>(data.get());
207  const std::size_t nx = ad_model->get_state()->get_nx();
208  const std::size_t nu = ad_model->get_nu();
209 
210  d->xu.head(nx) = x;
211  d->xu.segment(nx, nu) = u;
212  calcDiffFun_ptr->ForwardZero(d->xu, d->calcDiffout);
213  d->distribute_calcDiffout();
214  }
215 
216  boost::shared_ptr<ActionDataAbstract> createData() {
217  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
218  }
219 
221  Eigen::DenseIndex getInputDimension() const { return ad_X.size(); }
222 
223  protected:
225  using Base::nr_;
226  using Base::nu_;
227  using Base::state_;
228  using Base::u_lb_;
229  using Base::u_ub_;
230  using Base::unone_;
231 
232  boost::shared_ptr<Base> model;
233  boost::shared_ptr<ADBase> ad_model;
234  boost::shared_ptr<ADActionDataAbstract> ad_data;
235 
237  const std::string function_name_calc, function_name_calcDiff;
238 
240  const std::string library_name;
241 
243  const std::size_t n_env;
244 
246  std::function<void(boost::shared_ptr<ADBase>, const Eigen::Ref<const ADVectorXs>&)> fn_record_env;
247 
250 
251  ADVectorXs ad_X, ad_X2;
252 
253  ADVectorXs ad_calcout;
254  ADVectorXs ad_calcDiffout;
255 
256  ADFun ad_calc, ad_calcDiff;
257 
258  std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> > calcgen_ptr, calcDiffgen_ptr;
259  std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> > libcgen_ptr;
260  std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> > dynamicLibManager_ptr;
261  std::unique_ptr<CppAD::cg::DynamicLib<Scalar> > dynamicLib_ptr;
262  std::unique_ptr<CppAD::cg::GenericModel<Scalar> > calcFun_ptr, calcDiffFun_ptr;
263 
264 }; // struct CodeGenBase
265 
266 template <typename _Scalar>
267 struct ActionDataCodeGenTpl : public ActionDataAbstractTpl<_Scalar> {
268  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
269 
270  typedef _Scalar Scalar;
273  typedef typename MathBase::VectorXs VectorXs;
274  typedef typename MathBase::MatrixXs MatrixXs;
275 
276  using Base::cost;
277  using Base::Fu;
278  using Base::Fx;
279  using Base::Lu;
280  using Base::Luu;
281  using Base::Lx;
282  using Base::Lxu;
283  using Base::Lxx;
284  using Base::r;
285  using Base::xnext;
286 
287  VectorXs xu, calcout;
288 
289  VectorXs calcDiffout;
290 
291  void distribute_calcout() {
292  cost = calcout[0];
293  xnext = calcout.tail(xnext.size());
294  }
295 
296  void distribute_calcDiffout() {
297  VectorXs& Y = calcDiffout;
298  const std::size_t ndx = Fx.rows();
299  const std::size_t nu = Fu.cols();
300 
301  Eigen::DenseIndex it_Y = 0;
302  Fx = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, ndx);
303  it_Y += ndx * ndx;
304  Fu = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, nu);
305  it_Y += ndx * nu;
306  Lx = Eigen::Map<VectorXs>(Y.data() + it_Y, ndx);
307  it_Y += ndx;
308  Lu = Eigen::Map<VectorXs>(Y.data() + it_Y, nu);
309  it_Y += nu;
310  Lxx = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, ndx);
311  it_Y += ndx * ndx;
312  Lxu = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, nu);
313  it_Y += ndx * nu;
314  Luu = Eigen::Map<MatrixXs>(Y.data() + it_Y, nu, nu);
315  }
316 
317  template <template <typename Scalar> class Model>
318  explicit ActionDataCodeGenTpl(Model<Scalar>* const model) : Base(model), calcout(model->get_state()->get_nx() + 1) {
320  xu.resize(m->getInputDimension());
321  xu.setZero();
322  calcout.setZero();
323  const std::size_t ndx = model->get_state()->get_ndx();
324  const std::size_t nu = model->get_nu();
325  calcDiffout.resize(2 * ndx * ndx + 2 * ndx * nu + nu * nu + ndx + nu);
326  calcDiffout.setZero();
327  }
328 };
329 
330 } // namespace crocoddyl
331 
332 #endif // ifndef CROCODDYL_CORE_CODEGEN_ACTION_BASE_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
VectorXs u_lb_
Lower control limits.
VectorXs u_ub_
Upper control limits.
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
std::function< void(boost::shared_ptr< ADBase >, const Eigen::Ref< const ADVectorXs > &)> fn_record_env
A function that updates the environment variables before starting record.
Eigen::DenseIndex getInputDimension() const
Dimension of the input vector.
boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
const std::string function_name_calc
Name of the function.
boost::shared_ptr< Base > model
< Neutral state
const std::size_t n_env
Size of the environment variables.
void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the next state and cost value.
bool build_forward
Options to generate or not the source code for the evaluation function.
void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
const std::string library_name
Name of the library.
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.
VectorXs r
Cost residual.
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.