10 #ifndef CROCODDYL_CORE_CODEGEN_ACTION_BASE_HPP_
11 #define CROCODDYL_CORE_CODEGEN_ACTION_BASE_HPP_
15 #include "crocoddyl/core/action-base.hpp"
16 #include "pinocchio/codegen/cppadcg.hpp"
20 template <
typename Scalar>
21 struct ActionDataCodeGenTpl;
23 template <
typename _Scalar>
26 typedef _Scalar Scalar;
33 typedef CppAD::cg::CG<Scalar> CGScalar;
34 typedef CppAD::AD<CGScalar> ADScalar;
44 typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
46 typedef CppAD::ADFun<CGScalar> ADFun;
49 boost::shared_ptr<Base>
model,
51 const std::size_t
n_env = 0,
52 std::function<
void(boost::shared_ptr<ADBase>,
53 const Eigen::Ref<const ADVectorXs>&)>
56 const std::string& function_name_calcDiff =
"calcDiff")
60 ad_data(ad_model->createData()),
62 function_name_calcDiff(function_name_calcDiff),
66 ad_X(ad_model->get_state()->get_nx() + ad_model->get_nu() +
n_env),
67 ad_X2(ad_model->get_state()->get_nx() + ad_model->get_nu() +
n_env),
68 ad_calcout(ad_model->get_state()->get_nx() + 1) {
69 const std::size_t ndx = ad_model->get_state()->get_ndx();
70 const std::size_t nu = ad_model->get_nu();
71 ad_calcDiffout.resize(2 * ndx * ndx + 2 * ndx * nu + nu * nu + ndx + nu);
76 static void empty_record_env(boost::shared_ptr<ADBase>,
77 const Eigen::Ref<const ADVectorXs>&) {}
80 CppAD::Independent(ad_X);
81 const std::size_t nx = ad_model->get_state()->get_nx();
82 const std::size_t nu = ad_model->get_nu();
86 ad_model->calc(ad_data, ad_X.head(nx), ad_X.segment(nx, nu));
90 ad_calc.Dependent(ad_X, ad_calcout);
91 ad_calc.optimize(
"no_compare_op");
94 void collect_calcout() {
95 ad_calcout[0] = ad_data->cost;
96 ad_calcout.tail(ad_model->get_state()->get_nx()) = ad_data->xnext;
99 void collect_calcDiffout() {
100 ADVectorXs& ad_Y = ad_calcDiffout;
102 const std::size_t ndx = ad_model->get_state()->get_ndx();
103 const std::size_t nu = ad_model->get_nu();
104 Eigen::DenseIndex it_Y = 0;
105 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, ndx) = ad_data->Fx;
107 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, nu) = ad_data->Fu;
109 Eigen::Map<ADVectorXs>(ad_Y.data() + it_Y, ndx) = ad_data->Lx;
111 Eigen::Map<ADVectorXs>(ad_Y.data() + it_Y, nu) = ad_data->Lu;
113 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, ndx) = ad_data->Lxx;
115 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, nu) = ad_data->Lxu;
117 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, nu, nu) = ad_data->Luu;
120 void recordCalcDiff() {
121 CppAD::Independent(ad_X2);
122 const std::size_t nx = ad_model->get_state()->get_nx();
123 const std::size_t nu = ad_model->get_nu();
127 ad_model->calc(ad_data, ad_X2.head(nx), ad_X2.segment(nx, nu));
128 ad_model->calcDiff(ad_data, ad_X2.head(nx), ad_X2.segment(nx, nu));
130 collect_calcDiffout();
131 ad_calcDiff.Dependent(ad_X2, ad_calcDiffout);
132 ad_calcDiff.optimize(
"no_compare_op");
139 calcgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(
141 calcgen_ptr->setCreateForwardZero(
true);
142 calcgen_ptr->setCreateJacobian(
false);
146 calcDiffgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(
147 new CppAD::cg::ModelCSourceGen<Scalar>(ad_calcDiff,
148 function_name_calcDiff));
149 calcDiffgen_ptr->setCreateForwardZero(
true);
150 calcDiffgen_ptr->setCreateJacobian(
false);
152 libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> >(
153 new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*calcgen_ptr,
156 dynamicLibManager_ptr =
157 std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> >(
158 new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr,
163 CppAD::cg::GccCompiler<Scalar> compiler;
164 std::vector<std::string> compile_options = compiler.getCompileFlags();
165 compile_options[0] =
"-O3";
166 compiler.setCompileFlags(compile_options);
167 dynamicLibManager_ptr->createDynamicLibrary(compiler,
false);
170 bool existLib()
const {
171 const std::string filename =
172 dynamicLibManager_ptr->getLibraryName() +
173 CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION;
174 std::ifstream file(filename.c_str());
178 void loadLib(
const bool generate_if_not_exist =
true) {
179 if (not existLib() && generate_if_not_exist) compileLib();
181 const auto it = dynamicLibManager_ptr->getOptions().find(
"dlOpenMode");
182 if (it == dynamicLibManager_ptr->getOptions().end()) {
183 dynamicLib_ptr.reset(
new CppAD::cg::LinuxDynamicLib<Scalar>(
184 dynamicLibManager_ptr->getLibraryName() +
185 CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION));
187 int dlOpenMode = std::stoi(it->second);
188 dynamicLib_ptr.reset(
new CppAD::cg::LinuxDynamicLib<Scalar>(
189 dynamicLibManager_ptr->getLibraryName() +
190 CppAD::cg::system::SystemInfo<>::DYNAMIC_LIB_EXTENSION,
195 calcDiffFun_ptr = dynamicLib_ptr->model(function_name_calcDiff.c_str());
198 void set_env(
const boost::shared_ptr<ActionDataAbstract>& data,
199 const Eigen::Ref<const VectorXs>& env_val)
const {
200 Data* d =
static_cast<Data*
>(data.get());
201 d->xu.tail(
n_env) = env_val;
204 void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
205 const Eigen::Ref<const VectorXs>& x,
206 const Eigen::Ref<const VectorXs>& u) {
207 Data* d =
static_cast<Data*
>(data.get());
208 const std::size_t nx = ad_model->get_state()->get_nx();
209 const std::size_t nu = ad_model->get_nu();
212 d->xu.segment(nx, nu) = u;
214 calcFun_ptr->ForwardZero(d->xu, d->calcout);
215 d->distribute_calcout();
218 void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
219 const Eigen::Ref<const VectorXs>& x,
220 const Eigen::Ref<const VectorXs>& u) {
221 Data* d =
static_cast<Data*
>(data.get());
222 const std::size_t nx = ad_model->get_state()->get_nx();
223 const std::size_t nu = ad_model->get_nu();
226 d->xu.segment(nx, nu) = u;
227 calcDiffFun_ptr->ForwardZero(d->xu, d->calcDiffout);
228 d->distribute_calcDiffout();
232 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
249 boost::shared_ptr<ADBase> ad_model;
250 boost::shared_ptr<ADActionDataAbstract> ad_data;
263 std::function<void(boost::shared_ptr<ADBase>,
264 const Eigen::Ref<const ADVectorXs>&)>
271 ADVectorXs ad_X, ad_X2;
273 ADVectorXs ad_calcout;
274 ADVectorXs ad_calcDiffout;
276 ADFun ad_calc, ad_calcDiff;
278 std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> > calcgen_ptr,
280 std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> > libcgen_ptr;
281 std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> >
282 dynamicLibManager_ptr;
283 std::unique_ptr<CppAD::cg::DynamicLib<Scalar> > dynamicLib_ptr;
284 std::unique_ptr<CppAD::cg::GenericModel<Scalar> > calcFun_ptr,
289 template <
typename _Scalar>
291 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
293 typedef _Scalar Scalar;
296 typedef typename MathBase::VectorXs VectorXs;
297 typedef typename MathBase::MatrixXs MatrixXs;
310 VectorXs xu, calcout;
312 VectorXs calcDiffout;
314 void distribute_calcout() {
319 void distribute_calcDiffout() {
320 VectorXs& Y = calcDiffout;
321 const std::size_t ndx =
Fx.rows();
322 const std::size_t nu =
Fu.cols();
324 Eigen::DenseIndex it_Y = 0;
325 Fx = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, ndx);
327 Fu = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, nu);
329 Lx = Eigen::Map<VectorXs>(Y.data() + it_Y, ndx);
331 Lu = Eigen::Map<VectorXs>(Y.data() + it_Y, nu);
333 Lxx = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, ndx);
335 Lxu = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, nu);
337 Luu = Eigen::Map<MatrixXs>(Y.data() + it_Y, nu, nu);
340 template <
template <
typename Scalar>
class Model>
342 :
Base(model), calcout(model->get_state()->get_nx() + 1) {
348 const std::size_t ndx = model->
get_state()->get_ndx();
349 const std::size_t nu = model->get_nu();
350 calcDiffout.resize(2 * ndx * ndx + 2 * ndx * nu + nu * nu + ndx + nu);
351 calcDiffout.setZero();
Abstract class for action model.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
VectorXs u_lb_
Lower control limits.
VectorXs u_ub_
Upper control limits.
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 w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .