1 /**************************************************************************************************
2 * *
3 * This file is part of HPIPM. *
4 * *
5 * HPIPM -- High-Performance Interior Point Method. *
6 * Copyright (C) 2019 by Gianluca Frison. *
7 * Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
8 * All rights reserved. *
9 * *
10 * The 2-Clause BSD License *
11 * *
12 * Redistribution and use in source and binary forms, with or without *
13 * modification, are permitted provided that the following conditions are met: *
14 * *
15 * 1. Redistributions of source code must retain the above copyright notice, this *
16 * list of conditions and the following disclaimer. *
17 * 2. Redistributions in binary form must reproduce the above copyright notice, *
18 * this list of conditions and the following disclaimer in the documentation *
19 * and/or other materials provided with the distribution. *
20 * *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND *
22 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED *
23 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE *
24 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR *
25 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES *
26 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
27 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND *
28 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT *
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS *
30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
31 * *
32 * Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
33 * *
34 **************************************************************************************************/
35
36 #include <stdlib.h>
37 #include <stdio.h>
38 #include <math.h>
39 #include <sys/time.h>
40
41 #include <blasfeo_target.h>
42 #include <blasfeo_common.h>
43 #include <blasfeo_v_aux_ext_dep.h>
44 #include <blasfeo_s_aux_ext_dep.h>
45 #include <blasfeo_i_aux_ext_dep.h>
46 #include <blasfeo_s_aux.h>
47 #include <blasfeo_s_blas.h>
48
49 #include "../include/hpipm_s_ocp_qp.h"
50 #include "../include/hpipm_s_ocp_qp_sol.h"
51 #include "../include/hpipm_s_ocp_qp_ipm_hard.h"
52
53 #include "s_tools.h"
54
55
56
57 #define KEEP_X0 0
58
59 // printing
60 #define PRINT 1
61
62 /************************************************
63 Mass-spring system: nx/2 masses connected each other with springs (in a row), and the first and the last one to walls. nu (<=nx) controls act on the first nu masses. The system is sampled with sampling time Ts.
64 ************************************************/
mass_spring_system(float Ts,int nx,int nu,int N,float * A,float * B,float * b,float * x0)65 void mass_spring_system(float Ts, int nx, int nu, int N, float *A, float *B, float *b, float *x0)
66 {
67
68 int nx2 = nx*nx;
69
70 int info = 0;
71
72 int pp = nx/2; // number of masses
73
74 /************************************************
75 * build the continuous time system
76 ************************************************/
77
78 float *T; s_zeros(&T, pp, pp);
79 int ii;
80 for(ii=0; ii<pp; ii++) T[ii*(pp+1)] = -2;
81 for(ii=0; ii<pp-1; ii++) T[ii*(pp+1)+1] = 1;
82 for(ii=1; ii<pp; ii++) T[ii*(pp+1)-1] = 1;
83
84 float *Z; s_zeros(&Z, pp, pp);
85 float *I; s_zeros(&I, pp, pp); for(ii=0; ii<pp; ii++) I[ii*(pp+1)]=1.0; // = eye(pp);
86 float *Ac; s_zeros(&Ac, nx, nx);
87 smcopy(pp, pp, Z, pp, Ac, nx);
88 smcopy(pp, pp, T, pp, Ac+pp, nx);
89 smcopy(pp, pp, I, pp, Ac+pp*nx, nx);
90 smcopy(pp, pp, Z, pp, Ac+pp*(nx+1), nx);
91 free(T);
92 free(Z);
93 free(I);
94
95 s_zeros(&I, nu, nu); for(ii=0; ii<nu; ii++) I[ii*(nu+1)]=1.0; //I = eye(nu);
96 float *Bc; s_zeros(&Bc, nx, nu);
97 smcopy(nu, nu, I, nu, Bc+pp, nx);
98 free(I);
99
100 /************************************************
101 * compute the discrete time system
102 ************************************************/
103
104 float *bb; s_zeros(&bb, nx, 1);
105 smcopy(nx, 1, bb, nx, b, nx);
106
107 smcopy(nx, nx, Ac, nx, A, nx);
108 sscal_3l(nx2, Ts, A);
109 expm(nx, A);
110
111 s_zeros(&T, nx, nx);
112 s_zeros(&I, nx, nx); for(ii=0; ii<nx; ii++) I[ii*(nx+1)]=1.0; //I = eye(nx);
113 smcopy(nx, nx, A, nx, T, nx);
114 saxpy_3l(nx2, -1.0, I, T);
115 sgemm_nn_3l(nx, nu, nx, T, nx, Bc, nx, B, nx);
116 free(T);
117 free(I);
118
119 int *ipiv = (int *) malloc(nx*sizeof(int));
120 sgesv_3l(nx, nu, Ac, nx, ipiv, B, nx, &info);
121 free(ipiv);
122
123 free(Ac);
124 free(Bc);
125 free(bb);
126
127
128 /************************************************
129 * initial state
130 ************************************************/
131
132 if(nx==4)
133 {
134 x0[0] = 5;
135 x0[1] = 10;
136 x0[2] = 15;
137 x0[3] = 20;
138 }
139 else
140 {
141 int jj;
142 for(jj=0; jj<nx; jj++)
143 x0[jj] = 1;
144 }
145
146 }
147
148
149
main()150 int main()
151 {
152
153
154 // local variables
155
156 int ii, jj;
157
158 int rep, nrep=1000;
159
160 struct timeval tv0, tv1;
161
162
163
164 // problem size
165
166 int nx_ = 16; // number of states (it has to be even for the mass-spring system test problem)
167 int nu_ = 7; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem)
168 int N = 5; // horizon lenght
169
170
171
172 // stage-wise variant size
173
174 int nx[N+1];
175 #if KEEP_X0
176 nx[0] = nx_;
177 #else
178 nx[0] = 0;
179 #endif
180 for(ii=1; ii<=N; ii++)
181 nx[ii] = nx_;
182 // nx[N] = 0;
183
184 int nu[N+1];
185 for(ii=0; ii<N; ii++)
186 nu[ii] = nu_;
187 nu[N] = 0;
188
189 #if 1
190 int nb[N+1];
191 #if KEEP_X0
192 nb[0] = nu[0]+nx[0]/2;
193 #else
194 nb[0] = nu[0];
195 #endif
196 for(ii=1; ii<N; ii++)
197 nb[ii] = nu[1]+nx[1]/2;
198 nb[N] = nx[N]/2;
199
200 int ng[N+1];
201 ng[0] = 0;
202 for(ii=1; ii<N; ii++)
203 ng[ii] = 0;
204 ng[N] = 0;
205 #elif 0
206 int nb[N+1];
207 nb[0] = 0;
208 for(ii=1; ii<N; ii++)
209 nb[ii] = 0;
210 nb[N] = 0;
211
212 int ng[N+1];
213 #if KEEP_X0
214 ng[0] = nu[0]+nx[0]/2;
215 #else
216 ng[0] = nu[0];
217 #endif
218 for(ii=1; ii<N; ii++)
219 ng[ii] = nu[1]+nx[1]/2;
220 ng[N] = nx[N]/2;
221 #else
222 int nb[N+1];
223 nb[0] = nu[0] + nx[0]/2;
224 for(ii=1; ii<N; ii++)
225 nb[ii] = nu[ii] + nx[ii]/2;
226 nb[N] = nu[N] + nx[N]/2;
227
228 int ng[N+1];
229 #if KEEP_X0
230 ng[0] = nx[0]/2;
231 #else
232 ng[0] = 0;
233 #endif
234 for(ii=1; ii<N; ii++)
235 ng[ii] = nx[1]/2;
236 ng[N] = nx[N]/2;
237 #endif
238
239 /************************************************
240 * dynamical system
241 ************************************************/
242
243 float *A; s_zeros(&A, nx_, nx_); // states update matrix
244
245 float *B; s_zeros(&B, nx_, nu_); // inputs matrix
246
247 float *b; s_zeros(&b, nx_, 1); // states offset
248 float *x0; s_zeros(&x0, nx_, 1); // initial state
249
250 float Ts = 0.5; // sampling time
251 mass_spring_system(Ts, nx_, nu_, N, A, B, b, x0);
252
253 for(jj=0; jj<nx_; jj++)
254 b[jj] = 0.1;
255
256 for(jj=0; jj<nx_; jj++)
257 x0[jj] = 0;
258 x0[0] = 2.5;
259 x0[1] = 2.5;
260
261 float *b0; s_zeros(&b0, nx_, 1);
262 sgemv_n_3l(nx_, nx_, A, nx_, x0, b0);
263 saxpy_3l(nx_, 1.0, b, b0);
264
265 #if PRINT
266 s_print_mat(nx_, nx_, A, nx_);
267 s_print_mat(nx_, nu_, B, nu_);
268 s_print_mat(1, nx_, b, 1);
269 s_print_mat(1, nx_, x0, 1);
270 s_print_mat(1, nx_, b0, 1);
271 #endif
272
273 /************************************************
274 * cost function
275 ************************************************/
276
277 float *Q; s_zeros(&Q, nx_, nx_);
278 for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 1.0;
279
280 float *R; s_zeros(&R, nu_, nu_);
281 for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 2.0;
282
283 float *S; s_zeros(&S, nu_, nx_);
284
285 float *q; s_zeros(&q, nx_, 1);
286 for(ii=0; ii<nx_; ii++) q[ii] = 0.1;
287
288 float *r; s_zeros(&r, nu_, 1);
289 for(ii=0; ii<nu_; ii++) r[ii] = 0.2;
290
291 float *r0; s_zeros(&r0, nu_, 1);
292 sgemv_n_3l(nu_, nx_, S, nu_, x0, r0);
293 saxpy_3l(nu_, 1.0, r, r0);
294
295 #if PRINT
296 s_print_mat(nx_, nx_, Q, nx_);
297 s_print_mat(nu_, nu_, R, nu_);
298 s_print_mat(nu_, nx_, S, nu_);
299 s_print_mat(1, nx_, q, 1);
300 s_print_mat(1, nu_, r, 1);
301 s_print_mat(1, nu_, r0, 1);
302 #endif
303
304 // maximum element in cost functions
305 float mu0 = 2.0;
306
307 /************************************************
308 * box & general constraints
309 ************************************************/
310
311 int *idxb0; int_zeros(&idxb0, nb[0], 1);
312 float *d_lb0; s_zeros(&d_lb0, nb[0], 1);
313 float *d_ub0; s_zeros(&d_ub0, nb[0], 1);
314 float *d_lg0; s_zeros(&d_lg0, ng[0], 1);
315 float *d_ug0; s_zeros(&d_ug0, ng[0], 1);
316 for(ii=0; ii<nb[0]; ii++)
317 {
318 if(ii<nu[0]) // input
319 {
320 d_lb0[ii] = - 0.5; // umin
321 d_ub0[ii] = 0.5; // umax
322 }
323 else // state
324 {
325 d_lb0[ii] = - 4.0; // xmin
326 d_ub0[ii] = 4.0; // xmax
327 }
328 idxb0[ii] = ii;
329 }
330 for(ii=0; ii<ng[0]; ii++)
331 {
332 if(ii<nu[0]-nb[0]) // input
333 {
334 d_lg0[ii] = - 0.5; // umin
335 d_ug0[ii] = 0.5; // umax
336 }
337 else // state
338 {
339 d_lg0[ii] = - 4.0; // xmin
340 d_ug0[ii] = 4.0; // xmax
341 }
342 }
343
344 int *idxb1; int_zeros(&idxb1, nb[1], 1);
345 float *d_lb1; s_zeros(&d_lb1, nb[1], 1);
346 float *d_ub1; s_zeros(&d_ub1, nb[1], 1);
347 float *d_lg1; s_zeros(&d_lg1, ng[1], 1);
348 float *d_ug1; s_zeros(&d_ug1, ng[1], 1);
349 for(ii=0; ii<nb[1]; ii++)
350 {
351 if(ii<nu[1]) // input
352 {
353 d_lb1[ii] = - 0.5; // umin
354 d_ub1[ii] = 0.5; // umax
355 }
356 else // state
357 {
358 d_lb1[ii] = - 4.0; // xmin
359 d_ub1[ii] = 4.0; // xmax
360 }
361 idxb1[ii] = ii;
362 }
363 for(ii=0; ii<ng[1]; ii++)
364 {
365 if(ii<nu[1]-nb[1]) // input
366 {
367 d_lg1[ii] = - 0.5; // umin
368 d_ug1[ii] = 0.5; // umax
369 }
370 else // state
371 {
372 d_lg1[ii] = - 4.0; // xmin
373 d_ug1[ii] = 4.0; // xmax
374 }
375 }
376
377
378 int *idxbN; int_zeros(&idxbN, nb[N], 1);
379 float *d_lbN; s_zeros(&d_lbN, nb[N], 1);
380 float *d_ubN; s_zeros(&d_ubN, nb[N], 1);
381 float *d_lgN; s_zeros(&d_lgN, ng[N], 1);
382 float *d_ugN; s_zeros(&d_ugN, ng[N], 1);
383 for(ii=0; ii<nb[N]; ii++)
384 {
385 d_lbN[ii] = - 4.0; // xmin
386 d_ubN[ii] = 4.0; // xmax
387 idxbN[ii] = ii;
388 }
389 for(ii=0; ii<ng[N]; ii++)
390 {
391 d_lgN[ii] = - 4.0; // dmin
392 d_ugN[ii] = 4.0; // dmax
393 }
394
395 float *C0; s_zeros(&C0, ng[0], nx[0]);
396 float *D0; s_zeros(&D0, ng[0], nu[0]);
397 for(ii=0; ii<nu[0]-nb[0] & ii<ng[0]; ii++)
398 D0[ii+(nb[0]+ii)*ng[0]] = 1.0;
399 for(; ii<ng[0]; ii++)
400 C0[ii+(nb[0]+ii-nu[0])*ng[0]] = 1.0;
401
402 float *C1; s_zeros(&C1, ng[1], nx[1]);
403 float *D1; s_zeros(&D1, ng[1], nu[1]);
404 for(ii=0; ii<nu[1]-nb[1] & ii<ng[1]; ii++)
405 D1[ii+(nb[1]+ii)*ng[1]] = 1.0;
406 for(; ii<ng[1]; ii++)
407 C1[ii+(nb[1]+ii-nu[1])*ng[1]] = 1.0;
408
409 float *CN; s_zeros(&CN, ng[N], nx[N]);
410 float *DN; s_zeros(&DN, ng[N], nu[N]);
411 for(ii=0; ii<nu[N]-nb[N] & ii<ng[N]; ii++)
412 DN[ii+(nb[N]+ii)*ng[N]] = 1.0;
413 for(; ii<ng[N]; ii++)
414 CN[ii+(nb[N]+ii-nu[N])*ng[N]] = 1.0;
415
416 #if PRINT
417 // box constraints
418 int_print_mat(1, nb[0], idxb0, 1);
419 s_print_mat(1, nb[0], d_lb0, 1);
420 s_print_mat(1, nb[0], d_ub0, 1);
421 int_print_mat(1, nb[1], idxb1, 1);
422 s_print_mat(1, nb[1], d_lb1, 1);
423 s_print_mat(1, nb[1], d_ub1, 1);
424 int_print_mat(1, nb[N], idxbN, 1);
425 s_print_mat(1, nb[N], d_lbN, 1);
426 s_print_mat(1, nb[N], d_ubN, 1);
427 // general constraints
428 s_print_mat(1, ng[0], d_lg0, 1);
429 s_print_mat(1, ng[0], d_ug0, 1);
430 s_print_mat(ng[0], nu[0], D0, ng[0]);
431 s_print_mat(ng[0], nx[0], C0, ng[0]);
432 s_print_mat(1, ng[1], d_lg1, 1);
433 s_print_mat(1, ng[1], d_ug1, 1);
434 s_print_mat(ng[1], nu[1], D1, ng[1]);
435 s_print_mat(ng[1], nx[1], C1, ng[1]);
436 s_print_mat(1, ng[N], d_lgN, 1);
437 s_print_mat(1, ng[N], d_ugN, 1);
438 s_print_mat(ng[N], nu[N], DN, ng[N]);
439 s_print_mat(ng[N], nx[N], CN, ng[N]);
440 #endif
441
442 /************************************************
443 * array of matrices
444 ************************************************/
445
446 float *hA[N];
447 float *hB[N];
448 float *hb[N];
449 float *hQ[N+1];
450 float *hS[N+1];
451 float *hR[N+1];
452 float *hq[N+1];
453 float *hr[N+1];
454 float *hd_lb[N+1];
455 float *hd_ub[N+1];
456 float *hd_lg[N+1];
457 float *hd_ug[N+1];
458 float *hC[N+1];
459 float *hD[N+1];
460 int *hidxb[N+1];
461
462 hA[0] = A;
463 hB[0] = B;
464 hb[0] = b0;
465 hQ[0] = Q;
466 hS[0] = S;
467 hR[0] = R;
468 hq[0] = q;
469 hr[0] = r0;
470 hidxb[0] = idxb0;
471 hd_lb[0] = d_lb0;
472 hd_ub[0] = d_ub0;
473 hd_lg[0] = d_lg0;
474 hd_ug[0] = d_ug0;
475 hC[0] = C0;
476 hD[0] = D0;
477 for(ii=1; ii<N; ii++)
478 {
479 hA[ii] = A;
480 hB[ii] = B;
481 hb[ii] = b;
482 hQ[ii] = Q;
483 hS[ii] = S;
484 hR[ii] = R;
485 hq[ii] = q;
486 hr[ii] = r;
487 hidxb[ii] = idxb1;
488 hd_lb[ii] = d_lb1;
489 hd_ub[ii] = d_ub1;
490 hd_lg[ii] = d_lg1;
491 hd_ug[ii] = d_ug1;
492 hC[ii] = C1;
493 hD[ii] = D1;
494 }
495 hQ[N] = Q;
496 hS[N] = S;
497 hR[N] = R;
498 hq[N] = q;
499 hr[N] = r;
500 hidxb[N] = idxbN;
501 hd_lb[N] = d_lbN;
502 hd_ub[N] = d_ubN;
503 hd_lg[N] = d_lgN;
504 hd_ug[N] = d_ugN;
505 hC[N] = CN;
506 hD[N] = DN;
507
508 /************************************************
509 * ocp qp
510 ************************************************/
511
512 int qp_size = s_memsize_ocp_qp(N, nx, nu, nb, ng);
513 printf("\nqp size = %d\n", qp_size);
514 void *qp_mem = malloc(qp_size);
515
516 struct s_ocp_qp qp;
517 s_create_ocp_qp(N, nx, nu, nb, ng, &qp, qp_mem);
518 s_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hd_lb, hd_ub, hC, hD, hd_lg, hd_ug, &qp);
519 #if 0
520 printf("\nN = %d\n", qp.N);
521 for(ii=0; ii<N; ii++)
522 blasfeo_print_smat(qp.nu[ii]+qp.nx[ii]+1, qp.nx[ii+1], qp.BAbt+ii, 0, 0);
523 for(ii=0; ii<N; ii++)
524 blasfeo_print_tran_svec(qp.nx[ii+1], qp.b+ii, 0);
525 for(ii=0; ii<=N; ii++)
526 blasfeo_print_smat(qp.nu[ii]+qp.nx[ii]+1, qp.nu[ii]+qp.nx[ii], qp.RSQrq+ii, 0, 0);
527 for(ii=0; ii<=N; ii++)
528 blasfeo_print_tran_svec(qp.nu[ii]+qp.nx[ii], qp.rq+ii, 0);
529 for(ii=0; ii<=N; ii++)
530 int_print_mat(1, nb[ii], qp.idxb[ii], 1);
531 for(ii=0; ii<=N; ii++)
532 blasfeo_print_tran_svec(qp.nb[ii], qp.d_lb+ii, 0);
533 for(ii=0; ii<=N; ii++)
534 blasfeo_print_tran_svec(qp.nb[ii], qp.d_ub+ii, 0);
535 for(ii=0; ii<=N; ii++)
536 blasfeo_print_smat(qp.nu[ii]+qp.nx[ii], qp.ng[ii], qp.DCt+ii, 0, 0);
537 for(ii=0; ii<=N; ii++)
538 blasfeo_print_tran_svec(qp.ng[ii], qp.d_lg+ii, 0);
539 for(ii=0; ii<=N; ii++)
540 blasfeo_print_tran_svec(qp.ng[ii], qp.d_ug+ii, 0);
541 return;
542 #endif
543
544 /************************************************
545 * ocp qp
546 ************************************************/
547
548 int qp_sol_size = s_memsize_ocp_qp_sol(N, nx, nu, nb, ng);
549 printf("\nqp sol size = %d\n", qp_sol_size);
550 void *qp_sol_mem = malloc(qp_sol_size);
551
552 struct s_ocp_qp_sol qp_sol;
553 s_create_ocp_qp_sol(N, nx, nu, nb, ng, &qp_sol, qp_sol_mem);
554
555 /************************************************
556 * ipm
557 ************************************************/
558
559 struct s_ipm_hard_ocp_qp_arg arg;
560 arg.alpha_min = 1e-8;
561 arg.mu_max = 1e-12;
562 arg.iter_max = 20;
563 arg.mu0 = 2.0;
564
565 int ipm_size = s_memsize_ipm_hard_ocp_qp(&qp, &arg);
566 printf("\nipm size = %d\n", ipm_size);
567 void *ipm_mem = malloc(ipm_size);
568
569 struct s_ipm_hard_ocp_qp_workspace workspace;
570 s_create_ipm_hard_ocp_qp(&qp, &arg, &workspace, ipm_mem);
571
572 gettimeofday(&tv0, NULL); // start
573
574 for(rep=0; rep<nrep; rep++)
575 {
576 // s_solve_ipm_hard_ocp_qp(&qp, &qp_sol, &workspace);
577 s_solve_ipm2_hard_ocp_qp(&qp, &qp_sol, &workspace);
578 }
579
580 gettimeofday(&tv1, NULL); // stop
581
582 float time_ocp_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
583
584 #if 1
585 printf("\nsolution\n\n");
586 printf("\nux\n");
587 for(ii=0; ii<=N; ii++)
588 blasfeo_print_tran_svec(nu[ii]+nx[ii], qp_sol.ux+ii, 0);
589 printf("\npi\n");
590 for(ii=0; ii<N; ii++)
591 blasfeo_print_tran_svec(nx[ii+1], qp_sol.pi+ii, 0);
592 printf("\nlam_lb\n");
593 for(ii=0; ii<=N; ii++)
594 blasfeo_print_tran_svec(nb[ii], qp_sol.lam_lb+ii, 0);
595 printf("\nlam_ub\n");
596 for(ii=0; ii<=N; ii++)
597 blasfeo_print_tran_svec(nb[ii], qp_sol.lam_ub+ii, 0);
598 printf("\nlam_lg\n");
599 for(ii=0; ii<=N; ii++)
600 blasfeo_print_tran_svec(ng[ii], qp_sol.lam_lg+ii, 0);
601 printf("\nlam_ug\n");
602 for(ii=0; ii<=N; ii++)
603 blasfeo_print_tran_svec(ng[ii], qp_sol.lam_ug+ii, 0);
604 printf("\nt_lb\n");
605 for(ii=0; ii<=N; ii++)
606 blasfeo_print_tran_svec(nb[ii], qp_sol.t_lb+ii, 0);
607 printf("\nt_ub\n");
608 for(ii=0; ii<=N; ii++)
609 blasfeo_print_tran_svec(nb[ii], qp_sol.t_ub+ii, 0);
610 printf("\nt_lg\n");
611 for(ii=0; ii<=N; ii++)
612 blasfeo_print_tran_svec(ng[ii], qp_sol.t_lg+ii, 0);
613 printf("\nt_ug\n");
614 for(ii=0; ii<=N; ii++)
615 blasfeo_print_tran_svec(ng[ii], qp_sol.t_ug+ii, 0);
616
617 printf("\nresiduals\n\n");
618 printf("\nres_g\n");
619 for(ii=0; ii<=N; ii++)
620 blasfeo_print_exp_tran_svec(nu[ii]+nx[ii], workspace.res_g+ii, 0);
621 printf("\nres_b\n");
622 for(ii=0; ii<N; ii++)
623 blasfeo_print_exp_tran_svec(nx[ii+1], workspace.res_b+ii, 0);
624 printf("\nres_m_lb\n");
625 for(ii=0; ii<=N; ii++)
626 blasfeo_print_exp_tran_svec(nb[ii], workspace.res_m_lb+ii, 0);
627 printf("\nres_m_ub\n");
628 for(ii=0; ii<=N; ii++)
629 blasfeo_print_exp_tran_svec(nb[ii], workspace.res_m_ub+ii, 0);
630 printf("\nres_m_lg\n");
631 for(ii=0; ii<=N; ii++)
632 blasfeo_print_exp_tran_svec(ng[ii], workspace.res_m_lg+ii, 0);
633 printf("\nres_m_ug\n");
634 for(ii=0; ii<=N; ii++)
635 blasfeo_print_exp_tran_svec(ng[ii], workspace.res_m_ug+ii, 0);
636 printf("\nres_d_lb\n");
637 for(ii=0; ii<=N; ii++)
638 blasfeo_print_exp_tran_svec(nb[ii], workspace.res_d_lb+ii, 0);
639 printf("\nres_d_ub\n");
640 for(ii=0; ii<=N; ii++)
641 blasfeo_print_exp_tran_svec(nb[ii], workspace.res_d_ub+ii, 0);
642 printf("\nres_d_lg\n");
643 for(ii=0; ii<=N; ii++)
644 blasfeo_print_exp_tran_svec(ng[ii], workspace.res_d_lg+ii, 0);
645 printf("\nres_d_ug\n");
646 for(ii=0; ii<=N; ii++)
647 blasfeo_print_exp_tran_svec(ng[ii], workspace.res_d_ug+ii, 0);
648 printf("\nres_mu\n");
649 printf("\n%e\n\n", workspace.res_mu);
650 #endif
651
652 printf("\nipm iter = %d\n", workspace.iter);
653 printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
654 s_print_exp_tran_mat(5, workspace.iter, workspace.stat, 5);
655
656 printf("\nocp ipm time = %e [s]\n\n", time_ocp_ipm);
657
658 /************************************************
659 * free memory
660 ************************************************/
661
662 s_free(A);
663 s_free(B);
664 s_free(b);
665 s_free(x0);
666 s_free(Q);
667 s_free(R);
668 s_free(S);
669 s_free(q);
670 s_free(r);
671 s_free(r0);
672 int_free(idxb0);
673 s_free(d_lb0);
674 s_free(d_ub0);
675 int_free(idxb1);
676 s_free(d_lb1);
677 s_free(d_ub1);
678 int_free(idxbN);
679 s_free(d_lbN);
680 s_free(d_ubN);
681 s_free(C0);
682 s_free(D0);
683 s_free(d_lg0);
684 s_free(d_ug0);
685 s_free(C1);
686 s_free(D1);
687 s_free(d_lg1);
688 s_free(d_ug1);
689 s_free(CN);
690 s_free(DN);
691 s_free(d_lgN);
692 s_free(d_ugN);
693
694 free(qp_mem);
695 free(qp_sol_mem);
696 free(ipm_mem);
697
698 /************************************************
699 * return
700 ************************************************/
701
702 return 0;
703
704 }
705