GCC Code Coverage Report

Directory: ./
File: src/core/solvers/ipopt/ipopt-iface.cpp
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 406 432 94.0%
Branches: 306 622 49.2%
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 <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
17 namespace crocoddyl {
18
19 6 IpoptInterface::IpoptInterface(const std::shared_ptr<ShootingProblem>& problem)
20 6 : problem_(problem) {
21 6 const std::size_t T = problem_->get_T();
22
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 xs_.resize(T + 1);
23
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 us_.resize(T);
24
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 datas_.resize(T + 1);
25
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 ixu_.resize(T + 1);
26
27 6 nconst_ = 0;
28 6 nvar_ = 0;
29 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
30 6 problem_->get_runningModels();
31
2/2
✓ Branch 0 taken 60 times.
✓ Branch 1 taken 6 times.
 66 for (std::size_t t = 0; t < T; ++t) {
32 60 const std::size_t nxi = models[t]->get_state()->get_nx();
33 60 const std::size_t ndxi = models[t]->get_state()->get_ndx();
34 60 const std::size_t nui = models[t]->get_nu();
35
36
1/2
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
 60 xs_[t] = models[t]->get_state()->zero();
37
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
 60 us_[t] = Eigen::VectorXd::Zero(nui);
38
1/2
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
 60 datas_[t] = createData(nxi, ndxi, nui);
39 60 ixu_[t] = nvar_;
40 60 nconst_ += ndxi; // T*ndx eq. constraints for dynamics
41 60 nvar_ += ndxi + nui; // Multiple shooting, states and controls
42 }
43 6 ixu_[T] = nvar_;
44
45 // Initial condition
46 6 nconst_ += models[0]->get_state()->get_ndx();
47
48 const std::shared_ptr<ActionModelAbstract>& model =
49 6 problem_->get_terminalModel();
50 6 const std::size_t nxi = model->get_state()->get_nx();
51 6 const std::size_t ndxi = model->get_state()->get_ndx();
52 6 nvar_ += ndxi; // final node
53
1/2
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 xs_[T] = model->get_state()->zero();
54
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 datas_[T] = createData(nxi, ndxi, 0);
55 6 }
56
57 ✗ void 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
88 24 IpoptInterface::~IpoptInterface() {}
89
90 5 bool 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 5 n = static_cast<Ipopt::Index>(nvar_); // number of variables
95 5 m = static_cast<Ipopt::Index>(nconst_); // number of constraints
96
97 5 nnz_jac_g = 0; // Jacobian nonzeros for dynamic constraints
98 5 nnz_h_lag = 0; // Hessian nonzeros (only lower triangular part)
99 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
100 5 problem_->get_runningModels();
101 5 const std::size_t T = problem_->get_T();
102
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 5 times.
 55 for (std::size_t t = 0; t < T; ++t) {
103 50 const std::size_t ndxi = models[t]->get_state()->get_ndx();
104 const std::size_t ndxi_next =
105
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 45 times.
 50 t + 1 == T ? problem_->get_terminalModel()->get_state()->get_ndx()
106 45 : models[t + 1]->get_state()->get_ndx();
107 50 const std::size_t nui = models[t]->get_nu();
108 50 nnz_jac_g += ndxi * (ndxi + ndxi_next + nui);
109
110 // Hessian
111 50 std::size_t nonzero = 0;
112
2/2
✓ Branch 0 taken 490 times.
✓ Branch 1 taken 50 times.
 540 for (std::size_t i = 1; i <= (ndxi + nui); ++i) {
113 490 nonzero += i;
114 }
115 50 nnz_h_lag += nonzero;
116 }
117
118 // Initial condition
119 5 nnz_jac_g +=
120 5 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 5 problem_->get_terminalModel()->get_state()->get_ndx();
125 5 std::size_t nonzero = 0;
126
2/2
✓ Branch 0 taken 35 times.
✓ Branch 1 taken 5 times.
 40 for (std::size_t i = 1; i <= ndxi; ++i) {
127 35 nonzero += i;
128 }
129 5 nnz_h_lag += nonzero;
130
131 // use the C style indexing (0-based)
132 5 index_style = Ipopt::TNLP::C_STYLE;
133
134 5 return true;
135 }
136
137 #ifndef NDEBUG
138 10 bool 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
142 bool 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
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 10 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
147 "Inconsistent number of decision variables");
148
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 10 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 10 problem_->get_runningModels();
154
2/2
✓ Branch 2 taken 100 times.
✓ Branch 3 taken 10 times.
 110 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
