crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
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
18namespace crocoddyl {
19
20template <typename Scalar>
21struct ActionDataCodeGenTpl;
22
23template <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
266template <typename _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
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.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
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.
const std::string function_name_calc
Name of the function.
boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
boost::shared_ptr< Base > model
< Neutral state
const std::size_t n_env
Size of the environment variables.
bool build_forward
Options to generate or not the source code for the evaluation function.
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.