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>
24 class ActionModelCodeGenTpl : public ActionModelAbstractTpl<_Scalar> {
25  public:
26  typedef _Scalar Scalar;
27  typedef ActionModelAbstractTpl<Scalar> Base;
28  typedef ActionDataCodeGenTpl<Scalar> Data;
29  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
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;
35  typedef crocoddyl::ActionModelAbstractTpl<ADScalar> ADBase;
36  typedef ActionDataAbstractTpl<ADScalar> ADActionDataAbstract;
37  typedef ActionDataCodeGenTpl<ADScalar> ADActionDataCodeGen;
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()),
57  function_name_calc(function_name_calc),
58  function_name_calcDiff(function_name_calcDiff),
59  library_name(library_name),
60  n_env(n_env),
61  fn_record_env(fn_record_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:
224  using Base::has_control_limits_;
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 
249  bool build_forward;
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;
271  typedef MathBaseTpl<Scalar> MathBase;
272  typedef ActionDataAbstractTpl<Scalar> Base;
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) {
319  ActionModelCodeGenTpl<Scalar>* m = static_cast<ActionModelCodeGenTpl<Scalar>*>(model);
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_
crocoddyl::ActionDataAbstractTpl
Definition: action-base.hpp:236
crocoddyl::ActionModelAbstractTpl::has_control_limits_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: action-base.hpp:227
crocoddyl::ActionModelCodeGenTpl::build_forward
bool build_forward
Options to generate or not the source code for the evaluation function.
Definition: action-base.hpp:249
crocoddyl::ActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: action-base.hpp:276
crocoddyl::MathBaseTpl
Definition: mathbase.hpp:18
crocoddyl::ActionModelCodeGenTpl::n_env
const std::size_t n_env
Size of the environment variables.
Definition: action-base.hpp:243
crocoddyl::ActionDataCodeGenTpl
Definition: action-base.hpp:21
crocoddyl::ActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: action-base.hpp:221
crocoddyl::ActionDataAbstractTpl::Lx
VectorXs Lx
Jacobian of the cost function.
Definition: action-base.hpp:273
crocoddyl::ActionModelCodeGenTpl::fn_record_env
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.
Definition: action-base.hpp:246
crocoddyl::ActionModelCodeGenTpl::function_name_calc
const std::string function_name_calc
Name of the function.
Definition: action-base.hpp:237
crocoddyl::ActionDataAbstractTpl::xnext
VectorXs xnext
evolution state
Definition: action-base.hpp:269
crocoddyl::ActionModelAbstractTpl
Abstract class for action model.
Definition: action-base.hpp:59
crocoddyl::ActionDataAbstractTpl::cost
Scalar cost
cost value
Definition: action-base.hpp:268
crocoddyl::ActionModelCodeGenTpl::library_name
const std::string library_name
Name of the library.
Definition: action-base.hpp:240
crocoddyl::ActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: action-base.hpp:272
crocoddyl::ActionModelCodeGenTpl::getInputDimension
Eigen::DenseIndex getInputDimension() const
Dimension of the input vector.
Definition: action-base.hpp:221
crocoddyl::ActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:223
crocoddyl::ActionModelCodeGenTpl::calcDiff
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.
Definition: action-base.hpp:204
crocoddyl::ActionModelAbstractTpl::unone_
VectorXs unone_
Neutral state.
Definition: action-base.hpp:224
crocoddyl::ActionModelCodeGenTpl
Definition: action-base.hpp:24
crocoddyl::ActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: action-base.hpp:270
crocoddyl::ActionModelAbstractTpl::u_lb_
VectorXs u_lb_
Lower control limits.
Definition: action-base.hpp:225
crocoddyl::ActionModelCodeGenTpl::createData
boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
Definition: action-base.hpp:216
crocoddyl::ActionModelAbstractTpl::u_ub_
VectorXs u_ub_
Upper control limits.
Definition: action-base.hpp:226
crocoddyl::ActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: action-base.hpp:274
crocoddyl::ActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: action-base.hpp:275
crocoddyl::ActionDataAbstractTpl::Fu
MatrixXs Fu
Jacobian of the dynamics.
Definition: action-base.hpp:271
crocoddyl::ActionModelAbstractTpl::nr_
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:222
crocoddyl::ActionModelCodeGenTpl::model
boost::shared_ptr< Base > model
< Neutral state
Definition: action-base.hpp:232
crocoddyl::ActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: action-base.hpp:277
crocoddyl::ActionModelCodeGenTpl::calc
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.
Definition: action-base.hpp:191