155 // Running state bounds
156 100 const std::size_t ndxi = models[t]->get_state()->get_ndx();
157 100 const std::size_t nui = models[t]->get_nu();
158
159
2/2
✓ Branch 0 taken 700 times.
✓ Branch 1 taken 100 times.
 800 for (std::size_t j = 0; j < ndxi; ++j) {
160 700 x_l[ixu_[t] + j] = std::numeric_limits<double>::lowest();
161 700 x_u[ixu_[t] + j] = std::numeric_limits<double>::max();
162 }
163
2/2
✓ Branch 0 taken 280 times.
✓ Branch 1 taken 100 times.
 380 for (std::size_t j = 0; j < nui; ++j) {
164 560 x_l[ixu_[t] + ndxi + j] = models[t]->get_has_control_limits()
165
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 280 times.
 280 ? models[t]->get_u_lb()(j)
166 280 : std::numeric_limits<double>::lowest();
167 560 x_u[ixu_[t] + ndxi + j] = models[t]->get_has_control_limits()
168
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 280 times.
 280 ? models[t]->get_u_ub()(j)
169 280 : std::numeric_limits<double>::max();
170 }
171 }
172
173 // Final state bounds
174 const std::size_t ndxi =
175 10 problem_->get_terminalModel()->get_state()->get_ndx();
176
2/2
✓ Branch 0 taken 70 times.
✓ Branch 1 taken 10 times.
 80 for (std::size_t j = 0; j < ndxi; j++) {
177 70 x_l[ixu_.back() + j] = std::numeric_limits<double>::lowest();
178 70 x_u[ixu_.back() + j] = std::numeric_limits<double>::max();
179 }
180
181 // Dynamics & Initial conditions (all equal to zero)
182
2/2
✓ Branch 0 taken 770 times.
✓ Branch 1 taken 10 times.
 780 for (Ipopt::Index i = 0; i < static_cast<Ipopt::Index>(nconst_); ++i) {
183 770 g_l[i] = 0;
184 770 g_u[i] = 0;
185 }
186
187 10 return true;
188 }
189
190 #ifndef NDEBUG
191 10 bool 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
198 bool 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
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 10 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
204 "Inconsistent number of decision variables");
205
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 10 assert_pretty(m == static_cast<Ipopt::Index>(nconst_),
206 "Inconsistent number of constraints");
207
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 10 assert_pretty(init_x == true,
208 "Make sure to provide initial value for primal variables");
209
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 10 assert_pretty(init_z == false,
210 "Cannot provide initial value for bound multipliers");
211
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 10 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 10 problem_->get_runningModels();
219
2/2
✓ Branch 2 taken 100 times.
✓ Branch 3 taken 10 times.
 110 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
220 100 const std::size_t ndxi = models[t]->get_state()->get_ndx();
221 100 const std::size_t nui = models[t]->get_nu();
222
2/2
✓ Branch 0 taken 700 times.
✓ Branch 1 taken 100 times.
 800 for (std::size_t j = 0; j < ndxi; ++j) {
223 700 x[ixu_[t] + j] = 0;
224 }
225
2/2
✓ Branch 0 taken 280 times.
✓ Branch 1 taken 100 times.
 380 for (std::size_t j = 0; j < nui; ++j) {
226 280 x[ixu_[t] + ndxi + j] = us_[t](j);
227 }
228 }
229 const std::size_t ndxi =
230 10 problem_->get_terminalModel()->get_state()->get_ndx();
231
2/2
✓ Branch 0 taken 70 times.
✓ Branch 1 taken 10 times.
 80 for (std::size_t j = 0; j < ndxi; j++) {
232 70 x[ixu_.back() + j] = 0;
233 }
234
235 10 return true;
236 }
237
238 #ifndef NDEBUG
239 12 bool IpoptInterface::eval_f(Ipopt::Index n, const Ipopt::Number* x,
240 bool /*new_x*/, Ipopt::Number& obj_value) {
241 #else
242 bool IpoptInterface::eval_f(Ipopt::Index, const Ipopt::Number* x, bool,
243 Ipopt::Number& obj_value) {
244 #endif
245
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 12 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 12 problem_->get_runningModels();
251 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
252 12 problem_->get_runningDatas();
253 12 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
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 12 times.
 132 for (std::size_t t = 0; t < T; ++t) {
258 120 const std::shared_ptr<ActionModelAbstract>& model = models[t];
259 120 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
260 120 const std::size_t ndxi = model->get_state()->get_ndx();
261 120 const std::size_t nui = model->get_nu();
262
263
1/2
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
 120 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
264
1/2
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
 120 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
265
3/6
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 120 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 120 times.
✗ Branch 17 not taken.
 120 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
266
2/4
✓ Branch 7 taken 120 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 120 times.
✗ Branch 11 not taken.
 120 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
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 12 times.
 132 for (std::size_t t = 0; t < T; ++t) {
273 120 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
274 120 obj_value += data->cost;
275 }
276
277 // Terminal costs
278 const std::shared_ptr<ActionModelAbstract>& model =
279 12 problem_->get_terminalModel();
280 const std::shared_ptr<ActionDataAbstract>& data =
281 12 problem_->get_terminalData();
282 12 const std::size_t ndxi = model->get_state()->get_ndx();
283
284
1/2
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 12 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
285
3/6
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
 12 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
286
1/2
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 12 model->calc(data, datas_[T]->x);
287 12 obj_value += data->cost;
288
289 12 return true;
290 }
291
292 #ifndef NDEBUG
293 17 bool IpoptInterface::eval_grad_f(Ipopt::Index n, const Ipopt::Number* x,
294 bool /*new_x*/, Ipopt::Number* grad_f) {
295 #else
296 bool IpoptInterface::eval_grad_f(Ipopt::Index, const Ipopt::Number* x, bool,
297 Ipopt::Number* grad_f) {
298 #endif
299
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 17 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 17 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 17 problem_->get_runningModels();
304 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
305 17 problem_->get_runningDatas();
306 17 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
2/2
✓ Branch 0 taken 170 times.
✓ Branch 1 taken 17 times.
 187 for (std::size_t t = 0; t < T; ++t) {
311 170 const std::shared_ptr<ActionModelAbstract>& model = models[t];
312 170 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
313 170 const std::size_t ndxi = model->get_state()->get_ndx();
314 170 const std::size_t nui = model->get_nu();
315
316
1/2
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
 170 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
317
1/2
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
 170 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
318
3/6
✓ Branch 9 taken 170 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 170 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 170 times.
✗ Branch 17 not taken.
 170 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
319
4/8
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 170 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 170 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 170 times.
✗ Branch 20 not taken.
 340 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
320 170 datas_[t]->Jint_dx, second, setto);
321
2/4
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 170 times.
✗ Branch 11 not taken.
 170 model->calc(data, datas_[t]->x, datas_[t]->u);
322
2/4
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 170 times.
✗ Branch 11 not taken.
 170 model->calcDiff(data, datas_[t]->x, datas_[t]->u);
323
3/6
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 170 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 170 times.
✗ Branch 14 not taken.
 170 datas_[t]->Ldx.noalias() = datas_[t]->Jint_dx.transpose() * data->Lx;
324 }
325
2/2
✓ Branch 0 taken 170 times.
✓ Branch 1 taken 17 times.
 187 for (std::size_t t = 0; t < T; ++t) {
326 170 const std::shared_ptr<ActionModelAbstract>& model = models[t];
327 170 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
328 170 const std::size_t ndxi = model->get_state()->get_ndx();
329 170 const std::size_t nui = model->get_nu();
330
2/2
✓ Branch 0 taken 1110 times.
✓ Branch 1 taken 170 times.
 1280 for (std::size_t j = 0; j < ndxi; ++j) {
331 1110 grad_f[ixu_[t] + j] = datas_[t]->Ldx(j);
332 }
333
2/2
✓ Branch 0 taken 460 times.
✓ Branch 1 taken 170 times.
 630 for (std::size_t j = 0; j < nui; ++j) {
334 460 grad_f[ixu_[t] + ndxi + j] = data->Lu(j);
335 }
336 }
337
338 // Terminal model
339 const std::shared_ptr<ActionModelAbstract>& model =
340 17 problem_->get_terminalModel();
341 const std::shared_ptr<ActionDataAbstract>& data =
342 17 problem_->get_terminalData();
343 17 const std::size_t ndxi = model->get_state()->get_ndx();
344
345
1/2
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
346
3/6
✓ Branch 9 taken 17 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 17 times.
✗ Branch 17 not taken.
 17 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
347
4/8
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 17 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 17 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 17 times.
✗ Branch 20 not taken.
 34 model->get_state()->Jintegrate(xs_[T], datas_[T]->dx, datas_[T]->Jint_dx,
348 17 datas_[T]->Jint_dx, second, setto);
349
1/2
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 model->calc(data, datas_[T]->x);
350
1/2
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 model->calcDiff(data, datas_[T]->x);
351
3/6
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
 17 datas_[T]->Ldx.noalias() = datas_[T]->Jint_dx.transpose() * data->Lx;
352
2/2
✓ Branch 0 taken 111 times.
✓ Branch 1 taken 17 times.
 128 for (std::size_t j = 0; j < ndxi; ++j) {
353 111 grad_f[ixu_.back() + j] = datas_[T]->Ldx(j);
354 }
355
356 17 return true;
357 }
358
359 #ifndef NDEBUG
360 12 bool IpoptInterface::eval_g(Ipopt::Index n, const Ipopt::Number* x,
361 bool /*new_x*/, Ipopt::Index m, Ipopt::Number* g) {
362 #else
363 bool IpoptInterface::eval_g(Ipopt::Index, const Ipopt::Number* x,
364 bool /*new_x*/, Ipopt::Index, Ipopt::Number* g) {
365 #endif
366
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 12 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
367 "Inconsistent number of decision variables");
368
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 12 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 12 problem_->get_runningModels();
374 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
375 12 problem_->get_runningDatas();
376 12 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
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 12 times.
 132 for (std::size_t t = 0; t < T; ++t) {
381 120 const std::shared_ptr<ActionModelAbstract>& model = models[t];
382 120 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
383 120 const std::size_t ndxi = model->get_state()->get_ndx();
384 120 const std::size_t nui = model->get_nu();
385 const std::shared_ptr<ActionModelAbstract>& model_next =
386
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 108 times.
 120 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
387 120 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
388
389
1/2
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
 120 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
390
1/2
✓ Branch 5 taken 120 times.
✗ Branch 6 not taken.
 120 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
391 120 datas_[t]->dxnext =
392
1/2
✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
 240 Eigen::VectorXd::Map(x + ixu_[t] + ndxi + nui, ndxi_next);
393
3/6
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 120 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 120 times.
✗ Branch 17 not taken.
 120 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
394
3/6
✓ Branch 7 taken 120 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 120 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 120 times.
✗ Branch 15 not taken.
 240 model_next->get_state()->integrate(xs_[t + 1], datas_[t]->dxnext,
395 120 datas_[t]->xnext);
396
2/4
✓ Branch 7 taken 120 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 120 times.
✗ Branch 11 not taken.
 120 model->calc(data, datas_[t]->x, datas_[t]->u);
397
3/6
✓ Branch 9 taken 120 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 120 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 120 times.
✗ Branch 17 not taken.
 120 model->get_state()->diff(data->xnext, datas_[t]->xnext, datas_[t]->x_diff);
398 }
399
400 12 std::size_t ix = 0;
401
2/2
✓ Branch 0 taken 120 times.
✓ Branch 1 taken 12 times.
 132 for (std::size_t t = 0; t < T; ++t) {
402 120 const std::shared_ptr<ActionModelAbstract>& model = models[t];
403 120 const std::size_t ndxi = model->get_state()->get_ndx();
404
2/2
✓ Branch 0 taken 760 times.
✓ Branch 1 taken 120 times.
 880 for (std::size_t j = 0; j < ndxi; ++j) {
405 760 g[ix + j] = datas_[t]->x_diff[j];
406 }
407 120 ix += ndxi;
408 }
409
410 // Initial conditions
411 12 const std::shared_ptr<ActionModelAbstract>& model = models[0];
412 12 const std::size_t ndxi = model->get_state()->get_ndx();
413
1/2
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 datas_[0]->dx = Eigen::VectorXd::Map(x, ndxi);
414
3/6
✓ Branch 9 taken 12 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
 12 model->get_state()->integrate(xs_[0], datas_[0]->dx, datas_[0]->x);
415
3/6
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 12 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 12 times.
✗ Branch 16 not taken.
 24 model->get_state()->diff(datas_[0]->x, problem_->get_x0(),
416 12 datas_[0]->x_diff); // x(0) - x_0
417
2/2
✓ Branch 0 taken 76 times.
✓ Branch 1 taken 12 times.
 88 for (std::size_t j = 0; j < ndxi; j++) {
418 76 g[ix + j] = datas_[0]->x_diff[j];
419 }
420
421 12 return true;
422 }
423
424 #ifndef NDEBUG
425 22 bool 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
430 bool 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
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 22 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 22 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
435 "Inconsistent number of decision variables");
436
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 22 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 22 assert_pretty(m == static_cast<Ipopt::Index>(nconst_),
437 "Inconsistent number of constraints");
438
439 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
440 22 problem_->get_runningModels();
441
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 17 times.
 22 if (values == NULL) {
442 // Dynamic constraints
443 5 std::size_t idx = 0;
444 5 std::size_t ix = 0;
445 5 const std::size_t T = problem_->get_T();
446
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 5 times.
 55 for (std::size_t t = 0; t < T; ++t) {
447 50 const std::shared_ptr<ActionModelAbstract>& model = models[t];
448 50 const std::size_t ndxi = model->get_state()->get_ndx();
449 50 const std::size_t nui = model->get_nu();
450 const std::size_t ndxi_next =
451
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 45 times.
 50 t + 1 == T ? problem_->get_terminalModel()->get_state()->get_ndx()
452 45 : models[t + 1]->get_state()->get_ndx();
453
2/2
✓ Branch 0 taken 350 times.
✓ Branch 1 taken 50 times.
 400 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
454
2/2
✓ Branch 0 taken 6320 times.
✓ Branch 1 taken 350 times.
 6670 for (std::size_t idx_col = 0; idx_col < (ndxi + nui + ndxi_next);
455 ++idx_col) {
456 6320 iRow[idx] = static_cast<Ipopt::Index>(ix + idx_row);
457 6320 jCol[idx] = static_cast<Ipopt::Index>(ixu_[t] + idx_col);
458 6320 idx++;
459 }
460 }
461 50 ix += ndxi;
462 }
463
464 // Initial condition
465 5 const std::size_t ndxi = models[0]->get_state()->get_ndx();
466
2/2
✓ Branch 0 taken 35 times.
✓ Branch 1 taken 5 times.
 40 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
467
2/2
✓ Branch 0 taken 265 times.
✓ Branch 1 taken 35 times.
 300 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
468 265 iRow[idx] = static_cast<Ipopt::Index>(ix + idx_row);
469 265 jCol[idx] = static_cast<Ipopt::Index>(idx_col);
470 265 idx++;
471 }
472 }
473
474
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 5 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 17 problem_->get_runningDatas();
480 // Dynamic constraints
481 17 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
2/2
✓ Branch 0 taken 170 times.
✓ Branch 1 taken 17 times.
 187 for (std::size_t t = 0; t < T; ++t) {
486 170 const std::shared_ptr<ActionModelAbstract>& model = models[t];
487 170 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
488 const std::shared_ptr<ActionModelAbstract>& model_next =
489
2/2
✓ Branch 0 taken 17 times.
✓ Branch 1 taken 153 times.
 170 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
490 170 const std::size_t ndxi = model->get_state()->get_ndx();
491 170 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
492 170 const std::size_t nui = model->get_nu();
493
1/2
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
 170 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
494
1/2
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
 170 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
495 170 datas_[t]->dxnext =
496
1/2
✓ Branch 3 taken 170 times.
✗ Branch 4 not taken.
 340 Eigen::VectorXd::Map(x + ixu_[t] + ndxi + nui, ndxi_next);
497
498
3/6
✓ Branch 9 taken 170 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 170 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 170 times.
✗ Branch 17 not taken.
 170 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
499
3/6
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 170 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 170 times.
✗ Branch 15 not taken.
 340 model_next->get_state()->integrate(xs_[t + 1], datas_[t]->dxnext,
500 170 datas_[t]->xnext);
501
2/4
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 170 times.
✗ Branch 11 not taken.
 170 model->calcDiff(data, datas_[t]->x, datas_[t]->u);
502
3/6
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 170 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 170 times.
✗ Branch 12 not taken.
 680 model_next->get_state()->Jintegrate(
503
1/2
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
 340 xs_[t + 1], datas_[t]->dxnext, datas_[t]->Jint_dxnext,
504 170 datas_[t]->Jint_dxnext, second,
505 setto); // datas_[t]->Jsum_dxnext == eq. 81
506
2/4
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 170 times.
✗ Branch 9 not taken.
 680 model->get_state()->Jdiff(
507
2/4
✓ Branch 5 taken 170 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 170 times.
✗ Branch 10 not taken.
 340 data->xnext, datas_[t]->xnext, datas_[t]->Jdiff_x,
508 170 datas_[t]->Jdiff_xnext,
509 both); // datas_[t+1]->Jdiff_x == eq. 83, datas_[t]->Jdiff_x == eq.82
510
4/8
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 170 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 170 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 170 times.
✗ Branch 20 not taken.
 340 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
511 170 datas_[t]->Jint_dx, second,
512 setto); // datas_[t]->Jsum_dx == eq. 81
513
1/2
✓ Branch 3 taken 170 times.
✗ Branch 4 not taken.
 170 datas_[t]->Jg_dxnext.noalias() =
