Crocoddyl
 
Loading...
Searching...
No Matches
ipopt-iface.cpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2022-2023, IRI: CSIC-UPC, Heriot-Watt University
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#include <cmath>
10#include <iostream>
11#ifdef CROCODDYL_WITH_MULTITHREADING
12#include <omp.h>
13#endif // CROCODDYL_WITH_MULTITHREADING
14
15#include "crocoddyl/core/solvers/ipopt/ipopt-iface.hpp"
16
17namespace crocoddyl {
18
19IpoptInterface::IpoptInterface(const std::shared_ptr<ShootingProblem>& problem)
20 : problem_(problem) {
21 const std::size_t T = problem_->get_T();
22 xs_.resize(T + 1);
23 us_.resize(T);
24 datas_.resize(T + 1);
25 ixu_.resize(T + 1);
26
27 nconst_ = 0;
28 nvar_ = 0;
29 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
30 problem_->get_runningModels();
31 for (std::size_t t = 0; t < T; ++t) {
32 const std::size_t nxi = models[t]->get_state()->get_nx();
33 const std::size_t ndxi = models[t]->get_state()->get_ndx();
34 const std::size_t nui = models[t]->get_nu();
35
36 xs_[t] = models[t]->get_state()->zero();
37 us_[t] = Eigen::VectorXd::Zero(nui);
38 datas_[t] = createData(nxi, ndxi, nui);
39 ixu_[t] = nvar_;
40 nconst_ += ndxi; // T*ndx eq. constraints for dynamics
41 nvar_ += ndxi + nui; // Multiple shooting, states and controls
42 }
43 ixu_[T] = nvar_;
44
45 // Initial condition
46 nconst_ += models[0]->get_state()->get_ndx();
47
48 const std::shared_ptr<ActionModelAbstract>& model =
49 problem_->get_terminalModel();
50 const std::size_t nxi = model->get_state()->get_nx();
51 const std::size_t ndxi = model->get_state()->get_ndx();
52 nvar_ += ndxi; // final node
53 xs_[T] = model->get_state()->zero();
54 datas_[T] = createData(nxi, ndxi, 0);
55}
56
57void IpoptInterface::resizeData() {
58 const std::size_t T = problem_->get_T();
59 nvar_ = 0;
60 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
61 problem_->get_runningModels();
62 for (std::size_t t = 0; t < T; ++t) {
63 const std::size_t nxi = models[t]->get_state()->get_nx();
64 const std::size_t ndxi = models[t]->get_state()->get_ndx();
65 const std::size_t nui = models[t]->get_nu();
66
67 xs_[t].conservativeResize(nxi);
68 us_[t].conservativeResize(nui);
69 datas_[t]->resize(nxi, ndxi, nui);
70 ixu_[t] = nvar_;
71 nconst_ += ndxi; // T*ndx eq. constraints for dynamics
72 nvar_ += ndxi + nui; // Multiple shooting, states and controls
73 }
74 ixu_[T] = nvar_;
75
76 // Initial condition
77 nconst_ += models[0]->get_state()->get_ndx();
78
79 const std::shared_ptr<ActionModelAbstract>& model =
80 problem_->get_terminalModel();
81 const std::size_t nxi = model->get_state()->get_nx();
82 const std::size_t ndxi = model->get_state()->get_ndx();
83 nvar_ += ndxi; // final node
84 xs_[T].conservativeResize(nxi);
85 datas_[T]->resize(nxi, ndxi, 0);
86}
87
88IpoptInterface::~IpoptInterface() {}
89
90bool IpoptInterface::get_nlp_info(Ipopt::Index& n, Ipopt::Index& m,
91 Ipopt::Index& nnz_jac_g,
92 Ipopt::Index& nnz_h_lag,
93 IndexStyleEnum& index_style) {
94 n = static_cast<Ipopt::Index>(nvar_); // number of variables
95 m = static_cast<Ipopt::Index>(nconst_); // number of constraints
96
97 nnz_jac_g = 0; // Jacobian nonzeros for dynamic constraints
98 nnz_h_lag = 0; // Hessian nonzeros (only lower triangular part)
99 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
100 problem_->get_runningModels();
101 const std::size_t T = problem_->get_T();
102 for (std::size_t t = 0; t < T; ++t) {
103 const std::size_t ndxi = models[t]->get_state()->get_ndx();
104 const std::size_t ndxi_next =
105 t + 1 == T ? problem_->get_terminalModel()->get_state()->get_ndx()
106 : models[t + 1]->get_state()->get_ndx();
107 const std::size_t nui = models[t]->get_nu();
108 nnz_jac_g += ndxi * (ndxi + ndxi_next + nui);
109
110 // Hessian
111 std::size_t nonzero = 0;
112 for (std::size_t i = 1; i <= (ndxi + nui); ++i) {
113 nonzero += i;
114 }
115 nnz_h_lag += nonzero;
116 }
117
118 // Initial condition
119 nnz_jac_g +=
120 models[0]->get_state()->get_ndx() * models[0]->get_state()->get_ndx();
121
122 // Hessian nonzero for the terminal cost
123 const std::size_t ndxi =
124 problem_->get_terminalModel()->get_state()->get_ndx();
125 std::size_t nonzero = 0;
126 for (std::size_t i = 1; i <= ndxi; ++i) {
127 nonzero += i;
128 }
129 nnz_h_lag += nonzero;
130
131 // use the C style indexing (0-based)
132 index_style = Ipopt::TNLP::C_STYLE;
133
134 return true;
135}
136
137#ifndef NDEBUG
138bool IpoptInterface::get_bounds_info(Ipopt::Index n, Ipopt::Number* x_l,
139 Ipopt::Number* x_u, Ipopt::Index m,
140 Ipopt::Number* g_l, Ipopt::Number* g_u) {
141#else
142bool IpoptInterface::get_bounds_info(Ipopt::Index, Ipopt::Number* x_l,
143 Ipopt::Number* x_u, Ipopt::Index,
144 Ipopt::Number* g_l, Ipopt::Number* g_u) {
145#endif
146 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
147 "Inconsistent number of decision variables");
148 assert_pretty(m == static_cast<Ipopt::Index>(nconst_),
149 "Inconsistent number of constraints");
150
151 // Adding bounds
152 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
153 problem_->get_runningModels();
154 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
155 // Running state bounds
156 const std::size_t ndxi = models[t]->get_state()->get_ndx();
157 const std::size_t nui = models[t]->get_nu();
158
159 for (std::size_t j = 0; j < ndxi; ++j) {
160 x_l[ixu_[t] + j] = std::numeric_limits<double>::lowest();
161 x_u[ixu_[t] + j] = std::numeric_limits<double>::max();
162 }
163 for (std::size_t j = 0; j < nui; ++j) {
164 x_l[ixu_[t] + ndxi + j] = models[t]->get_has_control_limits()
165 ? models[t]->get_u_lb()(j)
166 : std::numeric_limits<double>::lowest();
167 x_u[ixu_[t] + ndxi + j] = models[t]->get_has_control_limits()
168 ? models[t]->get_u_ub()(j)
169 : std::numeric_limits<double>::max();
170 }
171 }
172
173 // Final state bounds
174 const std::size_t ndxi =
175 problem_->get_terminalModel()->get_state()->get_ndx();
176 for (std::size_t j = 0; j < ndxi; j++) {
177 x_l[ixu_.back() + j] = std::numeric_limits<double>::lowest();
178 x_u[ixu_.back() + j] = std::numeric_limits<double>::max();
179 }
180
181 // Dynamics & Initial conditions (all equal to zero)
182 for (Ipopt::Index i = 0; i < static_cast<Ipopt::Index>(nconst_); ++i) {
183 g_l[i] = 0;
184 g_u[i] = 0;
185 }
186
187 return true;
188}
189
190#ifndef NDEBUG
191bool IpoptInterface::get_starting_point(Ipopt::Index n, bool init_x,
192 Ipopt::Number* x, bool init_z,
193 Ipopt::Number* /*z_L*/,
194 Ipopt::Number* /*z_U*/, Ipopt::Index m,
195 bool init_lambda,
196 Ipopt::Number* /*lambda*/) {
197#else
198bool IpoptInterface::get_starting_point(Ipopt::Index, bool /*init_x*/,
199 Ipopt::Number* x, bool, Ipopt::Number*,
200 Ipopt::Number*, Ipopt::Index, bool,
201 Ipopt::Number*) {
202#endif
203 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
204 "Inconsistent number of decision variables");
205 assert_pretty(m == static_cast<Ipopt::Index>(nconst_),
206 "Inconsistent number of constraints");
207 assert_pretty(init_x == true,
208 "Make sure to provide initial value for primal variables");
209 assert_pretty(init_z == false,
210 "Cannot provide initial value for bound multipliers");
211 assert_pretty(init_lambda == false,
212 "Cannot provide initial value for constraint multipliers");
213
214 // initialize to the given starting point
215 // State variables are always at 0 since they represent increments from the
216 // given initial point
217 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
218 problem_->get_runningModels();
219 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
220 const std::size_t ndxi = models[t]->get_state()->get_ndx();
221 const std::size_t nui = models[t]->get_nu();
222 for (std::size_t j = 0; j < ndxi; ++j) {
223 x[ixu_[t] + j] = 0;
224 }
225 for (std::size_t j = 0; j < nui; ++j) {
226 x[ixu_[t] + ndxi + j] = us_[t](j);
227 }
228 }
229 const std::size_t ndxi =
230 problem_->get_terminalModel()->get_state()->get_ndx();
231 for (std::size_t j = 0; j < ndxi; j++) {
232 x[ixu_.back() + j] = 0;
233 }
234
235 return true;
236}
237
238#ifndef NDEBUG
239bool IpoptInterface::eval_f(Ipopt::Index n, const Ipopt::Number* x,
240 bool /*new_x*/, Ipopt::Number& obj_value) {
241#else
242bool IpoptInterface::eval_f(Ipopt::Index, const Ipopt::Number* x, bool,
243 Ipopt::Number& obj_value) {
244#endif
245 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
246 "Inconsistent number of decision variables");
247
248 // Running costs
249 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
250 problem_->get_runningModels();
251 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
252 problem_->get_runningDatas();
253 const std::size_t T = problem_->get_T();
254#ifdef CROCODDYL_WITH_MULTITHREADING
255#pragma omp parallel for num_threads(problem_->get_nthreads())
256#endif
257 for (std::size_t t = 0; t < T; ++t) {
258 const std::shared_ptr<ActionModelAbstract>& model = models[t];
259 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
260 const std::size_t ndxi = model->get_state()->get_ndx();
261 const std::size_t nui = model->get_nu();
262
263 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
264 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
265 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
266 model->calc(data, datas_[t]->x, datas_[t]->u);
267 }
268
269#ifdef CROCODDYL_WITH_MULTITHREADING
270#pragma omp simd reduction(+ : obj_value)
271#endif
272 for (std::size_t t = 0; t < T; ++t) {
273 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
274 obj_value += data->cost;
275 }
276
277 // Terminal costs
278 const std::shared_ptr<ActionModelAbstract>& model =
279 problem_->get_terminalModel();
280 const std::shared_ptr<ActionDataAbstract>& data =
281 problem_->get_terminalData();
282 const std::size_t ndxi = model->get_state()->get_ndx();
283
284 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
285 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
286 model->calc(data, datas_[T]->x);
287 obj_value += data->cost;
288
289 return true;
290}
291
292#ifndef NDEBUG
293bool IpoptInterface::eval_grad_f(Ipopt::Index n, const Ipopt::Number* x,
294 bool /*new_x*/, Ipopt::Number* grad_f) {
295#else
296bool IpoptInterface::eval_grad_f(Ipopt::Index, const Ipopt::Number* x, bool,
297 Ipopt::Number* grad_f) {
298#endif
299 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
300 "Inconsistent number of decision variables");
301
302 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
303 problem_->get_runningModels();
304 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
305 problem_->get_runningDatas();
306 const std::size_t T = problem_->get_T();
307#ifdef CROCODDYL_WITH_MULTITHREADING
308#pragma omp parallel for num_threads(problem_->get_nthreads())
309#endif
310 for (std::size_t t = 0; t < T; ++t) {
311 const std::shared_ptr<ActionModelAbstract>& model = models[t];
312 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
313 const std::size_t ndxi = model->get_state()->get_ndx();
314 const std::size_t nui = model->get_nu();
315
316 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
317 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
318 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
319 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
320 datas_[t]->Jint_dx, second, setto);
321 model->calc(data, datas_[t]->x, datas_[t]->u);
322 model->calcDiff(data, datas_[t]->x, datas_[t]->u);
323 datas_[t]->Ldx.noalias() = datas_[t]->Jint_dx.transpose() * data->Lx;
324 }
325 for (std::size_t t = 0; t < T; ++t) {
326 const std::shared_ptr<ActionModelAbstract>& model = models[t];
327 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
328 const std::size_t ndxi = model->get_state()->get_ndx();
329 const std::size_t nui = model->get_nu();
330 for (std::size_t j = 0; j < ndxi; ++j) {
331 grad_f[ixu_[t] + j] = datas_[t]->Ldx(j);
332 }
333 for (std::size_t j = 0; j < nui; ++j) {
334 grad_f[ixu_[t] + ndxi + j] = data->Lu(j);
335 }
336 }
337
338 // Terminal model
339 const std::shared_ptr<ActionModelAbstract>& model =
340 problem_->get_terminalModel();
341 const std::shared_ptr<ActionDataAbstract>& data =
342 problem_->get_terminalData();
343 const std::size_t ndxi = model->get_state()->get_ndx();
344
345 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
346 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
347 model->get_state()->Jintegrate(xs_[T], datas_[T]->dx, datas_[T]->Jint_dx,
348 datas_[T]->Jint_dx, second, setto);
349 model->calc(data, datas_[T]->x);
350 model->calcDiff(data, datas_[T]->x);
351 datas_[T]->Ldx.noalias() = datas_[T]->Jint_dx.transpose() * data->Lx;
352 for (std::size_t j = 0; j < ndxi; ++j) {
353 grad_f[ixu_.back() + j] = datas_[T]->Ldx(j);
354 }
355
356 return true;
357}
358
359#ifndef NDEBUG
360bool IpoptInterface::eval_g(Ipopt::Index n, const Ipopt::Number* x,
361 bool /*new_x*/, Ipopt::Index m, Ipopt::Number* g) {
362#else
363bool IpoptInterface::eval_g(Ipopt::Index, const Ipopt::Number* x,
364 bool /*new_x*/, Ipopt::Index, Ipopt::Number* g) {
365#endif
366 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
367 "Inconsistent number of decision variables");
368 assert_pretty(m == static_cast<Ipopt::Index>(nconst_),
369 "Inconsistent number of constraints");
370
371 // Dynamic constraints
372 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
373 problem_->get_runningModels();
374 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
375 problem_->get_runningDatas();
376 const std::size_t T = problem_->get_T();
377#ifdef CROCODDYL_WITH_MULTITHREADING
378#pragma omp parallel for num_threads(problem_->get_nthreads())
379#endif
380 for (std::size_t t = 0; t < T; ++t) {
381 const std::shared_ptr<ActionModelAbstract>& model = models[t];
382 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
383 const std::size_t ndxi = model->get_state()->get_ndx();
384 const std::size_t nui = model->get_nu();
385 const std::shared_ptr<ActionModelAbstract>& model_next =
386 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
387 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
388
389 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
390 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
391 datas_[t]->dxnext =
392 Eigen::VectorXd::Map(x + ixu_[t] + ndxi + nui, ndxi_next);
393 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
394 model_next->get_state()->integrate(xs_[t + 1], datas_[t]->dxnext,
395 datas_[t]->xnext);
396 model->calc(data, datas_[t]->x, datas_[t]->u);
397 model->get_state()->diff(data->xnext, datas_[t]->xnext, datas_[t]->x_diff);
398 }
399
400 std::size_t ix = 0;
401 for (std::size_t t = 0; t < T; ++t) {
402 const std::shared_ptr<ActionModelAbstract>& model = models[t];
403 const std::size_t ndxi = model->get_state()->get_ndx();
404 for (std::size_t j = 0; j < ndxi; ++j) {
405 g[ix + j] = datas_[t]->x_diff[j];
406 }
407 ix += ndxi;
408 }
409
410 // Initial conditions
411 const std::shared_ptr<ActionModelAbstract>& model = models[0];
412 const std::size_t ndxi = model->get_state()->get_ndx();
413 datas_[0]->dx = Eigen::VectorXd::Map(x, ndxi);
414 model->get_state()->integrate(xs_[0], datas_[0]->dx, datas_[0]->x);
415 model->get_state()->diff(datas_[0]->x, problem_->get_x0(),
416 datas_[0]->x_diff); // x(0) - x_0
417 for (std::size_t j = 0; j < ndxi; j++) {
418 g[ix + j] = datas_[0]->x_diff[j];
419 }
420
421 return true;
422}
423
424#ifndef NDEBUG
425bool IpoptInterface::eval_jac_g(Ipopt::Index n, const Ipopt::Number* x,
426 bool /*new_x*/, Ipopt::Index m,
427 Ipopt::Index nele_jac, Ipopt::Index* iRow,
428 Ipopt::Index* jCol, Ipopt::Number* values) {
429#else
430bool IpoptInterface::eval_jac_g(Ipopt::Index, const Ipopt::Number* x, bool,
431 Ipopt::Index, Ipopt::Index, Ipopt::Index* iRow,
432 Ipopt::Index* jCol, Ipopt::Number* values) {
433#endif
434 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
435 "Inconsistent number of decision variables");
436 assert_pretty(m == static_cast<Ipopt::Index>(nconst_),
437 "Inconsistent number of constraints");
438
439 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
440 problem_->get_runningModels();
441 if (values == NULL) {
442 // Dynamic constraints
443 std::size_t idx = 0;
444 std::size_t ix = 0;
445 const std::size_t T = problem_->get_T();
446 for (std::size_t t = 0; t < T; ++t) {
447 const std::shared_ptr<ActionModelAbstract>& model = models[t];
448 const std::size_t ndxi = model->get_state()->get_ndx();
449 const std::size_t nui = model->get_nu();
450 const std::size_t ndxi_next =
451 t + 1 == T ? problem_->get_terminalModel()->get_state()->get_ndx()
452 : models[t + 1]->get_state()->get_ndx();
453 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
454 for (std::size_t idx_col = 0; idx_col < (ndxi + nui + ndxi_next);
455 ++idx_col) {
456 iRow[idx] = static_cast<Ipopt::Index>(ix + idx_row);
457 jCol[idx] = static_cast<Ipopt::Index>(ixu_[t] + idx_col);
458 idx++;
459 }
460 }
461 ix += ndxi;
462 }
463
464 // Initial condition
465 const std::size_t ndxi = models[0]->get_state()->get_ndx();
466 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
467 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
468 iRow[idx] = static_cast<Ipopt::Index>(ix + idx_row);
469 jCol[idx] = static_cast<Ipopt::Index>(idx_col);
470 idx++;
471 }
472 }
473
474 assert_pretty(nele_jac == static_cast<Ipopt::Index>(idx),
475 "Number of jacobian elements set does not coincide with the "
476 "total non-zero Jacobian values");
477 } else {
478 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
479 problem_->get_runningDatas();
480 // Dynamic constraints
481 const std::size_t T = problem_->get_T();
482#ifdef CROCODDYL_WITH_MULTITHREADING
483#pragma omp parallel for num_threads(problem_->get_nthreads())
484#endif
485 for (std::size_t t = 0; t < T; ++t) {
486 const std::shared_ptr<ActionModelAbstract>& model = models[t];
487 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
488 const std::shared_ptr<ActionModelAbstract>& model_next =
489 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
490 const std::size_t ndxi = model->get_state()->get_ndx();
491 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
492 const std::size_t nui = model->get_nu();
493 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
494 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
495 datas_[t]->dxnext =
496 Eigen::VectorXd::Map(x + ixu_[t] + ndxi + nui, ndxi_next);
497
498 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
499 model_next->get_state()->integrate(xs_[t + 1], datas_[t]->dxnext,
500 datas_[t]->xnext);
501 model->calcDiff(data, datas_[t]->x, datas_[t]->u);
502 model_next->get_state()->Jintegrate(
503 xs_[t + 1], datas_[t]->dxnext, datas_[t]->Jint_dxnext,
504 datas_[t]->Jint_dxnext, second,
505 setto); // datas_[t]->Jsum_dxnext == eq. 81
506 model->get_state()->Jdiff(
507 data->xnext, datas_[t]->xnext, datas_[t]->Jdiff_x,
508 datas_[t]->Jdiff_xnext,
509 both); // datas_[t+1]->Jdiff_x == eq. 83, datas_[t]->Jdiff_x == eq.82
510 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
511 datas_[t]->Jint_dx, second,
512 setto); // datas_[t]->Jsum_dx == eq. 81
513 datas_[t]->Jg_dxnext.noalias() =
514 datas_[t]->Jdiff_xnext * datas_[t]->Jint_dxnext; // chain rule
515 datas_[t]->FxJint_dx.noalias() = data->Fx * datas_[t]->Jint_dx;
516 datas_[t]->Jg_dx.noalias() = datas_[t]->Jdiff_x * datas_[t]->FxJint_dx;
517 datas_[t]->Jg_u.noalias() = datas_[t]->Jdiff_x * data->Fu;
518 }
519 std::size_t idx = 0;
520 for (std::size_t t = 0; t < T; ++t) {
521 const std::shared_ptr<ActionModelAbstract>& model = models[t];
522 const std::shared_ptr<ActionModelAbstract>& model_next =
523 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
524 const std::size_t ndxi = model->get_state()->get_ndx();
525 const std::size_t nui = model->get_nu();
526 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
527 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
528 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
529 values[idx] = datas_[t]->Jg_dx(idx_row, idx_col);
530 idx++;
531 }
532 for (std::size_t idx_col = 0; idx_col < nui; ++idx_col) {
533 values[idx] = datas_[t]->Jg_u(idx_row, idx_col);
534 idx++;
535 }
536 // This could be more optimized since there are a lot of zeros!
537 for (std::size_t idx_col = 0; idx_col < ndxi_next; ++idx_col) {
538 values[idx] = datas_[t]->Jg_dxnext(idx_row, idx_col);
539 idx++;
540 }
541 }
542 }
543
544 // Initial condition
545 const std::shared_ptr<ActionModelAbstract>& model = models[0];
546 const std::size_t ndxi = model->get_state()->get_ndx();
547 datas_[0]->dx = Eigen::VectorXd::Map(x, ndxi);
548
549 model->get_state()->integrate(xs_[0], datas_[0]->dx, datas_[0]->x);
550 model->get_state()->Jdiff(datas_[0]->x, problem_->get_x0(),
551 datas_[0]->Jdiff_x, datas_[0]->Jdiff_x, first);
552 model->get_state()->Jintegrate(xs_[0], datas_[0]->dx, datas_[0]->Jint_dx,
553 datas_[0]->Jint_dx, second, setto);
554 datas_[0]->Jg_ic.noalias() = datas_[0]->Jdiff_x * datas_[0]->Jint_dx;
555 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
556 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
557 values[idx] = datas_[0]->Jg_ic(idx_row, idx_col);
558 idx++;
559 }
560 }
561 }
562
563 return true;
564}
565
566#ifndef NDEBUG
567bool IpoptInterface::eval_h(Ipopt::Index n, const Ipopt::Number* x,
568 bool /*new_x*/, Ipopt::Number obj_factor,
569 Ipopt::Index m, const Ipopt::Number* /*lambda*/,
570 bool /*new_lambda*/, Ipopt::Index nele_hess,
571 Ipopt::Index* iRow, Ipopt::Index* jCol,
572 Ipopt::Number* values) {
573#else
574bool IpoptInterface::eval_h(Ipopt::Index, const Ipopt::Number* x, bool,
575 Ipopt::Number obj_factor, Ipopt::Index,
576 const Ipopt::Number*, bool, Ipopt::Index,
577 Ipopt::Index* iRow, Ipopt::Index* jCol,
578 Ipopt::Number* values) {
579#endif
580 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
581 "Inconsistent number of decision variables");
582 assert_pretty(m == static_cast<Ipopt::Index>(nconst_),
583 "Inconsistent number of constraints");
584
585 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
586 problem_->get_runningModels();
587 const std::size_t T = problem_->get_T();
588 if (values == NULL) {
589 // return the structure. This is a symmetric matrix, fill the lower left
590 // triangle only
591
592 // Running Costs
593 std::size_t idx = 0;
594 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
595 const std::shared_ptr<ActionModelAbstract> model = models[t];
596 const std::size_t ndxi = model->get_state()->get_ndx();
597 const std::size_t nui = model->get_nu();
598 for (std::size_t idx_row = 0; idx_row < ndxi + nui; ++idx_row) {
599 for (std::size_t idx_col = 0; idx_col < ndxi + nui; ++idx_col) {
600 // We need the lower triangular matrix
601 if (idx_col > idx_row) {
602 break;
603 }
604 iRow[idx] = static_cast<Ipopt::Index>(ixu_[t] + idx_row);
605 jCol[idx] = static_cast<Ipopt::Index>(ixu_[t] + idx_col);
606 idx++;
607 }
608 }
609 }
610
611 // Terminal costs
612 const std::size_t ndxi =
613 problem_->get_terminalModel()->get_state()->get_ndx();
614 for (std::size_t idx_row = 0; idx_row < ndxi; idx_row++) {
615 for (std::size_t idx_col = 0; idx_col < ndxi; idx_col++) {
616 // We need the lower triangular matrix
617 if (idx_col > idx_row) {
618 break;
619 }
620 iRow[idx] = static_cast<Ipopt::Index>(ixu_.back() + idx_row);
621 jCol[idx] = static_cast<Ipopt::Index>(ixu_.back() + idx_col);
622 idx++;
623 }
624 }
625
626 assert_pretty(nele_hess == static_cast<Ipopt::Index>(idx),
627 "Number of Hessian elements set does not coincide with the "
628 "total non-zero Hessian values");
629 } else {
630 // return the values. This is a symmetric matrix, fill the lower left
631 // triangle only
632 // Running Costs
633 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
634 problem_->get_runningDatas();
635#ifdef CROCODDYL_WITH_MULTITHREADING
636#pragma omp parallel for num_threads(problem_->get_nthreads())
637#endif
638 for (std::size_t t = 0; t < T; ++t) {
639 const std::shared_ptr<ActionModelAbstract>& model = models[t];
640 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
641 const std::size_t ndxi = model->get_state()->get_ndx();
642 const std::size_t nui = model->get_nu();
643 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
644 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
645
646 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
647 model->calcDiff(data, datas_[t]->x,
648 datas_[t]->u); // this might be removed
649 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
650 datas_[t]->Jint_dx, second, setto);
651 datas_[t]->Ldxdx.noalias() =
652 datas_[t]->Jint_dx.transpose() * data->Lxx * datas_[t]->Jint_dx;
653 datas_[t]->Ldxu.noalias() = datas_[t]->Jint_dx.transpose() * data->Lxu;
654 }
655
656 std::size_t idx = 0;
657 for (std::size_t t = 0; t < T; ++t) {
658 const std::shared_ptr<ActionModelAbstract>& model = models[t];
659 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
660 const std::size_t ndxi = model->get_state()->get_ndx();
661 const std::size_t nui = model->get_nu();
662 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
663 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
664 // We need the lower triangular matrix
665 if (idx_col > idx_row) {
666 break;
667 }
668 values[idx] = obj_factor * datas_[t]->Ldxdx(idx_row, idx_col);
669 idx++;
670 }
671 }
672 for (std::size_t idx_row = 0; idx_row < nui; ++idx_row) {
673 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
674 values[idx] = obj_factor * datas_[t]->Ldxu(idx_col, idx_row);
675 idx++;
676 }
677 for (std::size_t idx_col = 0; idx_col < nui; ++idx_col) {
678 if (idx_col > idx_row) {
679 break;
680 }
681 values[idx] = obj_factor * data->Luu(idx_row, idx_col);
682 idx++;
683 }
684 }
685 }
686
687 // Terminal costs
688 const std::shared_ptr<ActionModelAbstract>& model =
689 problem_->get_terminalModel();
690 const std::shared_ptr<ActionDataAbstract>& data =
691 problem_->get_terminalData();
692 const std::size_t ndxi = model->get_state()->get_ndx();
693 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
694
695 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
696 model->calc(data, datas_[T]->x);
697 model->calcDiff(data, datas_[T]->x);
698 model->get_state()->Jintegrate(xs_[T], datas_[T]->dx, datas_[T]->Jint_dx,
699 datas_[T]->Jint_dx, second, setto);
700 datas_[T]->Ldxdx.noalias() =
701 datas_[T]->Jint_dx.transpose() * data->Lxx * datas_[T]->Jint_dx;
702 for (std::size_t idx_row = 0; idx_row < ndxi; idx_row++) {
703 for (std::size_t idx_col = 0; idx_col < ndxi; idx_col++) {
704 // We need the lower triangular matrix
705 if (idx_col > idx_row) {
706 break;
707 }
708 values[idx] = datas_[T]->Ldxdx(idx_row, idx_col);
709 idx++;
710 }
711 }
712 }
713
714 return true;
715}
716
717void IpoptInterface::finalize_solution(
718 Ipopt::SolverReturn /*status*/, Ipopt::Index /*n*/, const Ipopt::Number* x,
719 const Ipopt::Number* /*z_L*/, const Ipopt::Number* /*z_U*/,
720 Ipopt::Index /*m*/, const Ipopt::Number* /*g*/,
721 const Ipopt::Number* /*lambda*/, Ipopt::Number obj_value,
722 const Ipopt::IpoptData* /*ip_data*/,
723 Ipopt::IpoptCalculatedQuantities* /*ip_cq*/) {
724 // Copy the solution to vector once solver is finished
725 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
726 problem_->get_runningModels();
727 const std::size_t T = problem_->get_T();
728 for (std::size_t t = 0; t < T; ++t) {
729 const std::size_t ndxi = models[t]->get_state()->get_ndx();
730 const std::size_t nui = models[t]->get_nu();
731 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
732 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
733
734 models[t]->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
735 xs_[t] = datas_[t]->x;
736 us_[t] = datas_[t]->u;
737 }
738 // Terminal node
739 const std::shared_ptr<ActionModelAbstract>& model =
740 problem_->get_terminalModel();
741 const std::size_t ndxi = model->get_state()->get_ndx();
742 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
743 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
744 xs_[T] = datas_[T]->x;
745
746 cost_ = obj_value;
747}
748
749bool IpoptInterface::intermediate_callback(
750 Ipopt::AlgorithmMode /*mode*/, Ipopt::Index /*iter*/,
751 Ipopt::Number /*obj_value*/, Ipopt::Number /*inf_pr*/,
752 Ipopt::Number /*inf_du*/, Ipopt::Number /*mu*/, Ipopt::Number /*d_norm*/,
753 Ipopt::Number /*regularization_size*/, Ipopt::Number /*alpha_du*/,
754 Ipopt::Number /*alpha_pr*/, Ipopt::Index /*ls_trials*/,
755 const Ipopt::IpoptData* /*ip_data*/,
756 Ipopt::IpoptCalculatedQuantities* /*ip_cq*/) {
757 return true;
758}
759
760std::shared_ptr<IpoptInterfaceData> IpoptInterface::createData(
761 const std::size_t nx, const std::size_t ndx, const std::size_t nu) {
762 return std::allocate_shared<IpoptInterfaceData>(
763 Eigen::aligned_allocator<IpoptInterfaceData>(), nx, ndx, nu);
764}
765
766void IpoptInterface::set_xs(const std::vector<Eigen::VectorXd>& xs) {
767 xs_ = xs;
768}
769
770void IpoptInterface::set_us(const std::vector<Eigen::VectorXd>& us) {
771 us_ = us;
772}
773
774std::size_t IpoptInterface::get_nvar() const { return nvar_; }
775
776std::size_t IpoptInterface::get_nconst() const { return nconst_; }
777
778const std::vector<Eigen::VectorXd>& IpoptInterface::get_xs() const {
779 return xs_;
780}
781
782const std::vector<Eigen::VectorXd>& IpoptInterface::get_us() const {
783 return us_;
784}
785
786const std::shared_ptr<ShootingProblem>& IpoptInterface::get_problem() const {
787 return problem_;
788}
789
790double IpoptInterface::get_cost() const { return cost_; }
791
792} // namespace crocoddyl
EIGEN_MAKE_ALIGNED_OPERATOR_NEW IpoptInterface(const std::shared_ptr< crocoddyl::ShootingProblem > &problem)
Initialize the Ipopt interface.