11 #ifdef CROCODDYL_WITH_MULTITHREADING
15 #include "crocoddyl/core/solvers/ipopt/ipopt-iface.hpp"
20 const boost::shared_ptr<ShootingProblem>& problem)
22 const std::size_t T = problem_->get_T();
30 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
31 problem_->get_runningModels();
32 for (std::size_t t = 0; t < T; ++t) {
33 const std::size_t nxi = models[t]->get_state()->get_nx();
34 const std::size_t ndxi = models[t]->get_state()->get_ndx();
35 const std::size_t nui = models[t]->get_nu();
37 xs_[t] = models[t]->get_state()->zero();
38 us_[t] = Eigen::VectorXd::Zero(nui);
39 datas_[t] = createData(nxi, ndxi, nui);
47 nconst_ += models[0]->get_state()->get_ndx();
49 const boost::shared_ptr<ActionModelAbstract>& model =
50 problem_->get_terminalModel();
51 const std::size_t nxi = model->get_state()->get_nx();
52 const std::size_t ndxi = model->get_state()->get_ndx();
54 xs_[T] = model->get_state()->zero();
55 datas_[T] = createData(nxi, ndxi, 0);
58 void IpoptInterface::resizeData() {
59 const std::size_t T = problem_->get_T();
61 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
62 problem_->get_runningModels();
63 for (std::size_t t = 0; t < T; ++t) {
64 const std::size_t nxi = models[t]->get_state()->get_nx();
65 const std::size_t ndxi = models[t]->get_state()->get_ndx();
66 const std::size_t nui = models[t]->get_nu();
68 xs_[t].conservativeResize(nxi);
69 us_[t].conservativeResize(nui);
70 datas_[t]->resize(nxi, ndxi, nui);
78 nconst_ += models[0]->get_state()->get_ndx();
80 const boost::shared_ptr<ActionModelAbstract>& model =
81 problem_->get_terminalModel();
82 const std::size_t nxi = model->get_state()->get_nx();
83 const std::size_t ndxi = model->get_state()->get_ndx();
85 xs_[T].conservativeResize(nxi);
86 datas_[T]->resize(nxi, ndxi, 0);
89 IpoptInterface::~IpoptInterface() {}
91 bool IpoptInterface::get_nlp_info(Ipopt::Index& n, Ipopt::Index& m,
92 Ipopt::Index& nnz_jac_g,
93 Ipopt::Index& nnz_h_lag,
94 IndexStyleEnum& index_style) {
95 n =
static_cast<Ipopt::Index
>(nvar_);
96 m =
static_cast<Ipopt::Index
>(nconst_);
100 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
101 problem_->get_runningModels();
102 const std::size_t T = problem_->get_T();
103 for (std::size_t t = 0; t < T; ++t) {
104 const std::size_t ndxi = models[t]->get_state()->get_ndx();
105 const std::size_t ndxi_next =
106 t + 1 == T ? problem_->get_terminalModel()->get_state()->get_ndx()
107 : models[t + 1]->get_state()->get_ndx();
108 const std::size_t nui = models[t]->get_nu();
109 nnz_jac_g += ndxi * (ndxi + ndxi_next + nui);
112 std::size_t nonzero = 0;
113 for (std::size_t i = 1; i <= (ndxi + nui); ++i) {
116 nnz_h_lag += nonzero;
121 models[0]->get_state()->get_ndx() * models[0]->get_state()->get_ndx();
124 const std::size_t ndxi =
125 problem_->get_terminalModel()->get_state()->get_ndx();
126 std::size_t nonzero = 0;
127 for (std::size_t i = 1; i <= ndxi; ++i) {
130 nnz_h_lag += nonzero;
133 index_style = Ipopt::TNLP::C_STYLE;
139 bool IpoptInterface::get_bounds_info(Ipopt::Index n, Ipopt::Number* x_l,
140 Ipopt::Number* x_u, Ipopt::Index m,
141 Ipopt::Number* g_l, Ipopt::Number* g_u) {
143 bool IpoptInterface::get_bounds_info(Ipopt::Index, Ipopt::Number* x_l,
144 Ipopt::Number* x_u, Ipopt::Index,
145 Ipopt::Number* g_l, Ipopt::Number* g_u) {
147 assert_pretty(n ==
static_cast<Ipopt::Index
>(nvar_),
148 "Inconsistent number of decision variables");
149 assert_pretty(m ==
static_cast<Ipopt::Index
>(nconst_),
150 "Inconsistent number of constraints");
153 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
154 problem_->get_runningModels();
155 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
157 const std::size_t ndxi = models[t]->get_state()->get_ndx();
158 const std::size_t nui = models[t]->get_nu();
160 for (std::size_t j = 0; j < ndxi; ++j) {
161 x_l[ixu_[t] + j] = std::numeric_limits<double>::lowest();
162 x_u[ixu_[t] + j] = std::numeric_limits<double>::max();
164 for (std::size_t j = 0; j < nui; ++j) {
165 x_l[ixu_[t] + ndxi + j] = models[t]->get_has_control_limits()
166 ? models[t]->get_u_lb()(j)
167 : std::numeric_limits<double>::lowest();
168 x_u[ixu_[t] + ndxi + j] = models[t]->get_has_control_limits()
169 ? models[t]->get_u_ub()(j)
170 : std::numeric_limits<double>::max();
175 const std::size_t ndxi =
176 problem_->get_terminalModel()->get_state()->get_ndx();
177 for (std::size_t j = 0; j < ndxi; j++) {
178 x_l[ixu_.back() + j] = std::numeric_limits<double>::lowest();
179 x_u[ixu_.back() + j] = std::numeric_limits<double>::max();
183 for (Ipopt::Index i = 0; i < static_cast<Ipopt::Index>(nconst_); ++i) {
192 bool IpoptInterface::get_starting_point(Ipopt::Index n,
bool init_x,
193 Ipopt::Number* x,
bool init_z,
195 Ipopt::Number* , Ipopt::Index m,
199 bool IpoptInterface::get_starting_point(Ipopt::Index,
bool ,
200 Ipopt::Number* x,
bool, Ipopt::Number*,
201 Ipopt::Number*, Ipopt::Index,
bool,
204 assert_pretty(n ==
static_cast<Ipopt::Index
>(nvar_),
205 "Inconsistent number of decision variables");
206 assert_pretty(m ==
static_cast<Ipopt::Index
>(nconst_),
207 "Inconsistent number of constraints");
208 assert_pretty(init_x ==
true,
209 "Make sure to provide initial value for primal variables");
210 assert_pretty(init_z ==
false,
211 "Cannot provide initial value for bound multipliers");
212 assert_pretty(init_lambda ==
false,
213 "Cannot provide initial value for constraint multipliers");
218 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
219 problem_->get_runningModels();
220 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
221 const std::size_t ndxi = models[t]->get_state()->get_ndx();
222 const std::size_t nui = models[t]->get_nu();
223 for (std::size_t j = 0; j < ndxi; ++j) {
226 for (std::size_t j = 0; j < nui; ++j) {
227 x[ixu_[t] + ndxi + j] = us_[t](j);
230 const std::size_t ndxi =
231 problem_->get_terminalModel()->get_state()->get_ndx();
232 for (std::size_t j = 0; j < ndxi; j++) {
233 x[ixu_.back() + j] = 0;
240 bool IpoptInterface::eval_f(Ipopt::Index n,
const Ipopt::Number* x,
241 bool , Ipopt::Number& obj_value) {
243 bool IpoptInterface::eval_f(Ipopt::Index,
const Ipopt::Number* x,
bool,
244 Ipopt::Number& obj_value) {
246 assert_pretty(n ==
static_cast<Ipopt::Index
>(nvar_),
247 "Inconsistent number of decision variables");
250 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
251 problem_->get_runningModels();
252 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
253 problem_->get_runningDatas();
254 const std::size_t T = problem_->get_T();
255 #ifdef CROCODDYL_WITH_MULTITHREADING
256 #pragma omp parallel for num_threads(problem_->get_nthreads())
258 for (std::size_t t = 0; t < T; ++t) {
259 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
260 const boost::shared_ptr<ActionDataAbstract>& data = datas[t];
261 const std::size_t ndxi = model->get_state()->get_ndx();
262 const std::size_t nui = model->get_nu();
264 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
265 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
266 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
267 model->calc(data, datas_[t]->x, datas_[t]->u);
270 #ifdef CROCODDYL_WITH_MULTITHREADING
271 #pragma omp simd reduction(+ : obj_value)
273 for (std::size_t t = 0; t < T; ++t) {
274 const boost::shared_ptr<ActionDataAbstract>& data = datas[t];
275 obj_value += data->cost;
279 const boost::shared_ptr<ActionModelAbstract>& model =
280 problem_->get_terminalModel();
281 const boost::shared_ptr<ActionDataAbstract>& data =
282 problem_->get_terminalData();
283 const std::size_t ndxi = model->get_state()->get_ndx();
285 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
286 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
287 model->calc(data, datas_[T]->x);
288 obj_value += data->cost;
294 bool IpoptInterface::eval_grad_f(Ipopt::Index n,
const Ipopt::Number* x,
295 bool , Ipopt::Number* grad_f) {
297 bool IpoptInterface::eval_grad_f(Ipopt::Index,
const Ipopt::Number* x,
bool,
298 Ipopt::Number* grad_f) {
300 assert_pretty(n ==
static_cast<Ipopt::Index
>(nvar_),
301 "Inconsistent number of decision variables");
303 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
304 problem_->get_runningModels();
305 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
306 problem_->get_runningDatas();
307 const std::size_t T = problem_->get_T();
308 #ifdef CROCODDYL_WITH_MULTITHREADING
309 #pragma omp parallel for num_threads(problem_->get_nthreads())
311 for (std::size_t t = 0; t < T; ++t) {
312 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
313 const boost::shared_ptr<ActionDataAbstract>& data = datas[t];
314 const std::size_t ndxi = model->get_state()->get_ndx();
315 const std::size_t nui = model->get_nu();
317 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
318 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
319 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
320 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
321 datas_[t]->Jint_dx, second, setto);
322 model->calc(data, datas_[t]->x, datas_[t]->u);
323 model->calcDiff(data, datas_[t]->x, datas_[t]->u);
324 datas_[t]->Ldx.noalias() = datas_[t]->Jint_dx.transpose() * data->Lx;
326 for (std::size_t t = 0; t < T; ++t) {
327 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
328 const boost::shared_ptr<ActionDataAbstract>& data = datas[t];
329 const std::size_t ndxi = model->get_state()->get_ndx();
330 const std::size_t nui = model->get_nu();
331 for (std::size_t j = 0; j < ndxi; ++j) {
332 grad_f[ixu_[t] + j] = datas_[t]->Ldx(j);
334 for (std::size_t j = 0; j < nui; ++j) {
335 grad_f[ixu_[t] + ndxi + j] = data->Lu(j);
340 const boost::shared_ptr<ActionModelAbstract>& model =
341 problem_->get_terminalModel();
342 const boost::shared_ptr<ActionDataAbstract>& data =
343 problem_->get_terminalData();
344 const std::size_t ndxi = model->get_state()->get_ndx();
346 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
347 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
348 model->get_state()->Jintegrate(xs_[T], datas_[T]->dx, datas_[T]->Jint_dx,
349 datas_[T]->Jint_dx, second, setto);
350 model->calc(data, datas_[T]->x);
351 model->calcDiff(data, datas_[T]->x);
352 datas_[T]->Ldx.noalias() = datas_[T]->Jint_dx.transpose() * data->Lx;
353 for (std::size_t j = 0; j < ndxi; ++j) {
354 grad_f[ixu_.back() + j] = datas_[T]->Ldx(j);
361 bool IpoptInterface::eval_g(Ipopt::Index n,
const Ipopt::Number* x,
362 bool , Ipopt::Index m, Ipopt::Number* g) {
364 bool IpoptInterface::eval_g(Ipopt::Index,
const Ipopt::Number* x,
365 bool , Ipopt::Index, Ipopt::Number* g) {
367 assert_pretty(n ==
static_cast<Ipopt::Index
>(nvar_),
368 "Inconsistent number of decision variables");
369 assert_pretty(m ==
static_cast<Ipopt::Index
>(nconst_),
370 "Inconsistent number of constraints");
373 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
374 problem_->get_runningModels();
375 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
376 problem_->get_runningDatas();
377 const std::size_t T = problem_->get_T();
378 #ifdef CROCODDYL_WITH_MULTITHREADING
379 #pragma omp parallel for num_threads(problem_->get_nthreads())
381 for (std::size_t t = 0; t < T; ++t) {
382 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
383 const boost::shared_ptr<ActionDataAbstract>& data = datas[t];
384 const std::size_t ndxi = model->get_state()->get_ndx();
385 const std::size_t nui = model->get_nu();
386 const boost::shared_ptr<ActionModelAbstract>& model_next =
387 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
388 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
390 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
391 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
393 Eigen::VectorXd::Map(x + ixu_[t] + ndxi + nui, ndxi_next);
394 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
395 model_next->get_state()->integrate(xs_[t + 1], datas_[t]->dxnext,
397 model->calc(data, datas_[t]->x, datas_[t]->u);
398 model->get_state()->diff(data->xnext, datas_[t]->xnext, datas_[t]->x_diff);
402 for (std::size_t t = 0; t < T; ++t) {
403 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
404 const std::size_t ndxi = model->get_state()->get_ndx();
405 for (std::size_t j = 0; j < ndxi; ++j) {
406 g[ix + j] = datas_[t]->x_diff[j];
412 const boost::shared_ptr<ActionModelAbstract>& model = models[0];
413 const std::size_t ndxi = model->get_state()->get_ndx();
414 datas_[0]->dx = Eigen::VectorXd::Map(x, ndxi);
415 model->get_state()->integrate(xs_[0], datas_[0]->dx, datas_[0]->x);
416 model->get_state()->diff(datas_[0]->x, problem_->get_x0(),
418 for (std::size_t j = 0; j < ndxi; j++) {
419 g[ix + j] = datas_[0]->x_diff[j];
426 bool IpoptInterface::eval_jac_g(Ipopt::Index n,
const Ipopt::Number* x,
427 bool , Ipopt::Index m,
428 Ipopt::Index nele_jac, Ipopt::Index* iRow,
429 Ipopt::Index* jCol, Ipopt::Number* values) {
431 bool IpoptInterface::eval_jac_g(Ipopt::Index,
const Ipopt::Number* x,
bool,
432 Ipopt::Index, Ipopt::Index, Ipopt::Index* iRow,
433 Ipopt::Index* jCol, Ipopt::Number* values) {
435 assert_pretty(n ==
static_cast<Ipopt::Index
>(nvar_),
436 "Inconsistent number of decision variables");
437 assert_pretty(m ==
static_cast<Ipopt::Index
>(nconst_),
438 "Inconsistent number of constraints");
440 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
441 problem_->get_runningModels();
442 if (values == NULL) {
446 const std::size_t T = problem_->get_T();
447 for (std::size_t t = 0; t < T; ++t) {
448 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
449 const std::size_t ndxi = model->get_state()->get_ndx();
450 const std::size_t nui = model->get_nu();
451 const std::size_t ndxi_next =
452 t + 1 == T ? problem_->get_terminalModel()->get_state()->get_ndx()
453 : models[t + 1]->get_state()->get_ndx();
454 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
455 for (std::size_t idx_col = 0; idx_col < (ndxi + nui + ndxi_next);
457 iRow[idx] =
static_cast<Ipopt::Index
>(ix + idx_row);
458 jCol[idx] =
static_cast<Ipopt::Index
>(ixu_[t] + idx_col);
466 const std::size_t ndxi = models[0]->get_state()->get_ndx();
467 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
468 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
469 iRow[idx] =
static_cast<Ipopt::Index
>(ix + idx_row);
470 jCol[idx] =
static_cast<Ipopt::Index
>(idx_col);
475 assert_pretty(nele_jac ==
static_cast<Ipopt::Index
>(idx),
476 "Number of jacobian elements set does not coincide with the "
477 "total non-zero Jacobian values");
479 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
480 problem_->get_runningDatas();
482 const std::size_t T = problem_->get_T();
483 #ifdef CROCODDYL_WITH_MULTITHREADING
484 #pragma omp parallel for num_threads(problem_->get_nthreads())
486 for (std::size_t t = 0; t < T; ++t) {
487 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
488 const boost::shared_ptr<ActionDataAbstract>& data = datas[t];
489 const boost::shared_ptr<ActionModelAbstract>& model_next =
490 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
491 const std::size_t ndxi = model->get_state()->get_ndx();
492 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
493 const std::size_t nui = model->get_nu();
494 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
495 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
497 Eigen::VectorXd::Map(x + ixu_[t] + ndxi + nui, ndxi_next);
499 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
500 model_next->get_state()->integrate(xs_[t + 1], datas_[t]->dxnext,
502 model->calcDiff(data, datas_[t]->x, datas_[t]->u);
503 model_next->get_state()->Jintegrate(
504 xs_[t + 1], datas_[t]->dxnext, datas_[t]->Jint_dxnext,
505 datas_[t]->Jint_dxnext, second,
507 model->get_state()->Jdiff(
508 data->xnext, datas_[t]->xnext, datas_[t]->Jdiff_x,
509 datas_[t]->Jdiff_xnext,
511 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
512 datas_[t]->Jint_dx, second,
514 datas_[t]->Jg_dxnext.noalias() =
515 datas_[t]->Jdiff_xnext * datas_[t]->Jint_dxnext;
516 datas_[t]->FxJint_dx.noalias() = data->Fx * datas_[t]->Jint_dx;
517 datas_[t]->Jg_dx.noalias() = datas_[t]->Jdiff_x * datas_[t]->FxJint_dx;
518 datas_[t]->Jg_u.noalias() = datas_[t]->Jdiff_x * data->Fu;
521 for (std::size_t t = 0; t < T; ++t) {
522 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
523 const boost::shared_ptr<ActionModelAbstract>& model_next =
524 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
525 const std::size_t ndxi = model->get_state()->get_ndx();
526 const std::size_t nui = model->get_nu();
527 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
528 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
529 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
530 values[idx] = datas_[t]->Jg_dx(idx_row, idx_col);
533 for (std::size_t idx_col = 0; idx_col < nui; ++idx_col) {
534 values[idx] = datas_[t]->Jg_u(idx_row, idx_col);
538 for (std::size_t idx_col = 0; idx_col < ndxi_next; ++idx_col) {
539 values[idx] = datas_[t]->Jg_dxnext(idx_row, idx_col);
546 const boost::shared_ptr<ActionModelAbstract>& model = models[0];
547 const std::size_t ndxi = model->get_state()->get_ndx();
548 datas_[0]->dx = Eigen::VectorXd::Map(x, ndxi);
550 model->get_state()->integrate(xs_[0], datas_[0]->dx, datas_[0]->x);
551 model->get_state()->Jdiff(datas_[0]->x, problem_->get_x0(),
552 datas_[0]->Jdiff_x, datas_[0]->Jdiff_x, first);
553 model->get_state()->Jintegrate(xs_[0], datas_[0]->dx, datas_[0]->Jint_dx,
554 datas_[0]->Jint_dx, second, setto);
555 datas_[0]->Jg_ic.noalias() = datas_[0]->Jdiff_x * datas_[0]->Jint_dx;
556 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
557 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
558 values[idx] = datas_[0]->Jg_ic(idx_row, idx_col);
568 bool IpoptInterface::eval_h(Ipopt::Index n,
const Ipopt::Number* x,
569 bool , Ipopt::Number obj_factor,
570 Ipopt::Index m,
const Ipopt::Number* ,
571 bool , Ipopt::Index nele_hess,
572 Ipopt::Index* iRow, Ipopt::Index* jCol,
573 Ipopt::Number* values) {
575 bool IpoptInterface::eval_h(Ipopt::Index,
const Ipopt::Number* x,
bool,
576 Ipopt::Number obj_factor, Ipopt::Index,
577 const Ipopt::Number*,
bool, Ipopt::Index,
578 Ipopt::Index* iRow, Ipopt::Index* jCol,
579 Ipopt::Number* values) {
581 assert_pretty(n ==
static_cast<Ipopt::Index
>(nvar_),
582 "Inconsistent number of decision variables");
583 assert_pretty(m ==
static_cast<Ipopt::Index
>(nconst_),
584 "Inconsistent number of constraints");
586 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
587 problem_->get_runningModels();
588 const std::size_t T = problem_->get_T();
589 if (values == NULL) {
595 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
596 const boost::shared_ptr<ActionModelAbstract> model = models[t];
597 const std::size_t ndxi = model->get_state()->get_ndx();
598 const std::size_t nui = model->get_nu();
599 for (std::size_t idx_row = 0; idx_row < ndxi + nui; ++idx_row) {
600 for (std::size_t idx_col = 0; idx_col < ndxi + nui; ++idx_col) {
602 if (idx_col > idx_row) {
605 iRow[idx] =
static_cast<Ipopt::Index
>(ixu_[t] + idx_row);
606 jCol[idx] =
static_cast<Ipopt::Index
>(ixu_[t] + idx_col);
613 const std::size_t ndxi =
614 problem_->get_terminalModel()->get_state()->get_ndx();
615 for (std::size_t idx_row = 0; idx_row < ndxi; idx_row++) {
616 for (std::size_t idx_col = 0; idx_col < ndxi; idx_col++) {
618 if (idx_col > idx_row) {
621 iRow[idx] =
static_cast<Ipopt::Index
>(ixu_.back() + idx_row);
622 jCol[idx] =
static_cast<Ipopt::Index
>(ixu_.back() + idx_col);
627 assert_pretty(nele_hess ==
static_cast<Ipopt::Index
>(idx),
628 "Number of Hessian elements set does not coincide with the "
629 "total non-zero Hessian values");
634 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
635 problem_->get_runningDatas();
636 #ifdef CROCODDYL_WITH_MULTITHREADING
637 #pragma omp parallel for num_threads(problem_->get_nthreads())
639 for (std::size_t t = 0; t < T; ++t) {
640 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
641 const boost::shared_ptr<ActionDataAbstract>& data = datas[t];
642 const std::size_t ndxi = model->get_state()->get_ndx();
643 const std::size_t nui = model->get_nu();
644 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
645 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
647 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
648 model->calcDiff(data, datas_[t]->x,
650 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
651 datas_[t]->Jint_dx, second, setto);
652 datas_[t]->Ldxdx.noalias() =
653 datas_[t]->Jint_dx.transpose() * data->Lxx * datas_[t]->Jint_dx;
654 datas_[t]->Ldxu.noalias() = datas_[t]->Jint_dx.transpose() * data->Lxu;
658 for (std::size_t t = 0; t < T; ++t) {
659 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
660 const boost::shared_ptr<ActionDataAbstract>& data = datas[t];
661 const std::size_t ndxi = model->get_state()->get_ndx();
662 const std::size_t nui = model->get_nu();
663 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
664 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
666 if (idx_col > idx_row) {
669 values[idx] = obj_factor * datas_[t]->Ldxdx(idx_row, idx_col);
673 for (std::size_t idx_row = 0; idx_row < nui; ++idx_row) {
674 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
675 values[idx] = obj_factor * datas_[t]->Ldxu(idx_col, idx_row);
678 for (std::size_t idx_col = 0; idx_col < nui; ++idx_col) {
679 if (idx_col > idx_row) {
682 values[idx] = obj_factor * data->Luu(idx_row, idx_col);
689 const boost::shared_ptr<ActionModelAbstract>& model =
690 problem_->get_terminalModel();
691 const boost::shared_ptr<ActionDataAbstract>& data =
692 problem_->get_terminalData();
693 const std::size_t ndxi = model->get_state()->get_ndx();
694 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
696 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
697 model->calc(data, datas_[T]->x);
698 model->calcDiff(data, datas_[T]->x);
699 model->get_state()->Jintegrate(xs_[T], datas_[T]->dx, datas_[T]->Jint_dx,
700 datas_[T]->Jint_dx, second, setto);
701 datas_[T]->Ldxdx.noalias() =
702 datas_[T]->Jint_dx.transpose() * data->Lxx * datas_[T]->Jint_dx;
703 for (std::size_t idx_row = 0; idx_row < ndxi; idx_row++) {
704 for (std::size_t idx_col = 0; idx_col < ndxi; idx_col++) {
706 if (idx_col > idx_row) {
709 values[idx] = datas_[T]->Ldxdx(idx_row, idx_col);
718 void IpoptInterface::finalize_solution(
719 Ipopt::SolverReturn , Ipopt::Index ,
const Ipopt::Number* x,
720 const Ipopt::Number* ,
const Ipopt::Number* ,
721 Ipopt::Index ,
const Ipopt::Number* ,
722 const Ipopt::Number* , Ipopt::Number obj_value,
723 const Ipopt::IpoptData* ,
724 Ipopt::IpoptCalculatedQuantities* ) {
726 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
727 problem_->get_runningModels();
728 const std::size_t T = problem_->get_T();
729 for (std::size_t t = 0; t < T; ++t) {
730 const std::size_t ndxi = models[t]->get_state()->get_ndx();
731 const std::size_t nui = models[t]->get_nu();
732 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
733 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
735 models[t]->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
736 xs_[t] = datas_[t]->x;
737 us_[t] = datas_[t]->u;
740 const boost::shared_ptr<ActionModelAbstract>& model =
741 problem_->get_terminalModel();
742 const std::size_t ndxi = model->get_state()->get_ndx();
743 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
744 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
745 xs_[T] = datas_[T]->x;
750 bool IpoptInterface::intermediate_callback(
751 Ipopt::AlgorithmMode , Ipopt::Index ,
752 Ipopt::Number , Ipopt::Number ,
753 Ipopt::Number , Ipopt::Number , Ipopt::Number ,
754 Ipopt::Number , Ipopt::Number ,
755 Ipopt::Number , Ipopt::Index ,
756 const Ipopt::IpoptData* ,
757 Ipopt::IpoptCalculatedQuantities* ) {
761 boost::shared_ptr<IpoptInterfaceData> IpoptInterface::createData(
762 const std::size_t nx,
const std::size_t ndx,
const std::size_t nu) {
763 return boost::allocate_shared<IpoptInterfaceData>(
764 Eigen::aligned_allocator<IpoptInterfaceData>(), nx, ndx, nu);
767 void IpoptInterface::set_xs(
const std::vector<Eigen::VectorXd>& xs) {
771 void IpoptInterface::set_us(
const std::vector<Eigen::VectorXd>& us) {
775 std::size_t IpoptInterface::get_nvar()
const {
return nvar_; }
777 std::size_t IpoptInterface::get_nconst()
const {
return nconst_; }
779 const std::vector<Eigen::VectorXd>& IpoptInterface::get_xs()
const {
783 const std::vector<Eigen::VectorXd>& IpoptInterface::get_us()
const {
787 const boost::shared_ptr<ShootingProblem>& IpoptInterface::get_problem()
const {
791 double IpoptInterface::get_cost()
const {
return cost_; }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW IpoptInterface(const boost::shared_ptr< crocoddyl::ShootingProblem > &problem)
Initialize the Ipopt interface.