514
1/2
✓ Branch 6 taken 170 times.
✗ Branch 7 not taken.
 340 datas_[t]->Jdiff_xnext * datas_[t]->Jint_dxnext; // chain rule
515
2/4
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 170 times.
✗ Branch 11 not taken.
 170 datas_[t]->FxJint_dx.noalias() = data->Fx * datas_[t]->Jint_dx;
516
2/4
✓ Branch 8 taken 170 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 170 times.
✗ Branch 12 not taken.
 170 datas_[t]->Jg_dx.noalias() = datas_[t]->Jdiff_x * datas_[t]->FxJint_dx;
517
2/4
✓ Branch 7 taken 170 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 170 times.
✗ Branch 11 not taken.
 170 datas_[t]->Jg_u.noalias() = datas_[t]->Jdiff_x * data->Fu;
518 }
519 17 std::size_t idx = 0;
520
2/2
✓ Branch 0 taken 170 times.
✓ Branch 1 taken 17 times.
 187 for (std::size_t t = 0; t < T; ++t) {
521 170 const std::shared_ptr<ActionModelAbstract>& model = models[t];
522 const std::shared_ptr<ActionModelAbstract>& model_next =
523
2/2
✓ Branch 0 taken 17 times.
✓ Branch 1 taken 153 times.
 170 t + 1 == T ? problem_->get_terminalModel() : models[t + 1];
524 170 const std::size_t ndxi = model->get_state()->get_ndx();
525 170 const std::size_t nui = model->get_nu();
526 170 const std::size_t ndxi_next = model_next->get_state()->get_ndx();
527
2/2
✓ Branch 0 taken 1110 times.
✓ Branch 1 taken 170 times.
 1280 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
528
2/2
✓ Branch 0 taken 8130 times.
✓ Branch 1 taken 1110 times.
 9240 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
529 8130 values[idx] = datas_[t]->Jg_dx(idx_row, idx_col);
530 8130 idx++;
531 }
532
2/2
✓ Branch 0 taken 3180 times.
✓ Branch 1 taken 1110 times.
 4290 for (std::size_t idx_col = 0; idx_col < nui; ++idx_col) {
533 3180 values[idx] = datas_[t]->Jg_u(idx_row, idx_col);
534 3180 idx++;
535 }
536 // This could be more optimized since there are a lot of zeros!
537
2/2
✓ Branch 0 taken 8130 times.
✓ Branch 1 taken 1110 times.
 9240 for (std::size_t idx_col = 0; idx_col < ndxi_next; ++idx_col) {
538 8130 values[idx] = datas_[t]->Jg_dxnext(idx_row, idx_col);
539 8130 idx++;
540 }
541 }
542 }
543
544 // Initial condition
545 17 const std::shared_ptr<ActionModelAbstract>& model = models[0];
546 17 const std::size_t ndxi = model->get_state()->get_ndx();
547
1/2
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 datas_[0]->dx = Eigen::VectorXd::Map(x, ndxi);
548
549
3/6
✓ Branch 9 taken 17 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 17 times.
✗ Branch 17 not taken.
 17 model->get_state()->integrate(xs_[0], datas_[0]->dx, datas_[0]->x);
