GCC Code Coverage Report

Directory: ./
File: src/core/solvers/ipopt/ipopt-iface.cpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 433 0.0%
Functions: 0 22 0.0%
Branches: 0 796 0.0%
Line Branch Exec Source
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.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/core/solvers/ipopt/ipopt-iface.hpp"
10
11 #include <cmath>
12
13 namespace crocoddyl {
14
15 ✗ IpoptInterface::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
53 ✗ void 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
84 ✗ IpoptInterface::~IpoptInterface() {}
85
86 ✗ bool 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
134 ✗ bool 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
138 bool 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
187 ✗ bool 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
194 bool 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
235 ✗ bool IpoptInterface::eval_f(Ipopt::Index n, const Ipopt::Number* x,
236 bool /*new_x*/, Ipopt::Number& obj_value) {
237 #else
238 bool 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
289 ✗ bool IpoptInterface::eval_grad_f(Ipopt::Index n, const Ipopt::Number* x,
290 bool /*new_x*/, Ipopt::Number* grad_f) {
291 #else
292 bool 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
356 ✗ bool IpoptInterface::eval_g(Ipopt::Index n, const Ipopt::Number* x,
357 bool /*new_x*/, Ipopt::Index m, Ipopt::Number* g) {
358 #else
359 bool 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
421 ✗ bool 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
426 bool 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
563 ✗ bool 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
570 bool 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
713 ✗ void 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
745 ✗ bool 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
756 ✗ std::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
762 ✗ void IpoptInterface::set_xs(const std::vector<Eigen::VectorXd>& xs) {
763 ✗ xs_ = xs;
764 ✗ }
765
766 ✗ void IpoptInterface::set_us(const std::vector<Eigen::VectorXd>& us) {
767 ✗ us_ = us;
768 ✗ }
769
770 ✗ std::size_t IpoptInterface::get_nvar() const { return nvar_; }
771
772 ✗ std::size_t IpoptInterface::get_nconst() const { return nconst_; }
773
774 ✗ const std::vector<Eigen::VectorXd>& IpoptInterface::get_xs() const {
775 ✗ return xs_;
776 }
777
778 ✗ const std::vector<Eigen::VectorXd>& IpoptInterface::get_us() const {
779 ✗ return us_;
780 }
781
782 ✗ const std::shared_ptr<ShootingProblem>& IpoptInterface::get_problem() const {
783 ✗ return problem_;
784 }
785
786 ✗ double IpoptInterface::get_cost() const { return cost_; }
787
788 } // namespace crocoddyl
789