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_d_aux_ext_dep.h>
45 #include <blasfeo_i_aux_ext_dep.h>
46 #include <blasfeo_d_aux.h>
47
48 #include "../include/hpipm_d_ocp_qp_dim.h"
49 #include "../include/hpipm_d_ocp_qp.h"
50 #include "../include/hpipm_d_ocp_qp_sol.h"
51 #include "../include/hpipm_d_ocp_qp_ipm.h"
52
53 #include "d_tools.h"
54
55
56
57 #define KEEP_X0 0
58
59 // printing
60 #define PRINT 1
61
62
63
64 #if ! defined(EXT_DEP)
65 /* creates a zero matrix */
d_zeros(double ** pA,int row,int col)66 void d_zeros(double **pA, int row, int col)
67 {
68 *pA = malloc((row*col)*sizeof(double));
69 double *A = *pA;
70 int i;
71 for(i=0; i<row*col; i++) A[i] = 0.0;
72 }
73 /* frees matrix */
d_free(double * pA)74 void d_free(double *pA)
75 {
76 free( pA );
77 }
78 /* prints a matrix in column-major format */
d_print_mat(int m,int n,double * A,int lda)79 void d_print_mat(int m, int n, double *A, int lda)
80 {
81 int i, j;
82 for(i=0; i<m; i++)
83 {
84 for(j=0; j<n; j++)
85 {
86 printf("%9.5f ", A[i+lda*j]);
87 }
88 printf("\n");
89 }
90 printf("\n");
91 }
92 /* prints the transposed of a matrix in column-major format */
d_print_tran_mat(int row,int col,double * A,int lda)93 void d_print_tran_mat(int row, int col, double *A, int lda)
94 {
95 int i, j;
96 for(j=0; j<col; j++)
97 {
98 for(i=0; i<row; i++)
99 {
100 printf("%9.5f ", A[i+lda*j]);
101 }
102 printf("\n");
103 }
104 printf("\n");
105 }
106 /* prints a matrix in column-major format (exponential notation) */
d_print_exp_mat(int m,int n,double * A,int lda)107 void d_print_exp_mat(int m, int n, double *A, int lda)
108 {
109 int i, j;
110 for(i=0; i<m; i++)
111 {
112 for(j=0; j<n; j++)
113 {
114 printf("%e\t", A[i+lda*j]);
115 }
116 printf("\n");
117 }
118 printf("\n");
119 }
120 /* prints the transposed of a matrix in column-major format (exponential notation) */
d_print_exp_tran_mat(int row,int col,double * A,int lda)121 void d_print_exp_tran_mat(int row, int col, double *A, int lda)
122 {
123 int i, j;
124 for(j=0; j<col; j++)
125 {
126 for(i=0; i<row; i++)
127 {
128 printf("%e\t", A[i+lda*j]);
129 }
130 printf("\n");
131 }
132 printf("\n");
133 }
134 /* creates a zero matrix aligned */
int_zeros(int ** pA,int row,int col)135 void int_zeros(int **pA, int row, int col)
136 {
137 void *temp = malloc((row*col)*sizeof(int));
138 *pA = temp;
139 int *A = *pA;
140 int i;
141 for(i=0; i<row*col; i++) A[i] = 0;
142 }
143 /* frees matrix */
int_free(int * pA)144 void int_free(int *pA)
145 {
146 free( pA );
147 }
148 /* prints a matrix in column-major format */
int_print_mat(int row,int col,int * A,int lda)149 void int_print_mat(int row, int col, int *A, int lda)
150 {
151 int i, j;
152 for(i=0; i<row; i++)
153 {
154 for(j=0; j<col; j++)
155 {
156 printf("%d ", A[i+lda*j]);
157 }
158 printf("\n");
159 }
160 printf("\n");
161 }
162 #endif
163
164
165
166 /************************************************
167 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.
168 ************************************************/
mass_spring_system(double Ts,int nx,int nu,double * A,double * B,double * b,double * x0)169 void mass_spring_system(double Ts, int nx, int nu, double *A, double *B, double *b, double *x0)
170 {
171
172 int nx2 = nx*nx;
173
174 int info = 0;
175
176 int pp = nx/2; // number of masses
177
178 /************************************************
179 * build the continuous time system
180 ************************************************/
181
182 double *T; d_zeros(&T, pp, pp);
183 int ii;
184 for(ii=0; ii<pp; ii++) T[ii*(pp+1)] = -2;
185 for(ii=0; ii<pp-1; ii++) T[ii*(pp+1)+1] = 1;
186 for(ii=1; ii<pp; ii++) T[ii*(pp+1)-1] = 1;
187
188 double *Z; d_zeros(&Z, pp, pp);
189 double *I; d_zeros(&I, pp, pp); for(ii=0; ii<pp; ii++) I[ii*(pp+1)]=1.0; // = eye(pp);
190 double *Ac; d_zeros(&Ac, nx, nx);
191 dmcopy(pp, pp, Z, pp, Ac, nx);
192 dmcopy(pp, pp, T, pp, Ac+pp, nx);
193 dmcopy(pp, pp, I, pp, Ac+pp*nx, nx);
194 dmcopy(pp, pp, Z, pp, Ac+pp*(nx+1), nx);
195 free(T);
196 free(Z);
197 free(I);
198
199 d_zeros(&I, nu, nu); for(ii=0; ii<nu; ii++) I[ii*(nu+1)]=1.0; //I = eye(nu);
200 double *Bc; d_zeros(&Bc, nx, nu);
201 dmcopy(nu, nu, I, nu, Bc+pp, nx);
202 free(I);
203
204 /************************************************
205 * compute the discrete time system
206 ************************************************/
207
208 double *bb; d_zeros(&bb, nx, 1);
209 dmcopy(nx, 1, bb, nx, b, nx);
210
211 dmcopy(nx, nx, Ac, nx, A, nx);
212 dscal_3l(nx2, Ts, A);
213 expm(nx, A);
214
215 d_zeros(&T, nx, nx);
216 d_zeros(&I, nx, nx); for(ii=0; ii<nx; ii++) I[ii*(nx+1)]=1.0; //I = eye(nx);
217 dmcopy(nx, nx, A, nx, T, nx);
218 daxpy_3l(nx2, -1.0, I, T);
219 dgemm_nn_3l(nx, nu, nx, T, nx, Bc, nx, B, nx);
220 free(T);
221 free(I);
222
223 int *ipiv = (int *) malloc(nx*sizeof(int));
224 dgesv_3l(nx, nu, Ac, nx, ipiv, B, nx, &info);
225 free(ipiv);
226
227 free(Ac);
228 free(Bc);
229 free(bb);
230
231
232 /************************************************
233 * initial state
234 ************************************************/
235
236 if(nx==4)
237 {
238 x0[0] = 5;
239 x0[1] = 10;
240 x0[2] = 15;
241 x0[3] = 20;
242 }
243 else
244 {
245 int jj;
246 for(jj=0; jj<nx; jj++)
247 x0[jj] = 1;
248 }
249
250 }
251
252
253
main()254 int main()
255 {
256
257
258 // local variables
259
260 int ii, jj;
261
262
263
264 // problem size
265
266 int nx_ = 8; // number of states (it has to be even for the mass-spring system test problem)
267 int nu_ = 3; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem)
268 int N = 8; // horizon lenght
269
270
271
272 // stage-wise variant size
273
274 int nx[N+1];
275 #if KEEP_X0
276 nx[0] = nx_;
277 #else
278 nx[0] = 0;
279 #endif
280 for(ii=1; ii<=N; ii++)
281 nx[ii] = nx_;
282 // nx[N] = 0;
283
284 int nu[N+1];
285 for(ii=0; ii<N; ii++)
286 nu[ii] = nu_;
287 nu[N] = 0;
288
289 int nbu[N+1];
290 for (ii=0; ii<N; ii++)
291 nbu[ii] = nu[ii];
292 nbu[N] = 0;
293 #if 1
294 int nbx[N+1];
295 #if KEEP_X0
296 nbx[0] = nx[0]/2;
297 #else
298 nbx[0] = 0;
299 #endif
300 for(ii=1; ii<=N; ii++)
301 nbx[ii] = nx[ii]/2;
302
303 int nb[N+1];
304 for (ii=0; ii<=N; ii++)
305 nb[ii] = nbu[ii]+nbx[ii];
306
307 int ng[N+1];
308 ng[0] = 0;
309 for(ii=1; ii<N; ii++)
310 ng[ii] = 0;
311 ng[N] = 0;
312
313 int nsbx[N+1];
314 nsbx[0] = 0;
315 for(ii=1; ii<N; ii++)
316 nsbx[ii] = nx[ii]/2;
317 nsbx[N] = nx[N]/2;
318
319 int nsbu[N+1];
320 for(ii=0; ii<=N; ii++)
321 nsbu[ii] = 0;
322
323 int nsg[N+1];
324 for(ii=0; ii<=N; ii++)
325 nsg[ii] = 0;
326
327 int ns[N+1];
328 for(ii=0; ii<=N; ii++)
329 ns[ii] = nsbx[ii] + nsbu[ii] + nsg[ii];
330
331 #elif 0
332 int nb[N+1];
333 nb[0] = 0;
334 for(ii=1; ii<N; ii++)
335 nb[ii] = 0;
336 nb[N] = 0;
337
338 int ng[N+1];
339 #if KEEP_X0
340 ng[0] = nu[0]+nx[0]/2;
341 #else
342 ng[0] = nu[0];
343 #endif
344 for(ii=1; ii<N; ii++)
345 ng[ii] = nu[1]+nx[1]/2;
346 ng[N] = nx[N]/2;
347 #else
348 int nb[N+1];
349 nb[0] = nu[0] + nx[0]/2;
350 for(ii=1; ii<N; ii++)
351 nb[ii] = nu[ii] + nx[ii]/2;
352 nb[N] = nu[N] + nx[N]/2;
353
354 int ng[N+1];
355 #if KEEP_X0
356 ng[0] = nx[0]/2;
357 #else
358 ng[0] = 0;
359 #endif
360 for(ii=1; ii<N; ii++)
361 ng[ii] = nx[1]/2;
362 ng[N] = nx[N]/2;
363 #endif
364
365 /************************************************
366 * dynamical system
367 ************************************************/
368
369 double *A; d_zeros(&A, nx_, nx_); // states update matrix
370
371 double *B; d_zeros(&B, nx_, nu_); // inputs matrix
372
373 double *b; d_zeros(&b, nx_, 1); // states offset
374 double *x0; d_zeros(&x0, nx_, 1); // initial state
375
376 double Ts = 0.5; // sampling time
377 mass_spring_system(Ts, nx_, nu_, A, B, b, x0);
378
379 for(jj=0; jj<nx_; jj++)
380 b[jj] = 0.0;
381
382 for(jj=0; jj<nx_; jj++)
383 x0[jj] = 0;
384 x0[0] = 2.5;
385 x0[1] = 2.5;
386
387 double *b0; d_zeros(&b0, nx_, 1);
388 dgemv_n_3l(nx_, nx_, A, nx_, x0, b0);
389 daxpy_3l(nx_, 1.0, b, b0);
390
391 #if PRINT
392 d_print_mat(nx_, nx_, A, nx_);
393 d_print_mat(nx_, nu_, B, nu_);
394 d_print_mat(1, nx_, b, 1);
395 d_print_mat(1, nx_, x0, 1);
396 d_print_mat(1, nx_, b0, 1);
397 #endif
398
399 /************************************************
400 * cost function
401 ************************************************/
402
403 double *Q; d_zeros(&Q, nx_, nx_);
404 for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 0.0;
405
406 double *R; d_zeros(&R, nu_, nu_);
407 for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 1e-12;//0.0;
408
409 double *S; d_zeros(&S, nu_, nx_);
410
411 double *q; d_zeros(&q, nx_, 1);
412 for(ii=0; ii<nx_; ii++) q[ii] = 0.0;
413
414 double *r; d_zeros(&r, nu_, 1);
415 for(ii=0; ii<nu_; ii++) r[ii] = 0.0;
416
417 double *r0; d_zeros(&r0, nu_, 1);
418 dgemv_n_3l(nu_, nx_, S, nu_, x0, r0);
419 daxpy_3l(nu_, 1.0, r, r0);
420
421 #if 0
422 double *QN; d_zeros(&QN, nx_, nx_);
423 for(ii=0; ii<2; ii++) QN[ii*(nx_+1)] = 1e15;
424 for(ii=0; ii<nx_; ii++) QN[ii*(nx_+1)] += Q[ii*(nx_+1)];
425 double *qN; d_zeros(&qN, nx_, 1);
426 qN[0] = - 0.1;
427 qN[1] = - 0.1;
428 for(ii=0; ii<2; ii++) qN[ii] *= 1e15;
429 for(ii=0; ii<nx_; ii++) qN[ii] += q[ii];
430 #endif
431
432 #if PRINT
433 d_print_mat(nx_, nx_, Q, nx_);
434 d_print_mat(nu_, nu_, R, nu_);
435 d_print_mat(nu_, nx_, S, nu_);
436 d_print_mat(1, nx_, q, 1);
437 d_print_mat(1, nu_, r, 1);
438 d_print_mat(1, nu_, r0, 1);
439 // d_print_mat(nx_, nx_, QN, nx_);
440 // d_print_mat(1, nx_, qN, 1);
441 #endif
442
443 // maximum element in cost functions
444 double mu0;
445 if(ns[1]>0 | ns[N]>0)
446 mu0 = 1000.0;
447 else
448 mu0 = 2.0;
449
450 /************************************************
451 * box & general constraints
452 ************************************************/
453
454 int *idxbx0; int_zeros(&idxbx0, nbx[0], 1);
455 double *d_lbx0; d_zeros(&d_lbx0, nbx[0], 1);
456 double *d_ubx0; d_zeros(&d_ubx0, nbx[0], 1);
457 int *idxbu0; int_zeros(&idxbu0, nbu[0], 1);
458 double *d_lbu0; d_zeros(&d_lbu0, nbu[0], 1);
459 double *d_ubu0; d_zeros(&d_ubu0, nbu[0], 1);
460 double *d_lg0; d_zeros(&d_lg0, ng[0], 1);
461 double *d_ug0; d_zeros(&d_ug0, ng[0], 1);
462 for(ii=0; ii<nbu[0]; ii++)
463 {
464 d_lbu0[ii] = - 0.5; // umin
465 d_ubu0[ii] = 0.5; // umax
466 idxbu0[ii] = ii;
467 }
468 for(ii=0; ii<nbx[0]; ii++)
469 {
470 d_lbx0[ii] = - 4.0; // xmin
471 d_ubx0[ii] = 4.0; // xmax
472 idxbx0[ii] = ii;
473 }
474 for(ii=0; ii<ng[0]; ii++)
475 {
476 if(ii<nu[0]-nb[0]) // input
477 {
478 d_lg0[ii] = - 0.5; // umin
479 d_ug0[ii] = 0.5; // umax
480 }
481 else // state
482 {
483 d_lg0[ii] = - 4.0; // xmin
484 d_ug0[ii] = 4.0; // xmax
485 }
486 }
487
488 int *idxbx1; int_zeros(&idxbx1, nbx[1], 1);
489 double *d_lbx1; d_zeros(&d_lbx1, nbx[1], 1);
490 double *d_ubx1; d_zeros(&d_ubx1, nbx[1], 1);
491 int *idxbu1; int_zeros(&idxbu1, nbu[1], 1);
492 double *d_lbu1; d_zeros(&d_lbu1, nbu[1], 1);
493 double *d_ubu1; d_zeros(&d_ubu1, nbu[1], 1);
494 double *d_lg1; d_zeros(&d_lg1, ng[1], 1);
495 double *d_ug1; d_zeros(&d_ug1, ng[1], 1);
496 for(ii=0; ii<nbu[1]; ii++)
497 {
498 d_lbu1[ii] = - 0.5; // umin
499 d_ubu1[ii] = 0.5; // umax
500 idxbu1[ii] = ii;
501 }
502 for(ii=0; ii<nbx[1]; ii++)
503 {
504 d_lbx1[ii] = - 1.0; // xmin
505 d_ubx1[ii] = 1.0; // xmax
506 idxbx1[ii] = ii;
507 }
508 for(ii=0; ii<ng[1]; ii++)
509 {
510 if(ii<nu[1]-nb[1]) // input
511 {
512 d_lg1[ii] = - 0.5; // umin
513 d_ug1[ii] = 0.5; // umax
514 }
515 else // state
516 {
517 d_lg1[ii] = - 4.0; // xmin
518 d_ug1[ii] = 4.0; // xmax
519 }
520 }
521
522
523 int *idxbxN; int_zeros(&idxbxN, nbx[N], 1);
524 double *d_lbxN; d_zeros(&d_lbxN, nbx[N], 1);
525 double *d_ubxN; d_zeros(&d_ubxN, nbx[N], 1);
526 double *d_lgN; d_zeros(&d_lgN, ng[N], 1);
527 double *d_ugN; d_zeros(&d_ugN, ng[N], 1);
528 for(ii=0; ii<nbx[N]; ii++)
529 {
530 d_lbxN[ii] = - 1.0; // xmin
531 d_ubxN[ii] = 1.0; // xmax
532 idxbxN[ii] = ii;
533 }
534 for(ii=0; ii<ng[N]; ii++)
535 {
536 d_lgN[ii] = - 4.0; // dmin
537 d_ugN[ii] = 4.0; // dmax
538 }
539
540 double *C0; d_zeros(&C0, ng[0], nx[0]);
541 double *D0; d_zeros(&D0, ng[0], nu[0]);
542 for(ii=0; ii<nu[0]-nb[0] & ii<ng[0]; ii++)
543 D0[ii+(nb[0]+ii)*ng[0]] = 1.0;
544 for(; ii<ng[0]; ii++)
545 C0[ii+(nb[0]+ii-nu[0])*ng[0]] = 1.0;
546
547 double *C1; d_zeros(&C1, ng[1], nx[1]);
548 double *D1; d_zeros(&D1, ng[1], nu[1]);
549 for(ii=0; ii<nu[1]-nb[1] & ii<ng[1]; ii++)
550 D1[ii+(nb[1]+ii)*ng[1]] = 1.0;
551 for(; ii<ng[1]; ii++)
552 C1[ii+(nb[1]+ii-nu[1])*ng[1]] = 1.0;
553
554 double *CN; d_zeros(&CN, ng[N], nx[N]);
555 double *DN; d_zeros(&DN, ng[N], nu[N]);
556 for(ii=0; ii<nu[N]-nb[N] & ii<ng[N]; ii++)
557 DN[ii+(nb[N]+ii)*ng[N]] = 1.0;
558 for(; ii<ng[N]; ii++)
559 CN[ii+(nb[N]+ii-nu[N])*ng[N]] = 1.0;
560
561 #if PRINT
562 // box constraints
563 int_print_mat(1, nbx[0], idxbx0, 1);
564 d_print_mat(1, nbx[0], d_lbx0, 1);
565 d_print_mat(1, nbx[0], d_ubx0, 1);
566 int_print_mat(1, nbu[0], idxbu0, 1);
567 d_print_mat(1, nbu[0], d_lbu0, 1);
568 d_print_mat(1, nbu[0], d_ubu0, 1);
569 int_print_mat(1, nbx[1], idxbx1, 1);
570 d_print_mat(1, nbx[1], d_lbx1, 1);
571 d_print_mat(1, nbx[1], d_ubx1, 1);
572 int_print_mat(1, nbu[1], idxbu1, 1);
573 d_print_mat(1, nbu[1], d_lbu1, 1);
574 d_print_mat(1, nbu[1], d_ubu1, 1);
575 int_print_mat(1, nbx[N], idxbxN, 1);
576 d_print_mat(1, nbx[N], d_lbxN, 1);
577 d_print_mat(1, nbx[N], d_ubxN, 1);
578 // general constraints
579 d_print_mat(1, ng[0], d_lg0, 1);
580 d_print_mat(1, ng[0], d_ug0, 1);
581 d_print_mat(ng[0], nu[0], D0, ng[0]);
582 d_print_mat(ng[0], nx[0], C0, ng[0]);
583 d_print_mat(1, ng[1], d_lg1, 1);
584 d_print_mat(1, ng[1], d_ug1, 1);
585 d_print_mat(ng[1], nu[1], D1, ng[1]);
586 d_print_mat(ng[1], nx[1], C1, ng[1]);
587 d_print_mat(1, ng[N], d_lgN, 1);
588 d_print_mat(1, ng[N], d_ugN, 1);
589 d_print_mat(ng[N], nu[N], DN, ng[N]);
590 d_print_mat(ng[N], nx[N], CN, ng[N]);
591 #endif
592
593 /************************************************
594 * soft constraints
595 ************************************************/
596
597 double *Zl0; d_zeros(&Zl0, ns[0], 1);
598 for(ii=0; ii<ns[0]; ii++)
599 Zl0[ii] = 0e3;
600 double *Zu0; d_zeros(&Zu0, ns[0], 1);
601 for(ii=0; ii<ns[0]; ii++)
602 Zu0[ii] = 0e3;
603 double *zl0; d_zeros(&zl0, ns[0], 1);
604 for(ii=0; ii<ns[0]; ii++)
605 zl0[ii] = 1e0;
606 double *zu0; d_zeros(&zu0, ns[0], 1);
607 for(ii=0; ii<ns[0]; ii++)
608 zu0[ii] = 1e0;
609 int *idxs0; int_zeros(&idxs0, ns[0], 1);
610 for(ii=0; ii<ns[0]; ii++)
611 idxs0[ii] = nu[0]+ii;
612 double *d_ls0; d_zeros(&d_ls0, ns[0], 1);
613 for(ii=0; ii<ns[0]; ii++)
614 d_ls0[ii] = -0.0;
615 double *d_us0; d_zeros(&d_us0, ns[0], 1);
616 for(ii=0; ii<ns[0]; ii++)
617 d_us0[ii] = 0.0;
618
619 double *Zl1; d_zeros(&Zl1, ns[1], 1);
620 for(ii=0; ii<ns[1]; ii++)
621 Zl1[ii] = 0e3;
622 double *Zu1; d_zeros(&Zu1, ns[1], 1);
623 for(ii=0; ii<ns[1]; ii++)
624 Zu1[ii] = 0e3;
625 double *zl1; d_zeros(&zl1, ns[1], 1);
626 for(ii=0; ii<ns[1]; ii++)
627 zl1[ii] = 1e0;
628 double *zu1; d_zeros(&zu1, ns[1], 1);
629 for(ii=0; ii<ns[1]; ii++)
630 zu1[ii] = 1e0;
631 int *idxs1; int_zeros(&idxs1, ns[1], 1);
632 for(ii=0; ii<ns[1]; ii++)
633 idxs1[ii] = nu[1]+ii;
634 double *d_ls1; d_zeros(&d_ls1, ns[1], 1);
635 for(ii=0; ii<ns[1]; ii++)
636 d_ls1[ii] = -0.0;
637 double *d_us1; d_zeros(&d_us1, ns[1], 1);
638 for(ii=0; ii<ns[1]; ii++)
639 d_us1[ii] = 0.0;
640
641 double *ZlN; d_zeros(&ZlN, ns[N], 1);
642 for(ii=0; ii<ns[N]; ii++)
643 ZlN[ii] = 0e3;
644 double *ZuN; d_zeros(&ZuN, ns[N], 1);
645 for(ii=0; ii<ns[N]; ii++)
646 ZuN[ii] = 0e3;
647 double *zlN; d_zeros(&zlN, ns[N], 1);
648 for(ii=0; ii<ns[N]; ii++)
649 zlN[ii] = 1e0;
650 double *zuN; d_zeros(&zuN, ns[N], 1);
651 for(ii=0; ii<ns[N]; ii++)
652 zuN[ii] = 1e0;
653 int *idxsN; int_zeros(&idxsN, ns[N], 1);
654 for(ii=0; ii<ns[N]; ii++)
655 idxsN[ii] = nu[N]+ii;
656 double *d_lsN; d_zeros(&d_lsN, ns[N], 1);
657 for(ii=0; ii<ns[N]; ii++)
658 d_lsN[ii] = -0.0;
659 double *d_usN; d_zeros(&d_usN, ns[N], 1);
660 for(ii=0; ii<ns[N]; ii++)
661 d_usN[ii] = 0.0;
662
663 #if 1
664 // soft constraints
665 int_print_mat(1, ns[0], idxs0, 1);
666 d_print_mat(1, ns[0], Zl0, 1);
667 d_print_mat(1, ns[0], Zu0, 1);
668 d_print_mat(1, ns[0], zl0, 1);
669 d_print_mat(1, ns[0], zu0, 1);
670 d_print_mat(1, ns[0], d_ls0, 1);
671 d_print_mat(1, ns[0], d_us0, 1);
672 int_print_mat(1, ns[1], idxs1, 1);
673 d_print_mat(1, ns[1], Zl1, 1);
674 d_print_mat(1, ns[1], Zu1, 1);
675 d_print_mat(1, ns[1], zl1, 1);
676 d_print_mat(1, ns[1], zu1, 1);
677 d_print_mat(1, ns[1], d_ls1, 1);
678 d_print_mat(1, ns[1], d_us1, 1);
679 int_print_mat(1, ns[N], idxsN, 1);
680 d_print_mat(1, ns[N], ZlN, 1);
681 d_print_mat(1, ns[N], ZuN, 1);
682 d_print_mat(1, ns[N], zlN, 1);
683 d_print_mat(1, ns[N], zuN, 1);
684 d_print_mat(1, ns[N], d_lsN, 1);
685 d_print_mat(1, ns[N], d_usN, 1);
686 #endif
687
688 /************************************************
689 * array of matrices
690 ************************************************/
691
692 double *hA[N];
693 double *hB[N];
694 double *hb[N];
695 double *hQ[N+1];
696 double *hS[N+1];
697 double *hR[N+1];
698 double *hq[N+1];
699 double *hr[N+1];
700 int *hidxbx[N+1];
701 double *hd_lbx[N+1];
702 double *hd_ubx[N+1];
703 int *hidxbu[N+1];
704 double *hd_lbu[N+1];
705 double *hd_ubu[N+1];
706 double *hC[N+1];
707 double *hD[N+1];
708 double *hd_lg[N+1];
709 double *hd_ug[N+1];
710 double *hZl[N+1];
711 double *hZu[N+1];
712 double *hzl[N+1];
713 double *hzu[N+1];
714 int *hidxs[N+1]; // XXX
715 double *hd_ls[N+1];
716 double *hd_us[N+1];
717
718 hA[0] = A;
719 hB[0] = B;
720 hb[0] = b0;
721 hQ[0] = Q;
722 hS[0] = S;
723 hR[0] = R;
724 hq[0] = q;
725 hr[0] = r0;
726 hidxbx[0] = idxbx0;
727 hd_lbx[0] = d_lbx0;
728 hd_ubx[0] = d_ubx0;
729 hidxbu[0] = idxbu0;
730 hd_lbu[0] = d_lbu0;
731 hd_ubu[0] = d_ubu0;
732 hC[0] = C0;
733 hD[0] = D0;
734 hd_lg[0] = d_lg0;
735 hd_ug[0] = d_ug0;
736 hZl[0] = Zl0;
737 hZu[0] = Zu0;
738 hzl[0] = zl0;
739 hzu[0] = zu0;
740 hidxs[0] = idxs0;
741 hd_ls[0] = d_ls0;
742 hd_us[0] = d_us0;
743 for(ii=1; ii<N; ii++)
744 {
745 hA[ii] = A;
746 hB[ii] = B;
747 hb[ii] = b;
748 hQ[ii] = Q;
749 hS[ii] = S;
750 hR[ii] = R;
751 hq[ii] = q;
752 hr[ii] = r;
753 hidxbx[ii] = idxbx1;
754 hd_lbx[ii] = d_lbx1;
755 hd_ubx[ii] = d_ubx1;
756 hidxbu[ii] = idxbu1;
757 hd_lbu[ii] = d_lbu1;
758 hd_ubu[ii] = d_ubu1;
759 hd_lg[ii] = d_lg1;
760 hd_ug[ii] = d_ug1;
761 hC[ii] = C1;
762 hD[ii] = D1;
763 hZl[ii] = Zl1;
764 hZu[ii] = Zu1;
765 hzl[ii] = zl1;
766 hzu[ii] = zu1;
767 hidxs[ii] = idxs1;
768 hd_ls[ii] = d_ls1;
769 hd_us[ii] = d_us1;
770 }
771 hQ[N] = Q;
772 hS[N] = S;
773 hR[N] = R;
774 hq[N] = q;
775 hr[N] = r;
776 hidxbx[N] = idxbxN;
777 hd_lbx[N] = d_lbxN;
778 hd_ubx[N] = d_ubxN;
779 hd_lg[N] = d_lgN;
780 hd_ug[N] = d_ugN;
781 hC[N] = CN;
782 hD[N] = DN;
783 hZl[N] = ZlN;
784 hZu[N] = ZuN;
785 hzl[N] = zlN;
786 hzu[N] = zuN;
787 hidxs[N] = idxsN;
788 hd_ls[N] = d_lsN;
789 hd_us[N] = d_usN;
790
791 /************************************************
792 * ocp qp dim
793 ************************************************/
794
795 int dim_size = d_ocp_qp_dim_memsize(N);
796 printf("\ndim size = %d\n", dim_size);
797 void *dim_mem = malloc(dim_size);
798
799 struct d_ocp_qp_dim dim;
800 d_ocp_qp_dim_create(N, &dim, dim_mem);
801 d_ocp_qp_dim_set_all(nx, nu, nbx, nbu, ng, nsbx, nsbu, nsg, &dim);
802
803 /************************************************
804 * ocp qp
805 ************************************************/
806
807 int qp_size = d_ocp_qp_memsize(&dim);
808 printf("\nqp size = %d\n", qp_size);
809 void *qp_mem = malloc(qp_size);
810
811 struct d_ocp_qp qp;
812 d_ocp_qp_create(&dim, &qp, qp_mem);
813 d_ocp_qp_set_all(hA, hB, hb, hQ, hS, hR, hq, hr, hidxbx, hd_lbx, hd_ubx, hidxbu, hd_lbu, hd_ubu, hC, hD, hd_lg, hd_ug, hZl, hZu, hzl, hzu, hidxs, hd_ls, hd_us, &qp);
814
815 #if 0
816 printf("\nN = %d\n", qp.dim->N);
817 for(ii=0; ii<N; ii++)
818 d_print_strmat(qp.dim->nu[ii]+qp.dim->nx[ii]+1, qp.dim->nx[ii+1], qp.BAbt+ii, 0, 0);
819 for(ii=0; ii<N; ii++)
820 blasfeo_print_tran_dvec(qp.nx[ii+1], qp.b+ii, 0);
821 for(ii=0; ii<=N; ii++)
822 d_print_strmat(qp.nu[ii]+qp.nx[ii]+1, qp.nu[ii]+qp.nx[ii], qp.RSQrq+ii, 0, 0);
823 for(ii=0; ii<=N; ii++)
824 blasfeo_print_tran_dvec(qp.nu[ii]+qp.nx[ii], qp.rq+ii, 0);
825 for(ii=0; ii<=N; ii++)
826 int_print_mat(1, nb[ii], qp.idxb[ii], 1);
827 for(ii=0; ii<=N; ii++)
828 blasfeo_print_tran_dvec(qp.nb[ii], qp.d_lb+ii, 0);
829 for(ii=0; ii<=N; ii++)
830 blasfeo_print_tran_dvec(qp.nb[ii], qp.d_ub+ii, 0);
831 for(ii=0; ii<=N; ii++)
832 d_print_strmat(qp.nu[ii]+qp.nx[ii], qp.ng[ii], qp.DCt+ii, 0, 0);
833 for(ii=0; ii<=N; ii++)
834 blasfeo_print_tran_dvec(qp.ng[ii], qp.d_lg+ii, 0);
835 for(ii=0; ii<=N; ii++)
836 blasfeo_print_tran_dvec(qp.ng[ii], qp.d_ug+ii, 0);
837 return;
838 #endif
839
840 /************************************************
841 * ocp qp sol
842 ************************************************/
843
844 int qp_sol_size = d_ocp_qp_sol_memsize(&dim);
845 printf("\nqp sol size = %d\n", qp_sol_size);
846 void *qp_sol_mem = malloc(qp_sol_size);
847
848 struct d_ocp_qp_sol qp_sol;
849 d_ocp_qp_sol_create(&dim, &qp_sol, qp_sol_mem);
850
851 /************************************************
852 * ipm arg
853 ************************************************/
854
855 int ipm_arg_size = d_ocp_qp_ipm_arg_memsize(&dim);
856 void *ipm_arg_mem = malloc(ipm_arg_size);
857
858 struct d_ocp_qp_ipm_arg arg;
859 d_ocp_qp_ipm_arg_create(&dim, &arg, ipm_arg_mem);
860
861 // enum hpipm_mode mode = SPEED_ABS;
862 enum hpipm_mode mode = SPEED;
863 // enum hpipm_mode mode = BALANCE;
864 // enum hpipm_mode mode = ROBUST;
865 d_ocp_qp_ipm_arg_set_default(mode, &arg);
866
867 int iter_max = 20;
868 double alpha_min = 1e-8;
869 double tol_stat = 1e-8;
870 double tol_eq = 1e-12;
871 double tol_ineq = 1e-12;
872 double tol_comp = 1e-12;
873 double reg_prim = 1e-12;
874 int warm_start = 0;
875 int pred_corr = 1;
876 int ric_alg = 1;
877
878 d_ocp_qp_ipm_arg_set_mu0(&mu0, &arg);
879 d_ocp_qp_ipm_arg_set_iter_max(&iter_max, &arg);
880 d_ocp_qp_ipm_arg_set_alpha_min(&alpha_min, &arg);
881 d_ocp_qp_ipm_arg_set_tol_stat(&tol_stat, &arg);
882 d_ocp_qp_ipm_arg_set_tol_eq(&tol_eq, &arg);
883 d_ocp_qp_ipm_arg_set_tol_ineq(&tol_ineq, &arg);
884 d_ocp_qp_ipm_arg_set_tol_comp(&tol_comp, &arg);
885 d_ocp_qp_ipm_arg_set_reg_prim(®_prim, &arg);
886 d_ocp_qp_ipm_arg_set_warm_start(&warm_start, &arg);
887 d_ocp_qp_ipm_arg_set_pred_corr(&pred_corr, &arg);
888 d_ocp_qp_ipm_arg_set_ric_alg(&ric_alg, &arg);
889
890 /************************************************
891 * ipm
892 ************************************************/
893
894 int ipm_size = d_ocp_qp_ipm_ws_memsize(&dim, &arg);
895 printf("\nipm size = %d\n", ipm_size);
896 void *ipm_mem = malloc(ipm_size);
897
898 struct d_ocp_qp_ipm_ws workspace;
899 d_ocp_qp_ipm_ws_create(&dim, &arg, &workspace, ipm_mem);
900
901 int hpipm_status; // 0 normal; 1 max iter
902
903 int rep, nrep=1;
904
905 struct timeval tv0, tv1;
906
907 /************************************************
908 * solve phase I
909 ************************************************/
910
911 gettimeofday(&tv0, NULL); // start
912
913 for(rep=0; rep<nrep; rep++)
914 {
915 d_ocp_qp_ipm_solve(&qp, &qp_sol, &arg, &workspace);
916 d_ocp_qp_ipm_get_status(&workspace, &hpipm_status);
917 }
918
919 gettimeofday(&tv1, NULL); // stop
920
921 double time_ocp_ipm_phase_I = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
922
923 /************************************************
924 * extract and print solution
925 ************************************************/
926
927 double *u[N+1]; for(ii=0; ii<=N; ii++) d_zeros(u+ii, nu[ii], 1);
928 double *x[N+1]; for(ii=0; ii<=N; ii++) d_zeros(x+ii, nx[ii], 1);
929 double *ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(ls+ii, ns[ii], 1);
930 double *us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(us+ii, ns[ii], 1);
931 double *pi[N]; for(ii=0; ii<N; ii++) d_zeros(pi+ii, nx[ii+1], 1);
932 double *lam_lb[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lb+ii, nb[ii], 1);
933 double *lam_ub[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ub+ii, nb[ii], 1);
934 double *lam_lg[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lg+ii, ng[ii], 1);
935 double *lam_ug[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ug+ii, ng[ii], 1);
936 double *lam_ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ls+ii, ns[ii], 1);
937 double *lam_us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_us+ii, ns[ii], 1);
938
939 d_ocp_qp_sol_get_all(&qp_sol, u, x, ls, us, pi, lam_lb, lam_ub, lam_lg, lam_ug, lam_ls, lam_us);
940
941 #if 1
942 printf("\nsolution\n\n");
943 printf("\nu\n");
944 for(ii=0; ii<=N; ii++)
945 d_print_mat(1, nu[ii], u[ii], 1);
946 printf("\nx\n");
947 for(ii=0; ii<=N; ii++)
948 d_print_mat(1, nx[ii], x[ii], 1);
949 printf("\nls\n");
950 for(ii=0; ii<=N; ii++)
951 d_print_mat(1, ns[ii], ls[ii], 1);
952 printf("\nus\n");
953 for(ii=0; ii<=N; ii++)
954 d_print_mat(1, ns[ii], us[ii], 1);
955 printf("\npi\n");
956 for(ii=0; ii<N; ii++)
957 d_print_mat(1, nx[ii+1], pi[ii], 1);
958 printf("\nlam_lb\n");
959 for(ii=0; ii<=N; ii++)
960 d_print_mat(1, nb[ii], lam_lb[ii], 1);
961 printf("\nlam_ub\n");
962 for(ii=0; ii<=N; ii++)
963 d_print_mat(1, nb[ii], lam_ub[ii], 1);
964 printf("\nlam_lg\n");
965 for(ii=0; ii<=N; ii++)
966 d_print_mat(1, ng[ii], lam_lg[ii], 1);
967 printf("\nlam_ug\n");
968 for(ii=0; ii<=N; ii++)
969 d_print_mat(1, ng[ii], lam_ug[ii], 1);
970 printf("\nlam_ls\n");
971 for(ii=0; ii<=N; ii++)
972 d_print_mat(1, ns[ii], lam_ls[ii], 1);
973 printf("\nlam_us\n");
974 for(ii=0; ii<=N; ii++)
975 d_print_mat(1, ns[ii], lam_us[ii], 1);
976
977 printf("\nt_lb\n");
978 for(ii=0; ii<=N; ii++)
979 d_print_mat(1, nb[ii], (qp_sol.t+ii)->pa, 1);
980 printf("\nt_ub\n");
981 for(ii=0; ii<=N; ii++)
982 d_print_mat(1, nb[ii], (qp_sol.t+ii)->pa+nb[ii]+ng[ii], 1);
983 printf("\nt_lg\n");
984 for(ii=0; ii<=N; ii++)
985 d_print_mat(1, ng[ii], (qp_sol.t+ii)->pa+nb[ii], 1);
986 printf("\nt_ug\n");
987 for(ii=0; ii<=N; ii++)
988 d_print_mat(1, ng[ii], (qp_sol.t+ii)->pa+2*nb[ii]+ng[ii], 1);
989 printf("\nt_ls\n");
990 for(ii=0; ii<=N; ii++)
991 d_print_mat(1, ns[ii], (qp_sol.t+ii)->pa+2*nb[ii]+2*ng[ii], 1);
992 printf("\nt_us\n");
993 for(ii=0; ii<=N; ii++)
994 d_print_mat(1, ns[ii], (qp_sol.t+ii)->pa+2*nb[ii]+2*ng[ii]+ns[ii], 1);
995 #endif
996
997 /************************************************
998 * print ipm statistics
999 ************************************************/
1000
1001 int iter; d_ocp_qp_ipm_get_iter(&workspace, &iter);
1002 double res_stat; d_ocp_qp_ipm_get_max_res_stat(&workspace, &res_stat);
1003 double res_eq; d_ocp_qp_ipm_get_max_res_eq(&workspace, &res_eq);
1004 double res_ineq; d_ocp_qp_ipm_get_max_res_ineq(&workspace, &res_ineq);
1005 double res_comp; d_ocp_qp_ipm_get_max_res_comp(&workspace, &res_comp);
1006 double *stat; d_ocp_qp_ipm_get_stat(&workspace, &stat);
1007 int stat_m; d_ocp_qp_ipm_get_stat_m(&workspace, &stat_m);
1008
1009 printf("\nipm return = %d\n", hpipm_status);
1010 printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
1011
1012 printf("\nipm iter = %d\n", iter);
1013 printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\n");
1014 d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
1015
1016 printf("\nocp ipm time = %e [s]\n\n", time_ocp_ipm_phase_I);
1017
1018
1019 /************************************************
1020 * solve actual problem
1021 ************************************************/
1022
1023 for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 1.0;
1024
1025 for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 2.0;
1026
1027 d_ocp_qp_set_all(hA, hB, hb, hQ, hS, hR, hq, hr, hidxbx, hd_lbx, hd_ubx, hidxbu, hd_lbu, hd_ubu, hC, hD, hd_lg, hd_ug, hZl, hZu, hzl, hzu, hidxs, hd_ls, hd_us, &qp);
1028
1029 // war start
1030 arg.warm_start = 1;
1031
1032 // call solver
1033 gettimeofday(&tv0, NULL); // start
1034
1035 for(rep=0; rep<nrep; rep++)
1036 {
1037 d_ocp_qp_ipm_solve(&qp, &qp_sol, &arg, &workspace);
1038 d_ocp_qp_ipm_get_status(&workspace, &hpipm_status);
1039 }
1040
1041 gettimeofday(&tv1, NULL); // stop
1042
1043 double time_ocp_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
1044
1045 /************************************************
1046 * extract and print solution
1047 ************************************************/
1048
1049 #if 0
1050 double *u[N+1]; for(ii=0; ii<=N; ii++) d_zeros(u+ii, nu[ii], 1);
1051 double *x[N+1]; for(ii=0; ii<=N; ii++) d_zeros(x+ii, nx[ii], 1);
1052 double *ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(ls+ii, ns[ii], 1);
1053 double *us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(us+ii, ns[ii], 1);
1054 double *pi[N]; for(ii=0; ii<N; ii++) d_zeros(pi+ii, nx[ii+1], 1);
1055 double *lam_lb[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lb+ii, nb[ii], 1);
1056 double *lam_ub[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ub+ii, nb[ii], 1);
1057 double *lam_lg[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lg+ii, ng[ii], 1);
1058 double *lam_ug[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ug+ii, ng[ii], 1);
1059 double *lam_ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ls+ii, ns[ii], 1);
1060 double *lam_us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_us+ii, ns[ii], 1);
1061 #endif
1062
1063 d_ocp_qp_sol_get_all(&qp_sol, u, x, ls, us, pi, lam_lb, lam_ub, lam_lg, lam_ug, lam_ls, lam_us);
1064
1065 #if 1
1066 printf("\nsolution\n\n");
1067 printf("\nu\n");
1068 for(ii=0; ii<=N; ii++)
1069 d_print_mat(1, nu[ii], u[ii], 1);
1070 printf("\nx\n");
1071 for(ii=0; ii<=N; ii++)
1072 d_print_mat(1, nx[ii], x[ii], 1);
1073 printf("\nls\n");
1074 for(ii=0; ii<=N; ii++)
1075 d_print_mat(1, ns[ii], ls[ii], 1);
1076 printf("\nus\n");
1077 for(ii=0; ii<=N; ii++)
1078 d_print_mat(1, ns[ii], us[ii], 1);
1079 printf("\npi\n");
1080 for(ii=0; ii<N; ii++)
1081 d_print_mat(1, nx[ii+1], pi[ii], 1);
1082 printf("\nlam_lb\n");
1083 for(ii=0; ii<=N; ii++)
1084 d_print_mat(1, nb[ii], lam_lb[ii], 1);
1085 printf("\nlam_ub\n");
1086 for(ii=0; ii<=N; ii++)
1087 d_print_mat(1, nb[ii], lam_ub[ii], 1);
1088 printf("\nlam_lg\n");
1089 for(ii=0; ii<=N; ii++)
1090 d_print_mat(1, ng[ii], lam_lg[ii], 1);
1091 printf("\nlam_ug\n");
1092 for(ii=0; ii<=N; ii++)
1093 d_print_mat(1, ng[ii], lam_ug[ii], 1);
1094 printf("\nlam_ls\n");
1095 for(ii=0; ii<=N; ii++)
1096 d_print_mat(1, ns[ii], lam_ls[ii], 1);
1097 printf("\nlam_us\n");
1098 for(ii=0; ii<=N; ii++)
1099 d_print_mat(1, ns[ii], lam_us[ii], 1);
1100
1101 printf("\nt_lb\n");
1102 for(ii=0; ii<=N; ii++)
1103 d_print_mat(1, nb[ii], (qp_sol.t+ii)->pa, 1);
1104 printf("\nt_ub\n");
1105 for(ii=0; ii<=N; ii++)
1106 d_print_mat(1, nb[ii], (qp_sol.t+ii)->pa+nb[ii]+ng[ii], 1);
1107 printf("\nt_lg\n");
1108 for(ii=0; ii<=N; ii++)
1109 d_print_mat(1, ng[ii], (qp_sol.t+ii)->pa+nb[ii], 1);
1110 printf("\nt_ug\n");
1111 for(ii=0; ii<=N; ii++)
1112 d_print_mat(1, ng[ii], (qp_sol.t+ii)->pa+2*nb[ii]+ng[ii], 1);
1113 printf("\nt_ls\n");
1114 for(ii=0; ii<=N; ii++)
1115 d_print_mat(1, ns[ii], (qp_sol.t+ii)->pa+2*nb[ii]+2*ng[ii], 1);
1116 printf("\nt_us\n");
1117 for(ii=0; ii<=N; ii++)
1118 d_print_mat(1, ns[ii], (qp_sol.t+ii)->pa+2*nb[ii]+2*ng[ii]+ns[ii], 1);
1119 #endif
1120
1121 /************************************************
1122 * extract and print residuals
1123 ************************************************/
1124
1125 double *res_r[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_r+ii, nu[ii], 1);
1126 double *res_q[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_q+ii, nx[ii], 1);
1127 double *res_ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_ls+ii, ns[ii], 1);
1128 double *res_us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_us+ii, ns[ii], 1);
1129 double *res_b[N]; for(ii=0; ii<N; ii++) d_zeros(res_b+ii, nx[ii+1], 1);
1130 double *res_d_lb[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_d_lb+ii, nb[ii], 1);
1131 double *res_d_ub[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_d_ub+ii, nb[ii], 1);
1132 double *res_d_lg[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_d_lg+ii, ng[ii], 1);
1133 double *res_d_ug[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_d_ug+ii, ng[ii], 1);
1134 double *res_d_ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_d_ls+ii, ns[ii], 1);
1135 double *res_d_us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_d_us+ii, ns[ii], 1);
1136 double *res_m_lb[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_m_lb+ii, nb[ii], 1);
1137 double *res_m_ub[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_m_ub+ii, nb[ii], 1);
1138 double *res_m_lg[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_m_lg+ii, ng[ii], 1);
1139 double *res_m_ug[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_m_ug+ii, ng[ii], 1);
1140 double *res_m_ls[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_m_ls+ii, ns[ii], 1);
1141 double *res_m_us[N+1]; for(ii=0; ii<=N; ii++) d_zeros(res_m_us+ii, ns[ii], 1);
1142
1143 d_ocp_qp_res_get_all(workspace.res, res_r, res_q, res_ls, res_us, res_b, res_d_lb, res_d_ub, res_d_lg, res_d_ug, res_d_ls, res_d_us, res_m_lb, res_m_ub, res_m_lg, res_m_ug, res_m_ls, res_m_us);
1144
1145 #if 1
1146 printf("\nresiduals\n\n");
1147 printf("\nres_r\n");
1148 for(ii=0; ii<=N; ii++)
1149 d_print_exp_mat(1, nu[ii], res_r[ii], 1);
1150 printf("\nres_q\n");
1151 for(ii=0; ii<=N; ii++)
1152 d_print_exp_mat(1, nx[ii], res_q[ii], 1);
1153 printf("\nres_ls\n");
1154 for(ii=0; ii<=N; ii++)
1155 d_print_exp_mat(1, ns[ii], res_ls[ii], 1);
1156 printf("\nres_us\n");
1157 for(ii=0; ii<=N; ii++)
1158 d_print_exp_mat(1, ns[ii], res_us[ii], 1);
1159 printf("\nres_b\n");
1160 for(ii=0; ii<N; ii++)
1161 d_print_exp_mat(1, nx[ii+1], res_b[ii], 1);
1162 printf("\nres_d_lb\n");
1163 for(ii=0; ii<=N; ii++)
1164 d_print_exp_mat(1, nb[ii], res_d_lb[ii], 1);
1165 printf("\nres_d_ub\n");
1166 for(ii=0; ii<=N; ii++)
1167 d_print_exp_mat(1, nb[ii], res_d_ub[ii], 1);
1168 printf("\nres_d_lg\n");
1169 for(ii=0; ii<=N; ii++)
1170 d_print_exp_mat(1, ng[ii], res_d_lg[ii], 1);
1171 printf("\nres_d_ug\n");
1172 for(ii=0; ii<=N; ii++)
1173 d_print_exp_mat(1, ng[ii], res_d_ug[ii], 1);
1174 printf("\nres_d_ls\n");
1175 for(ii=0; ii<=N; ii++)
1176 d_print_exp_mat(1, ns[ii], res_d_ls[ii], 1);
1177 printf("\nres_d_us\n");
1178 for(ii=0; ii<=N; ii++)
1179 d_print_exp_mat(1, ns[ii], res_d_us[ii], 1);
1180 printf("\nres_m_lb\n");
1181 for(ii=0; ii<=N; ii++)
1182 d_print_exp_mat(1, nb[ii], res_m_lb[ii], 1);
1183 printf("\nres_m_ub\n");
1184 for(ii=0; ii<=N; ii++)
1185 d_print_exp_mat(1, nb[ii], res_m_ub[ii], 1);
1186 printf("\nres_m_lg\n");
1187 for(ii=0; ii<=N; ii++)
1188 d_print_exp_mat(1, ng[ii], res_m_lg[ii], 1);
1189 printf("\nres_m_ug\n");
1190 for(ii=0; ii<=N; ii++)
1191 d_print_exp_mat(1, ng[ii], res_m_ug[ii], 1);
1192 printf("\nres_m_ls\n");
1193 for(ii=0; ii<=N; ii++)
1194 d_print_exp_mat(1, ns[ii], res_m_ls[ii], 1);
1195 printf("\nres_m_us\n");
1196 for(ii=0; ii<=N; ii++)
1197 d_print_exp_mat(1, ns[ii], res_m_us[ii], 1);
1198 #endif
1199
1200 /************************************************
1201 * print ipm statistics
1202 ************************************************/
1203
1204 d_ocp_qp_ipm_get_iter(&workspace, &iter);
1205 d_ocp_qp_ipm_get_max_res_stat(&workspace, &res_stat);
1206 d_ocp_qp_ipm_get_max_res_eq(&workspace, &res_eq);
1207 d_ocp_qp_ipm_get_max_res_ineq(&workspace, &res_ineq);
1208 d_ocp_qp_ipm_get_max_res_comp(&workspace, &res_comp);
1209 d_ocp_qp_ipm_get_stat(&workspace, &stat);
1210 d_ocp_qp_ipm_get_stat_m(&workspace, &stat_m);
1211
1212 printf("\nipm return = %d\n", hpipm_status);
1213 printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
1214
1215 printf("\nipm iter = %d\n", iter);
1216 printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\n");
1217 d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
1218
1219 printf("\nocp ipm time = %e [s]\n\n", time_ocp_ipm);
1220
1221
1222 /************************************************
1223 * free memory
1224 ************************************************/
1225
1226 d_free(A);
1227 d_free(B);
1228 d_free(b);
1229 d_free(x0);
1230 d_free(Q);
1231 // d_free(QN);
1232 d_free(R);
1233 d_free(S);
1234 d_free(q);
1235 // d_free(qN);
1236 d_free(r);
1237 d_free(r0);
1238 int_free(idxbx0);
1239 d_free(d_lbx0);
1240 d_free(d_ubx0);
1241 int_free(idxbu0);
1242 d_free(d_lbu0);
1243 d_free(d_ubu0);
1244 int_free(idxbx1);
1245 d_free(d_lbx1);
1246 d_free(d_ubx1);
1247 int_free(idxbu1);
1248 d_free(d_lbu1);
1249 d_free(d_ubu1);
1250 int_free(idxbxN);
1251 d_free(d_lbxN);
1252 d_free(d_ubxN);
1253 d_free(C0);
1254 d_free(D0);
1255 d_free(d_lg0);
1256 d_free(d_ug0);
1257 d_free(C1);
1258 d_free(D1);
1259 d_free(d_lg1);
1260 d_free(d_ug1);
1261 d_free(CN);
1262 d_free(DN);
1263 d_free(d_lgN);
1264 d_free(d_ugN);
1265
1266 d_free(Zl0);
1267 d_free(Zu0);
1268 d_free(zl0);
1269 d_free(zu0);
1270 int_free(idxs0);
1271 d_free(d_ls0);
1272 d_free(d_us0);
1273 d_free(Zl1);
1274 d_free(Zu1);
1275 d_free(zl1);
1276 d_free(zu1);
1277 int_free(idxs1);
1278 d_free(d_ls1);
1279 d_free(d_us1);
1280 d_free(ZlN);
1281 d_free(ZuN);
1282 d_free(zlN);
1283 d_free(zuN);
1284 int_free(idxsN);
1285 d_free(d_lsN);
1286 d_free(d_usN);
1287
1288 for(ii=0; ii<N; ii++)
1289 {
1290 d_free(u[ii]);
1291 d_free(x[ii]);
1292 d_free(ls[ii]);
1293 d_free(us[ii]);
1294 d_free(pi[ii]);
1295 d_free(lam_lb[ii]);
1296 d_free(lam_ub[ii]);
1297 d_free(lam_lg[ii]);
1298 d_free(lam_ug[ii]);
1299 d_free(lam_ls[ii]);
1300 d_free(lam_us[ii]);
1301 d_free(res_r[ii]);
1302 d_free(res_q[ii]);
1303 d_free(res_ls[ii]);
1304 d_free(res_us[ii]);
1305 d_free(res_b[ii]);
1306 d_free(res_d_lb[ii]);
1307 d_free(res_d_ub[ii]);
1308 d_free(res_d_lg[ii]);
1309 d_free(res_d_ug[ii]);
1310 d_free(res_d_ls[ii]);
1311 d_free(res_d_us[ii]);
1312 d_free(res_m_lb[ii]);
1313 d_free(res_m_ub[ii]);
1314 d_free(res_m_lg[ii]);
1315 d_free(res_m_ug[ii]);
1316 d_free(res_m_ls[ii]);
1317 d_free(res_m_us[ii]);
1318 }
1319 d_free(u[ii]);
1320 d_free(x[ii]);
1321 d_free(ls[ii]);
1322 d_free(us[ii]);
1323 d_free(lam_lb[ii]);
1324 d_free(lam_ub[ii]);
1325 d_free(lam_lg[ii]);
1326 d_free(lam_ug[ii]);
1327 d_free(lam_ls[ii]);
1328 d_free(lam_us[ii]);
1329 d_free(res_r[ii]);
1330 d_free(res_q[ii]);
1331 d_free(res_ls[ii]);
1332 d_free(res_us[ii]);
1333 d_free(res_d_lb[ii]);
1334 d_free(res_d_ub[ii]);
1335 d_free(res_d_lg[ii]);
1336 d_free(res_d_ug[ii]);
1337 d_free(res_d_ls[ii]);
1338 d_free(res_d_us[ii]);
1339 d_free(res_m_lb[ii]);
1340 d_free(res_m_ub[ii]);
1341 d_free(res_m_lg[ii]);
1342 d_free(res_m_ug[ii]);
1343 d_free(res_m_ls[ii]);
1344 d_free(res_m_us[ii]);
1345
1346 free(qp_mem);
1347 free(qp_sol_mem);
1348 free(ipm_arg_mem);
1349 free(ipm_mem);
1350
1351 /************************************************
1352 * return
1353 ************************************************/
1354
1355 return 0;
1356
1357 }
1358
1359