1 /**************************************************************************************************
2 * *
3 * This file is part of HPIPM. *
4 * *
5 * HPIPM -- High-Performance Interior Point Method. *
6 * Copyright (C) 2017-2018 by Gianluca Frison. *
7 * Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
8 * All rights reserved. *
9 * *
10 * This program is free software: you can redistribute it and/or modify *
11 * it under the terms of the GNU General Public License as published by *
12 * the Free Software Foundation, either version 3 of the License, or *
13 * (at your option) any later version *.
14 * *
15 * This program is distributed in the hope that it will be useful, *
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
18 * GNU General Public License for more details. *
19 * *
20 * You should have received a copy of the GNU General Public License *
21 * along with this program. If not, see <https://www.gnu.org/licenses/>. *
22 * *
23 * The authors designate this particular file as subject to the "Classpath" exception *
24 * as provided by the authors in the LICENSE file that accompained this code. *
25 * *
26 * Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
27 * *
28 **************************************************************************************************/
29
30
31
32 #include <stdlib.h>
33 #include <stdio.h>
34 #include <math.h>
35 #include <sys/time.h>
36
37 #include <blasfeo_target.h>
38 #include <blasfeo_common.h>
39 #include <blasfeo_v_aux_ext_dep.h>
40 #include <blasfeo_d_aux_ext_dep.h>
41 #include <blasfeo_i_aux_ext_dep.h>
42 #include <blasfeo_d_aux.h>
43 #include <blasfeo_d_blas.h>
44
45 #include "../include/hpipm_d_rk_int.h"
46 #include "../include/hpipm_d_erk_int.h"
47 #include "../include/hpipm_d_ocp_qp.h"
48 #include "../include/hpipm_d_ocp_qp_sol.h"
49 #include "../include/hpipm_d_ocp_qp_ipm.h"
50 #include "../include/hpipm_d_ocp_nlp.h"
51 #include "../include/hpipm_d_ocp_nlp_sol.h"
52 #include "../include/hpipm_d_ocp_nlp_ipm.h"
53
54
55
56 int vdeFun(double** arg, double** res, int* iw, double* w, int mem);
57
58
59
60 struct vde_fun_arg
61 {
62 int nx;
63 int nu;
64 };
65
66
67
vde_fun_model(int t,double * x,double * u,void * ode_arg,double * out)68 void vde_fun_model(int t, double *x, double *u, void *ode_arg, double *out)
69 {
70
71 struct vde_fun_arg *arg = ode_arg;
72
73 int nx = arg->nx;
74 int nu = arg->nu;
75
76 double *Su = x + nx;
77 double *Sx = x + nx + nu * nx;
78
79 double *x_out = out;
80 double *Su_out = out + nx;
81 double *Sx_out = out + nx + nu * nx;
82
83 double *casadi_arg[4];
84 double *casadi_res[3];
85
86 casadi_arg[0] = x;
87 casadi_arg[1] = Sx;
88 casadi_arg[2] = Su;
89 casadi_arg[3] = u;
90
91 casadi_res[0] = x_out;
92 casadi_res[1] = Sx_out;
93 casadi_res[2] = Su_out;
94
95 int* iw = 0;
96 double* w = 0;
97 int mem = 0;
98
99 vdeFun(casadi_arg, casadi_res, iw, w, mem);
100
101 }
102
103
104
main()105 int main()
106 {
107
108 int ii;
109
110 /************************************************
111 * problem size
112 ************************************************/
113
114 int nx_ = 4;
115 int nu_ = 1;
116 int N = 25;
117
118 /************************************************
119 * initial state and control
120 ************************************************/
121
122 double *x0 = malloc(nx_*sizeof(double));
123 x0[0] = 0.0;
124 x0[1] = 0.5;
125 x0[2] = 0.0;
126 x0[3] = 0.0;
127
128 double *u0 = malloc(nu_*sizeof(double));
129 u0[0] = 0.0;
130
131 /************************************************
132 * quadratic cost function
133 ************************************************/
134
135 double *Q; d_zeros(&Q, nx_, nx_);
136 // for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 10.0;
137 Q[0+nx_*0] = 1;
138 Q[1+nx_*1] = 1000;
139 Q[2+nx_*2] = 1;
140 Q[3+nx_*3] = 0.1;
141
142 double *R; d_zeros(&R, nu_, nu_);
143 for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 0.1;
144
145 double *S; d_zeros(&S, nu_, nx_);
146
147 double *q; d_zeros(&q, nx_, 1);
148 for(ii=0; ii<nx_; ii++) q[ii] = 0.0;
149
150 double *r; d_zeros(&r, nu_, 1);
151 for(ii=0; ii<nu_; ii++) r[ii] = 0.0;
152
153 d_print_mat(nx_, nx_, Q, nx_);
154 d_print_mat(nu_, nu_, R, nu_);
155 d_print_mat(nu_, nx_, S, nu_);
156 d_print_mat(1, nx_, q, 1);
157 d_print_mat(1, nu_, r, 1);
158
159 /************************************************
160 * integrator type and arg
161 ************************************************/
162
163 #if 1
164 // rk4
165 int nsta = 4; // number of stages
166 int expl = 1;
167 double A_rk[] = {0.0, 0.0, 0.0, 0.0,
168 0.5, 0.0, 0.0, 0.0,
169 0.0, 0.5, 0.0, 0.0,
170 0.0, 0.0, 1.0, 0.0};
171 double B_rk[] = {1.0/6.0, 1.0/3.0, 1.0/3.0, 1.0/6.0};
172 double C_rk[] = {0.0, 0.5, 0.5, 0.0};
173 #elif 1
174 // midpoint rule
175 int nsta = 2; // number of stages
176 int expl = 1;
177 double A_rk[] = {0.0, 0.0,
178 0.5, 0.0};
179 double B_rk[] = {0.0, 1.0};
180 double C_rk[] = {0.0, 0.5};
181 #else
182 // explicit euler
183 int nsta = 1; // number of stages
184 int expl = 1;
185 double A_rk[] = {0.0};
186 double B_rk[] = {1.0};
187 double C_rk[] = {0.0};
188 #endif
189
190 // erk data structure
191 int memsize_rk_data = d_memsize_rk_data(nsta);
192 printf("\nmemsize rk data %d\n", memsize_rk_data);
193 void *memory_rk_data = malloc(memsize_rk_data);
194
195 struct d_rk_data rk_data;
196 d_create_rk_data(nsta, &rk_data, memory_rk_data);
197
198 d_cvt_rowmaj_to_rk_data(expl, A_rk, B_rk, C_rk, &rk_data);
199
200 double Ts = 0.1;
201
202 // erk arg structure
203 struct d_erk_arg erk_arg;
204 erk_arg.rk_data = &rk_data;
205 erk_arg.steps = 10;
206 erk_arg.h = Ts/erk_arg.steps;
207 erk_arg.for_sens = 1; // XXX needed ???
208 erk_arg.adj_sens = 0;
209
210 /************************************************
211 * ocp qp
212 ************************************************/
213
214 int nx[N+1];
215 int nu[N+1];
216 int nb[N+1];
217 int ng[N+1];
218 int ns[N+1];
219
220 nx[0] = nx_;//0;
221 nu[0] = nu_;
222 nb[0] = nu_+nx_;
223 ng[0] = 0;
224 ns[0] = 0;
225 for(ii=1; ii<N; ii++)
226 {
227 nx[ii] = nx_;
228 nu[ii] = nu_;
229 nb[ii] = nu_;
230 ng[ii] = 0;
231 ns[ii] = 0;
232 }
233 nx[N] = nx_;
234 nu[N] = 0;
235 nb[N] = 0;//nx_;
236 ng[N] = 0;
237 ns[N] = 0;
238
239 /************************************************
240 * box & general constraints
241 ************************************************/
242
243 int *idxb0; int_zeros(&idxb0, nb[0], 1);
244 double *d_lb0; d_zeros(&d_lb0, nb[0], 1);
245 double *d_ub0; d_zeros(&d_ub0, nb[0], 1);
246 double *d_lg0; d_zeros(&d_lg0, ng[0], 1);
247 double *d_ug0; d_zeros(&d_ug0, ng[0], 1);
248 for(ii=0; ii<nb[0]; ii++)
249 {
250 if(ii<nu[0]) // input
251 {
252 d_lb0[ii] = - 10.0; // umin
253 d_ub0[ii] = 10.0; // umax
254 }
255 else // (initial) state
256 {
257 d_lb0[ii] = x0[ii-nu[0]]; // xmin
258 d_ub0[ii] = x0[ii-nu[0]]; // xmax
259 }
260 idxb0[ii] = ii;
261 }
262
263 int *idxb1; int_zeros(&idxb1, nb[1], 1);
264 double *d_lb1; d_zeros(&d_lb1, nb[1], 1);
265 double *d_ub1; d_zeros(&d_ub1, nb[1], 1);
266 double *d_lg1; d_zeros(&d_lg1, ng[1], 1);
267 double *d_ug1; d_zeros(&d_ug1, ng[1], 1);
268 for(ii=0; ii<nb[1]; ii++)
269 {
270 if(ii<nu[1]) // input
271 {
272 d_lb1[ii] = - 10.0; // umin
273 d_ub1[ii] = 10.0; // umax
274 idxb1[ii] = ii;
275 }
276 }
277
278 int *idxbN; int_zeros(&idxbN, nb[N], 1);
279 double *d_lbN; d_zeros(&d_lbN, nb[N], 1);
280 double *d_ubN; d_zeros(&d_ubN, nb[N], 1);
281 double *d_lgN; d_zeros(&d_lgN, ng[N], 1);
282 double *d_ugN; d_zeros(&d_ugN, ng[N], 1);
283 if(nb[N]==nx[N])
284 {
285 d_lbN[0] = - 1000.0; //
286 d_ubN[0] = 1000.0; //
287 idxbN[0] = 0;
288 d_lbN[1] = - 0.0; //
289 d_ubN[1] = 0.0; //
290 idxbN[1] = 1;
291 d_lbN[2] = - 10.0; //
292 d_ubN[2] = 10.0; //
293 idxbN[2] = 2;
294 d_lbN[3] = - 10.0; //
295 d_ubN[3] = 10.0; //
296 idxbN[3] = 3;
297 }
298
299 double *C0; d_zeros(&C0, ng[0], nx[0]);
300 double *D0; d_zeros(&D0, ng[0], nu[0]);
301
302 double *C1; d_zeros(&C1, ng[1], nx[1]);
303 double *D1; d_zeros(&D1, ng[1], nu[1]);
304
305 double *CN; d_zeros(&CN, ng[N], nx[N]);
306 double *DN; d_zeros(&DN, ng[N], nu[N]);
307
308 #if 0
309 // box constraints
310 int_print_mat(1, nb[0], idxb0, 1);
311 d_print_mat(1, nb[0], d_lb0, 1);
312 d_print_mat(1, nb[0], d_ub0, 1);
313 int_print_mat(1, nb[1], idxb1, 1);
314 d_print_mat(1, nb[1], d_lb1, 1);
315 d_print_mat(1, nb[1], d_ub1, 1);
316 int_print_mat(1, nb[N], idxbN, 1);
317 d_print_mat(1, nb[N], d_lbN, 1);
318 d_print_mat(1, nb[N], d_ubN, 1);
319 // general constraints
320 d_print_mat(1, ng[0], d_lg0, 1);
321 d_print_mat(1, ng[0], d_ug0, 1);
322 d_print_mat(ng[0], nu[0], D0, ng[0]);
323 d_print_mat(ng[0], nx[0], C0, ng[0]);
324 d_print_mat(1, ng[1], d_lg1, 1);
325 d_print_mat(1, ng[1], d_ug1, 1);
326 d_print_mat(ng[1], nu[1], D1, ng[1]);
327 d_print_mat(ng[1], nx[1], C1, ng[1]);
328 d_print_mat(1, ng[N], d_lgN, 1);
329 d_print_mat(1, ng[N], d_ugN, 1);
330 d_print_mat(ng[N], nu[N], DN, ng[N]);
331 d_print_mat(ng[N], nx[N], CN, ng[N]);
332 exit(1);
333 #endif
334
335 /************************************************
336 * soft constraints
337 ************************************************/
338
339 double *Zl0; d_zeros(&Zl0, ns[0], 1);
340 for(ii=0; ii<ns[0]; ii++)
341 Zl0[ii] = 1e3;
342 double *Zu0; d_zeros(&Zu0, ns[0], 1);
343 for(ii=0; ii<ns[0]; ii++)
344 Zu0[ii] = 1e3;
345 double *zl0; d_zeros(&zl0, ns[0], 1);
346 for(ii=0; ii<ns[0]; ii++)
347 zl0[ii] = 1e2;
348 double *zu0; d_zeros(&zu0, ns[0], 1);
349 for(ii=0; ii<ns[0]; ii++)
350 zu0[ii] = 1e2;
351 int *idxs0; int_zeros(&idxs0, ns[0], 1);
352 for(ii=0; ii<ns[0]; ii++)
353 idxs0[ii] = nu[0]+ii;
354
355 double *Zl1; d_zeros(&Zl1, ns[1], 1);
356 for(ii=0; ii<ns[1]; ii++)
357 Zl1[ii] = 1e3;
358 double *Zu1; d_zeros(&Zu1, ns[1], 1);
359 for(ii=0; ii<ns[1]; ii++)
360 Zu1[ii] = 1e3;
361 double *zl1; d_zeros(&zl1, ns[1], 1);
362 for(ii=0; ii<ns[1]; ii++)
363 zl1[ii] = 1e2;
364 double *zu1; d_zeros(&zu1, ns[1], 1);
365 for(ii=0; ii<ns[1]; ii++)
366 zu1[ii] = 1e2;
367 int *idxs1; int_zeros(&idxs1, ns[1], 1);
368 for(ii=0; ii<ns[1]; ii++)
369 idxs1[ii] = nu[1]+ii;
370
371 double *ZlN; d_zeros(&ZlN, ns[N], 1);
372 for(ii=0; ii<ns[N]; ii++)
373 ZlN[ii] = 1e3;
374 double *ZuN; d_zeros(&ZuN, ns[N], 1);
375 for(ii=0; ii<ns[N]; ii++)
376 ZuN[ii] = 1e3;
377 double *zlN; d_zeros(&zlN, ns[N], 1);
378 for(ii=0; ii<ns[N]; ii++)
379 zlN[ii] = 1e2;
380 double *zuN; d_zeros(&zuN, ns[N], 1);
381 for(ii=0; ii<ns[N]; ii++)
382 zuN[ii] = 1e2;
383 int *idxsN; int_zeros(&idxsN, ns[N], 1);
384 for(ii=0; ii<ns[N]; ii++)
385 idxsN[ii] = nu[N]+ii;
386
387 #if 0
388 // soft constraints
389 int_print_mat(1, ns[0], idxs0, 1);
390 d_print_mat(1, ns[0], Zl0, 1);
391 d_print_mat(1, ns[0], Zu0, 1);
392 d_print_mat(1, ns[0], zl0, 1);
393 d_print_mat(1, ns[0], zu0, 1);
394 int_print_mat(1, ns[1], idxs1, 1);
395 d_print_mat(1, ns[1], Zl1, 1);
396 d_print_mat(1, ns[1], Zu1, 1);
397 d_print_mat(1, ns[1], zl1, 1);
398 d_print_mat(1, ns[1], zu1, 1);
399 int_print_mat(1, ns[N], idxsN, 1);
400 d_print_mat(1, ns[N], ZlN, 1);
401 d_print_mat(1, ns[N], ZuN, 1);
402 d_print_mat(1, ns[N], zlN, 1);
403 d_print_mat(1, ns[N], zuN, 1);
404 #endif
405
406 /************************************************
407 * input and state reference
408 ************************************************/
409
410 double *x_ref = malloc(nx_*sizeof(double));
411 for(ii=0; ii<nx_; ii++) x_ref[ii] = 0.0;
412
413 double *u_ref = malloc(nu_*sizeof(double));
414 for(ii=0; ii<nu_; ii++) u_ref[ii] = 0.0;
415
416 /************************************************
417 * ocp nlp model
418 ************************************************/
419
420 // stage 0
421 // double *fs0 = malloc(nx_*nu_*sizeof(double));
422 // for(ii=0; ii<nx_*nu_; ii++)
423 // fs0[ii] = 0.0;
424 //
425 // struct d_ocp_nlp_model model0;
426 // model0.expl_vde = &d_van_der_pol_vde0;
427 // model0.forward_seed = fs0;
428 // model0.arg = NULL;
429
430 // stage 1
431 double *fs1 = malloc(nx_*(nu_+nx_)*sizeof(double));
432 for(ii=0; ii<nx_*(nu_+nx_); ii++)
433 fs1[ii] = 0.0;
434 for(ii=0; ii<nx_; ii++)
435 fs1[nu_*nx_+ii*(nx_+1)] = 1.0;
436
437 struct vde_fun_arg vde_arg;
438 vde_arg.nx = nx_;
439 vde_arg.nu = nu_;
440
441 struct d_ocp_nlp_model model1;
442 model1.expl_ode = NULL;
443 model1.expl_vde_for = &vde_fun_model;
444 model1.expl_vde_adj = NULL;
445 model1.forward_seed = fs1;
446 model1.arg = &vde_arg;
447
448 /************************************************
449 * ocp nlp data
450 ************************************************/
451
452 struct d_ocp_nlp_model models[N];
453 double *hQ[N+1];
454 double *hS[N+1];
455 double *hR[N+1];
456 double *hx_ref[N+1];
457 double *hu_ref[N+1];
458 double *hd_lb[N+1];
459 double *hd_ub[N+1];
460 double *hd_lg[N+1];
461 double *hd_ug[N+1];
462 double *hC[N+1];
463 double *hD[N+1];
464 int *hidxb[N+1];
465 double *hZl[N+1];
466 double *hZu[N+1];
467 double *hzl[N+1];
468 double *hzu[N+1];
469 int *hidxs[N+1]; // XXX
470
471 models[0] = model1;
472 hQ[0] = Q;
473 hS[0] = S;
474 hR[0] = R;
475 hx_ref[0] = x_ref;
476 hu_ref[0] = u_ref;
477 hidxb[0] = idxb0;
478 hd_lb[0] = d_lb0;
479 hd_ub[0] = d_ub0;
480 hd_lg[0] = d_lg0;
481 hd_ug[0] = d_ug0;
482 hC[0] = C0;
483 hD[0] = D0;
484 hZl[0] = Zl0;
485 hZu[0] = Zu0;
486 hzl[0] = zl0;
487 hzu[0] = zu0;
488 hidxs[0] = idxs0;
489 for(ii=1; ii<N; ii++)
490 {
491 models[ii] = model1;
492 hQ[ii] = Q;
493 hS[ii] = S;
494 hR[ii] = R;
495 hx_ref[ii] = x_ref;
496 hu_ref[ii] = u_ref;
497 hidxb[ii] = idxb1;
498 hd_lb[ii] = d_lb1;
499 hd_ub[ii] = d_ub1;
500 hd_lg[ii] = d_lg1;
501 hd_ug[ii] = d_ug1;
502 hC[ii] = C1;
503 hD[ii] = D1;
504 hZl[ii] = Zl1;
505 hZu[ii] = Zu1;
506 hzl[ii] = zl1;
507 hzu[ii] = zu1;
508 hidxs[ii] = idxs1;
509 }
510 hQ[N] = Q;
511 hS[N] = S;
512 hR[N] = R;
513 hx_ref[N] = x_ref;
514 hu_ref[N] = u_ref;
515 hidxb[N] = idxbN;
516 hd_lb[N] = d_lbN;
517 hd_ub[N] = d_ubN;
518 hd_lg[N] = d_lgN;
519 hd_ug[N] = d_ugN;
520 hC[N] = CN;
521 hD[N] = DN;
522 hZl[N] = ZlN;
523 hZu[N] = ZuN;
524 hzl[N] = zlN;
525 hzu[N] = zuN;
526 hidxs[N] = idxsN;
527
528 /************************************************
529 * ocp nlp
530 ************************************************/
531
532 int nlp_size = d_memsize_ocp_nlp(N, nx, nu, nb, ng, ns);
533 printf("\nnlpsize = %d\n", nlp_size);
534 void *nlp_mem = malloc(nlp_size);
535
536 struct d_ocp_nlp nlp;
537 d_create_ocp_nlp(N, nx, nu, nb, ng, ns, &nlp, nlp_mem);
538
539 d_cvt_colmaj_to_ocp_nlp(models, hQ, hS, hR, hx_ref, hu_ref, hidxb, hd_lb, hd_ub, hC, hD, hd_lg, hd_ug, hZl, hZu, hzl, hzu, hidxs, &nlp);
540
541 /************************************************
542 * ocp nlp sol
543 ************************************************/
544
545 int nlp_sol_size = d_memsize_ocp_nlp_sol(N, nx, nu, nb, ng, ns);
546 printf("\nnlp sol size = %d\n", nlp_sol_size);
547 void *nlp_sol_mem = malloc(nlp_sol_size);
548
549 struct d_ocp_nlp_sol nlp_sol;
550 d_create_ocp_nlp_sol(N, nx, nu, nb, ng, ns, &nlp_sol, nlp_sol_mem);
551
552 /************************************************
553 * ocp nlp ipm arg
554 ************************************************/
555
556 int ipm_arg_size = d_memsize_ocp_nlp_ipm_arg(&nlp);
557 printf("\nipm arg size = %d\n", ipm_arg_size);
558 void *ipm_arg_mem = malloc(ipm_arg_size);
559
560 struct d_ocp_nlp_ipm_arg ipm_arg;
561 d_create_ocp_nlp_ipm_arg(&nlp, &ipm_arg, ipm_arg_mem);
562 d_set_default_ocp_nlp_ipm_arg(&ipm_arg);
563
564 struct d_erk_arg erk_args[N];
565 for(ii=0; ii<N; ii++)
566 erk_args[ii] = erk_arg;
567
568 // struct d_ocp_nlp_ipm_arg ipm_arg;
569 ipm_arg.erk_arg = erk_args;
570 // ipm_arg.alpha_min = 1e-8;
571 ipm_arg.nlp_res_g_max = 1e-6;
572 ipm_arg.nlp_res_b_max = 1e-8;
573 ipm_arg.nlp_res_d_max = 1e-8;
574 ipm_arg.nlp_res_m_max = 1e-8;
575 // ipm_arg.nlp_iter_max = 100;
576 // ipm_arg.stat_max = 100;
577 // ipm_arg.mu0 = 1000.0;
578 // ipm_arg.pred_corr = 1;
579
580 /************************************************
581 * ocp nlp ipm ws
582 ************************************************/
583
584 int nlp_ws_size = d_memsize_ocp_nlp_ipm(&nlp, &ipm_arg);
585 printf("\nnlp ws size = %d\n", nlp_ws_size);
586 void *nlp_ws_mem = malloc(nlp_ws_size);
587
588 struct d_ocp_nlp_ipm_workspace ipm_ws;
589 d_create_ocp_nlp_ipm(&nlp, &ipm_arg, &ipm_ws, nlp_ws_mem);
590
591 /************************************************
592 * ocp nlp ipm
593 ************************************************/
594
595 int nlp_return;
596
597 struct timeval tv0, tv1;
598 int rep, nrep = 100;
599
600 gettimeofday(&tv0, NULL); // start
601
602 for(rep=0; rep<nrep; rep++)
603 {
604 nlp_return = d_solve_ocp_nlp_ipm(&nlp, &nlp_sol, &ipm_arg, &ipm_ws);
605 }
606
607 gettimeofday(&tv1, NULL); // stop
608
609 double time_ocp_nlp_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
610
611 /************************************************
612 * extract and print solution
613 ************************************************/
614
615 double *u[N+1]; for(ii=0; ii<=N; ii++) d_zeros(u+ii, nu[ii], 1);
616 double *x[N+1]; for(ii=0; ii<=N; ii++) d_zeros(x+ii, nx[ii], 1);
617 double *ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(ls+ii, ns[ii], 1);
618 double *us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(us+ii, ns[ii], 1);
619 double *pi[N]; for(ii=0; ii<N; ii++) d_zeros(pi+ii, nx[ii+1], 1);
620 double *lam_lb[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lb+ii, nb[ii], 1);
621 double *lam_ub[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ub+ii, nb[ii], 1);
622 double *lam_lg[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lg+ii, ng[ii], 1);
623 double *lam_ug[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ug+ii, ng[ii], 1);
624 double *lam_ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ls+ii, ns[ii], 1);
625 double *lam_us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_us+ii, ns[ii], 1);
626
627 d_cvt_ocp_nlp_sol_to_colmaj(&nlp, &nlp_sol, u, x, ls, us, pi, lam_lb, lam_ub, lam_lg, lam_ug, lam_ls, lam_us);
628
629 #if 1
630 printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
631 d_print_exp_tran_mat(5, ipm_ws.iter, ipm_ws.ipm_workspace->stat, 5);
632
633 printf("\nsolution\n\n");
634 printf("\nu\n");
635 for(ii=0; ii<=N; ii++)
636 d_print_mat(1, nu[ii], u[ii], 1);
637 printf("\nx\n");
638 for(ii=0; ii<=N; ii++)
639 d_print_mat(1, nx[ii], x[ii], 1);
640 // printf("\nls\n");
641 // for(ii=0; ii<=N; ii++)
642 // d_print_mat(1, ns[ii], ls[ii], 1);
643 // printf("\nus\n");
644 // for(ii=0; ii<=N; ii++)
645 // d_print_mat(1, ns[ii], us[ii], 1);
646 printf("\npi\n");
647 for(ii=0; ii<N; ii++)
648 d_print_mat(1, nx[ii+1], pi[ii], 1);
649 printf("\nlam_lb\n");
650 for(ii=0; ii<=N; ii++)
651 d_print_mat(1, nb[ii], lam_lb[ii], 1);
652 printf("\nlam_ub\n");
653 for(ii=0; ii<=N; ii++)
654 d_print_mat(1, nb[ii], lam_ub[ii], 1);
655 // printf("\nlam_lg\n");
656 // for(ii=0; ii<=N; ii++)
657 // d_print_mat(1, ng[ii], lam_lg[ii], 1);
658 // printf("\nlam_ug\n");
659 // for(ii=0; ii<=N; ii++)
660 // d_print_mat(1, ng[ii], lam_ug[ii], 1);
661 // printf("\nlam_ls\n");
662 // for(ii=0; ii<=N; ii++)
663 // d_print_mat(1, ns[ii], lam_ls[ii], 1);
664 // printf("\nlam_us\n");
665 // for(ii=0; ii<=N; ii++)
666 // d_print_mat(1, ns[ii], lam_us[ii], 1);
667
668 // printf("\nt_lb\n");
669 // for(ii=0; ii<=N; ii++)
670 // d_print_mat(1, nb[ii], (nlp_sol.t+ii)->pa, 1);
671 // printf("\nt_ub\n");
672 // for(ii=0; ii<=N; ii++)
673 // d_print_mat(1, nb[ii], (nlp_sol.t+ii)->pa+nb[ii]+ng[ii], 1);
674 // printf("\nt_lg\n");
675 // for(ii=0; ii<=N; ii++)
676 // d_print_mat(1, ng[ii], (nlp_sol.t+ii)->pa+nb[ii], 1);
677 // printf("\nt_ug\n");
678 // for(ii=0; ii<=N; ii++)
679 // d_print_mat(1, ng[ii], (nlp_sol.t+ii)->pa+2*nb[ii]+ng[ii], 1);
680 // printf("\nt_ls\n");
681 // for(ii=0; ii<=N; ii++)
682 // d_print_mat(1, ns[ii], (nlp_sol.t+ii)->pa+2*nb[ii]+2*ng[ii], 1);
683 // printf("\nt_us\n");
684 // for(ii=0; ii<=N; ii++)
685 // d_print_mat(1, ns[ii], (nlp_sol.t+ii)->pa+2*nb[ii]+2*ng[ii]+ns[ii], 1);
686 #endif
687
688 /************************************************
689 * print ipm statistics
690 ************************************************/
691
692 printf("\nnlp_res_g = %e, nlp_res_b = %e, nlp_res_d = %e, nlp_res_m = %e\n", ipm_ws.nlp_res_g, ipm_ws.nlp_res_b, ipm_ws.nlp_res_d, ipm_ws.nlp_res_m);
693 printf("\nocp nlp ipm: iter = %d, time = %e [s]\n\n", ipm_ws.iter, time_ocp_nlp_ipm);
694
695 /************************************************
696 * free memory
697 ************************************************/
698
699 free(x0);
700 free(u0);
701
702 /************************************************
703 * return
704 ************************************************/
705
706 return 0;
707
708 }
709