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