10 #ifndef CROCODDYL_CORE_CODEGEN_ACTION_BASE_HPP_
11 #define CROCODDYL_CORE_CODEGEN_ACTION_BASE_HPP_
14 #include "pinocchio/codegen/cppadcg.hpp"
16 #include "crocoddyl/core/action-base.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;
43 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
45 typedef CppAD::ADFun<CGScalar> ADFun;
49 std::function<
void(boost::shared_ptr<ADBase>,
const Eigen::Ref<const ADVectorXs>&)>
52 const std::string& function_name_calcDiff =
"calcDiff")
56 ad_data(ad_model->createData()),
58 function_name_calcDiff(function_name_calcDiff),
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);
72 static void empty_record_env(boost::shared_ptr<ADBase>,
const Eigen::Ref<const ADVectorXs>&) {}
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();
81 ad_model->calc(ad_data, ad_X.head(nx), ad_X.segment(nx, nu));
85 ad_calc.Dependent(ad_X, ad_calcout);
86 ad_calc.optimize(
"no_compare_op");
89 void collect_calcout() {
90 ad_calcout[0] = ad_data->cost;
91 ad_calcout.tail(ad_model->get_state()->get_nx()) = ad_data->xnext;
94 void collect_calcDiffout() {
95 ADVectorXs& ad_Y = ad_calcDiffout;
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;
102 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, nu) = ad_data->Fu;
104 Eigen::Map<ADVectorXs>(ad_Y.data() + it_Y, ndx) = ad_data->Lx;
106 Eigen::Map<ADVectorXs>(ad_Y.data() + it_Y, nu) = ad_data->Lu;
108 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, ndx) = ad_data->Lxx;
110 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, ndx, nu) = ad_data->Lxu;
112 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, nu, nu) = ad_data->Luu;
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();
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));
125 collect_calcDiffout();
126 ad_calcDiff.Dependent(ad_X2, ad_calcDiffout);
127 ad_calcDiff.optimize(
"no_compare_op");
134 calcgen_ptr = std::unique_ptr<CppAD::cg::ModelCSourceGen<Scalar> >(
136 calcgen_ptr->setCreateForwardZero(
true);
137 calcgen_ptr->setCreateJacobian(
false);
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);
146 libcgen_ptr = std::unique_ptr<CppAD::cg::ModelLibraryCSourceGen<Scalar> >(
147 new CppAD::cg::ModelLibraryCSourceGen<Scalar>(*calcgen_ptr, *calcDiffgen_ptr));
149 dynamicLibManager_ptr = std::unique_ptr<CppAD::cg::DynamicModelLibraryProcessor<Scalar> >(
150 new CppAD::cg::DynamicModelLibraryProcessor<Scalar>(*libcgen_ptr,
library_name));
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);
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());
168 void loadLib(
const bool generate_if_not_exist =
true) {
169 if (not existLib() && generate_if_not_exist) compileLib();
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));
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,
183 calcDiffFun_ptr = dynamicLib_ptr->model(function_name_calcDiff.c_str());
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;
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();
198 d->xu.segment(nx, nu) = u;
200 calcFun_ptr->ForwardZero(d->xu, d->calcout);
201 d->distribute_calcout();
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();
211 d->xu.segment(nx, nu) = u;
212 calcDiffFun_ptr->ForwardZero(d->xu, d->calcDiffout);
213 d->distribute_calcDiffout();
217 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(),
this);
233 boost::shared_ptr<ADBase> ad_model;
234 boost::shared_ptr<ADActionDataAbstract> ad_data;
246 std::function<void(boost::shared_ptr<ADBase>,
const Eigen::Ref<const ADVectorXs>&)>
fn_record_env;
251 ADVectorXs ad_X, ad_X2;
253 ADVectorXs ad_calcout;
254 ADVectorXs ad_calcDiffout;
256 ADFun ad_calc, ad_calcDiff;
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;
266 template <
typename _Scalar>
268 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
270 typedef _Scalar Scalar;
273 typedef typename MathBase::VectorXs VectorXs;
274 typedef typename MathBase::MatrixXs MatrixXs;
287 VectorXs xu, calcout;
289 VectorXs calcDiffout;
291 void distribute_calcout() {
296 void distribute_calcDiffout() {
297 VectorXs& Y = calcDiffout;
298 const std::size_t ndx =
Fx.rows();
299 const std::size_t nu =
Fu.cols();
301 Eigen::DenseIndex it_Y = 0;
302 Fx = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, ndx);
304 Fu = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, nu);
306 Lx = Eigen::Map<VectorXs>(Y.data() + it_Y, ndx);
308 Lu = Eigen::Map<VectorXs>(Y.data() + it_Y, nu);
310 Lxx = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, ndx);
312 Lxu = Eigen::Map<MatrixXs>(Y.data() + it_Y, ndx, nu);
314 Luu = Eigen::Map<MatrixXs>(Y.data() + it_Y, nu, nu);
317 template <
template <
typename Scalar>
class Model>
318 explicit ActionDataCodeGenTpl(Model<Scalar>*
const model) :
Base(model), calcout(model->get_state()->get_nx() + 1) {
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();
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.
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 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.