550
4/8
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 17 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 17 times.
✗ Branch 19 not taken.
 51 model->get_state()->Jdiff(datas_[0]->x, problem_->get_x0(),
551 34 datas_[0]->Jdiff_x, datas_[0]->Jdiff_x, first);
552
4/8
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 17 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 17 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 17 times.
✗ Branch 20 not taken.
 34 model->get_state()->Jintegrate(xs_[0], datas_[0]->dx, datas_[0]->Jint_dx,
553 17 datas_[0]->Jint_dx, second, setto);
554
2/4
✓ Branch 8 taken 17 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 17 times.
✗ Branch 12 not taken.
 17 datas_[0]->Jg_ic.noalias() = datas_[0]->Jdiff_x * datas_[0]->Jint_dx;
555
2/2
✓ Branch 0 taken 111 times.
✓ Branch 1 taken 17 times.
 128 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
556
2/2
✓ Branch 0 taken 813 times.
✓ Branch 1 taken 111 times.
 924 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
557 813 values[idx] = datas_[0]->Jg_ic(idx_row, idx_col);
558 813 idx++;
559 }
560 }
561 }
562
563 22 return true;
564 }
565
566 #ifndef NDEBUG
567 12 bool 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
574 bool 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
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 12 assert_pretty(n == static_cast<Ipopt::Index>(nvar_),
581 "Inconsistent number of decision variables");
582
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 12 assert_pretty(m == static_cast<Ipopt::Index>(nconst_),
583 "Inconsistent number of constraints");
584
585 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
586 12 problem_->get_runningModels();
587 12 const std::size_t T = problem_->get_T();
588
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 7 times.
 12 if (values == NULL) {
589 // return the structure. This is a symmetric matrix, fill the lower left
590 // triangle only
591
592 // Running Costs
593 5 std::size_t idx = 0;
594
2/2
✓ Branch 2 taken 50 times.
✓ Branch 3 taken 5 times.
 55 for (std::size_t t = 0; t < problem_->get_T(); ++t) {
595 50 const std::shared_ptr<ActionModelAbstract> model = models[t];
596 50 const std::size_t ndxi = model->get_state()->get_ndx();
597 50 const std::size_t nui = model->get_nu();
598
2/2
✓ Branch 0 taken 490 times.
✓ Branch 1 taken 50 times.
 540 for (std::size_t idx_row = 0; idx_row < ndxi + nui; ++idx_row) {
599
2/2
✓ Branch 0 taken 3250 times.
✓ Branch 1 taken 50 times.
 3300 for (std::size_t idx_col = 0; idx_col < ndxi + nui; ++idx_col) {
600 // We need the lower triangular matrix
601
2/2
✓ Branch 0 taken 440 times.
✓ Branch 1 taken 2810 times.
 3250 if (idx_col > idx_row) {
602 440 break;
603 }
604 2810 iRow[idx] = static_cast<Ipopt::Index>(ixu_[t] + idx_row);
605 2810 jCol[idx] = static_cast<Ipopt::Index>(ixu_[t] + idx_col);
606 2810 idx++;
607 }
608 }
609 50 }
610
611 // Terminal costs
612 const std::size_t ndxi =
613 5 problem_->get_terminalModel()->get_state()->get_ndx();
614
2/2
✓ Branch 0 taken 35 times.
✓ Branch 1 taken 5 times.
 40 for (std::size_t idx_row = 0; idx_row < ndxi; idx_row++) {
615
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 5 times.
 185 for (std::size_t idx_col = 0; idx_col < ndxi; idx_col++) {
616 // We need the lower triangular matrix
617
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 150 times.
 180 if (idx_col > idx_row) {
618 30 break;
619 }
620 150 iRow[idx] = static_cast<Ipopt::Index>(ixu_.back() + idx_row);
621 150 jCol[idx] = static_cast<Ipopt::Index>(ixu_.back() + idx_col);
622 150 idx++;
623 }
624 }
625
626
1/10
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 5 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 7 problem_->get_runningDatas();
635 #ifdef CROCODDYL_WITH_MULTITHREADING
636 #pragma omp parallel for num_threads(problem_->get_nthreads())
637 #endif
638
2/2
✓ Branch 0 taken 70 times.
✓ Branch 1 taken 7 times.
 77 for (std::size_t t = 0; t < T; ++t) {
639 70 const std::shared_ptr<ActionModelAbstract>& model = models[t];
640 70 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
641 70 const std::size_t ndxi = model->get_state()->get_ndx();
642 70 const std::size_t nui = model->get_nu();
643
1/2
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
 70 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
644
1/2
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
 70 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
645
646
3/6
✓ Branch 9 taken 70 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 70 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
 70 model->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
647
2/4
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
 140 model->calcDiff(data, datas_[t]->x,
648 70 datas_[t]->u); // this might be removed
649
4/8
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 70 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
 140 model->get_state()->Jintegrate(xs_[t], datas_[t]->dx, datas_[t]->Jint_dx,
650 70 datas_[t]->Jint_dx, second, setto);
651
1/2
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
 70 datas_[t]->Ldxdx.noalias() =
652
3/6
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 70 times.
✗ Branch 14 not taken.
 140 datas_[t]->Jint_dx.transpose() * data->Lxx * datas_[t]->Jint_dx;
653
3/6
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 70 times.
✗ Branch 14 not taken.
 70 datas_[t]->Ldxu.noalias() = datas_[t]->Jint_dx.transpose() * data->Lxu;
654 }
655
656 7 std::size_t idx = 0;
657
2/2
✓ Branch 0 taken 70 times.
✓ Branch 1 taken 7 times.
 77 for (std::size_t t = 0; t < T; ++t) {
658 70 const std::shared_ptr<ActionModelAbstract>& model = models[t];
659 70 const std::shared_ptr<ActionDataAbstract>& data = datas[t];
660 70 const std::size_t ndxi = model->get_state()->get_ndx();
661 70 const std::size_t nui = model->get_nu();
662
2/2
✓ Branch 0 taken 410 times.
✓ Branch 1 taken 70 times.
 480 for (std::size_t idx_row = 0; idx_row < ndxi; ++idx_row) {
663
2/2
✓ Branch 0 taken 1960 times.
✓ Branch 1 taken 70 times.
 2030 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
664 // We need the lower triangular matrix
665
2/2
✓ Branch 0 taken 340 times.
✓ Branch 1 taken 1620 times.
 1960 if (idx_col > idx_row) {
666 340 break;
667 }
668 1620 values[idx] = obj_factor * datas_[t]->Ldxdx(idx_row, idx_col);
669 1620 idx++;
670 }
671 }
672
2/2
✓ Branch 0 taken 180 times.
✓ Branch 1 taken 70 times.
 250 for (std::size_t idx_row = 0; idx_row < nui; ++idx_row) {
673
2/2
✓ Branch 0 taken 1140 times.
✓ Branch 1 taken 180 times.
 1320 for (std::size_t idx_col = 0; idx_col < ndxi; ++idx_col) {
674 1140 values[idx] = obj_factor * datas_[t]->Ldxu(idx_col, idx_row);
675 1140 idx++;
676 }
677
2/2
✓ Branch 0 taken 460 times.
✓ Branch 1 taken 70 times.
 530 for (std::size_t idx_col = 0; idx_col < nui; ++idx_col) {
678
2/2
✓ Branch 0 taken 110 times.
✓ Branch 1 taken 350 times.
 460 if (idx_col > idx_row) {
679 110 break;
680 }
681 350 values[idx] = obj_factor * data->Luu(idx_row, idx_col);
682 350 idx++;
683 }
684 }
685 }
686
687 // Terminal costs
688 const std::shared_ptr<ActionModelAbstract>& model =
689 7 problem_->get_terminalModel();
690 const std::shared_ptr<ActionDataAbstract>& data =
691 7 problem_->get_terminalData();
692 7 const std::size_t ndxi = model->get_state()->get_ndx();
693
1/2
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
694
695
3/6
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
 7 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
