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