696
1/2
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 model->calc(data, datas_[T]->x);
697
1/2
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 model->calcDiff(data, datas_[T]->x);
698
4/8
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 7 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
 14 model->get_state()->Jintegrate(xs_[T], datas_[T]->dx, datas_[T]->Jint_dx,
699 7 datas_[T]->Jint_dx, second, setto);
700
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 datas_[T]->Ldxdx.noalias() =
701
3/6
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 14 datas_[T]->Jint_dx.transpose() * data->Lxx * datas_[T]->Jint_dx;
702
2/2
✓ Branch 0 taken 41 times.
✓ Branch 1 taken 7 times.
 48 for (std::size_t idx_row = 0; idx_row < ndxi; idx_row++) {
703
2/2
✓ Branch 0 taken 196 times.
✓ Branch 1 taken 7 times.
 203 for (std::size_t idx_col = 0; idx_col < ndxi; idx_col++) {
704 // We need the lower triangular matrix
705
2/2
✓ Branch 0 taken 34 times.
✓ Branch 1 taken 162 times.
 196 if (idx_col > idx_row) {
706 34 break;
707 }
708 162 values[idx] = datas_[T]->Ldxdx(idx_row, idx_col);
709 162 idx++;
710 }
711 }
712 }
713
714 12 return true;
715 }
716
717 5 void 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 5 problem_->get_runningModels();
727 5 const std::size_t T = problem_->get_T();
728
2/2
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 5 times.
 55 for (std::size_t t = 0; t < T; ++t) {
729 50 const std::size_t ndxi = models[t]->get_state()->get_ndx();
730 50 const std::size_t nui = models[t]->get_nu();
731
1/2
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 datas_[t]->dx = Eigen::VectorXd::Map(x + ixu_[t], ndxi);
732
1/2
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 datas_[t]->u = Eigen::VectorXd::Map(x + ixu_[t] + ndxi, nui);
733
734
3/6
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 50 times.
✗ Branch 18 not taken.
 50 models[t]->get_state()->integrate(xs_[t], datas_[t]->dx, datas_[t]->x);
735 50 xs_[t] = datas_[t]->x;
736 50 us_[t] = datas_[t]->u;
737 }
738 // Terminal node
739 const std::shared_ptr<ActionModelAbstract>& model =
740 5 problem_->get_terminalModel();
741 5 const std::size_t ndxi = model->get_state()->get_ndx();
742
1/2
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 datas_[T]->dx = Eigen::VectorXd::Map(x + ixu_.back(), ndxi);
743
3/6
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 5 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
 5 model->get_state()->integrate(xs_[T], datas_[T]->dx, datas_[T]->x);
744 5 xs_[T] = datas_[T]->x;
745
746 5 cost_ = obj_value;
747 5 }
748
749 12 bool 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 12 return true;
758 }
759
760 66 std::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
1/2
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
 132 Eigen::aligned_allocator<IpoptInterfaceData>(), nx, ndx, nu);
764 }
765
766 5 void IpoptInterface::set_xs(const std::vector<Eigen::VectorXd>& xs) {
767 5 xs_ = xs;
768 5 }
769
770 5 void IpoptInterface::set_us(const std::vector<Eigen::VectorXd>& us) {
771 5 us_ = us;
772 5 }
773
774 ✗ std::size_t IpoptInterface::get_nvar() const { return nvar_; }
775
776 ✗ std::size_t IpoptInterface::get_nconst() const { return nconst_; }
777
778 10 const std::vector<Eigen::VectorXd>& IpoptInterface::get_xs() const {
779 10 return xs_;
780 }
781
782 10 const std::vector<Eigen::VectorXd>& IpoptInterface::get_us() const {
783 10 return us_;
784 }
785
786 ✗ const std::shared_ptr<ShootingProblem>& IpoptInterface::get_problem() const {
787 ✗ return problem_;
788 }
789
790 5 double IpoptInterface::get_cost() const { return cost_; }
791
792 } // namespace crocoddyl
793