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 #include <stdlib.h>
31 #include <stdio.h>
32 #include <sys/time.h>
33
34 #include <blasfeo_d_aux_ext_dep.h>
35
36 #include "../../include/hpipm_d_ocp_qp_ipm.h"
37 #include "../../include/hpipm_d_ocp_qp_dim.h"
38 #include "../../include/hpipm_d_ocp_qp.h"
39 #include "../../include/hpipm_d_ocp_qp_sol.h"
40 #include "../include/hpipm_d_part_cond.h"
41
42
43
44 // qp data as global data
45 extern int N;
46 extern int *nx;
47 extern int *nu;
48 extern int *nbu;
49 extern int *nbx;
50 extern int *ng;
51 extern int *nsbx;
52 extern int *nsbu;
53 extern int *nsg;
54 extern double **hA;
55 extern double **hB;
56 extern double **hb;
57 extern double **hQ;
58 extern double **hR;
59 extern double **hS;
60 extern double **hq;
61 extern double **hr;
62 extern int **hidxbx;
63 extern double **hlbx;
64 extern double **hubx;
65 extern int **hidxbu;
66 extern double **hlbu;
67 extern double **hubu;
68 extern double **hC;
69 extern double **hD;
70 extern double **hlg;
71 extern double **hug;
72 extern double **hZl;
73 extern double **hZu;
74 extern double **hzl;
75 extern double **hzu;
76 extern int **hidxs;
77 extern double **hlls;
78 extern double **hlus;
79 // arg
80 extern int mode;
81 extern int iter_max;
82 extern double alpha_min;
83 extern double mu0;
84 extern double tol_stat;
85 extern double tol_eq;
86 extern double tol_ineq;
87 extern double tol_comp;
88 extern double reg_prim;
89 extern int warm_start;
90 extern int pred_corr;
91 extern int ric_alg;
92
93
94
95 // main
main()96 int main()
97 {
98
99 int ii;
100
101 int hpipm_status;
102
103 int rep, nrep=10;
104
105 struct timeval tv0, tv1;
106
107 /************************************************
108 * ocp qp dim
109 ************************************************/
110
111 int dim_size = d_ocp_qp_dim_memsize(N);
112 void *dim_mem = malloc(dim_size);
113
114 struct d_ocp_qp_dim dim;
115 d_ocp_qp_dim_create(N, &dim, dim_mem);
116
117 d_ocp_qp_dim_set_all(nx, nu, nbx, nbu, ng, nsbx, nsbu, nsg, &dim);
118
119 /************************************************
120 * ocp qp dim part cond
121 ************************************************/
122
123 // horizon length of partially condensed OCP QP
124 int N2 = 2;
125
126 int dim_size2 = d_ocp_qp_dim_memsize(N2);
127 void *dim_mem2 = malloc(dim_size2);
128
129 struct d_ocp_qp_dim dim2;
130 d_ocp_qp_dim_create(N2, &dim2, dim_mem2);
131
132 int *block_size = malloc((N+1)*sizeof(int));
133 d_part_cond_qp_compute_block_size(N, N2, block_size);
134 // block_size[0] = 1;
135 // block_size[1] = 1;
136 // printf("\nblock_size\n");
137 // int_print_mat(1, N2+1, block_size, 1);
138
139 d_part_cond_qp_compute_dim(&dim, block_size, &dim2);
140
141 /************************************************
142 * ocp qp
143 ************************************************/
144
145 int qp_size = d_ocp_qp_memsize(&dim);
146 void *qp_mem = malloc(qp_size);
147
148 struct d_ocp_qp qp;
149 d_ocp_qp_create(&dim, &qp, qp_mem);
150
151 d_ocp_qp_set_all(hA, hB, hb, hQ, hS, hR, hq, hr, hidxbx, hlbx, hubx, hidxbu, hlbu, hubu, hC, hD, hlg, hug, hZl, hZu, hzl, hzu, hidxs, hlls, hlus, &qp);
152
153 /************************************************
154 * ocp qp part cond
155 ************************************************/
156
157 int qp_size2 = d_ocp_qp_memsize(&dim2);
158 void *qp_mem2 = malloc(qp_size2);
159
160 struct d_ocp_qp qp2;
161 d_ocp_qp_create(&dim2, &qp2, qp_mem2);
162
163 /************************************************
164 * ocp qp sol
165 ************************************************/
166
167 int qp_sol_size = d_ocp_qp_sol_memsize(&dim);
168 void *qp_sol_mem = malloc(qp_sol_size);
169
170 struct d_ocp_qp_sol qp_sol;
171 d_ocp_qp_sol_create(&dim, &qp_sol, qp_sol_mem);
172
173 /************************************************
174 * ocp qp sol part cond
175 ************************************************/
176
177 int qp_sol_size2 = d_ocp_qp_sol_memsize(&dim2);
178 void *qp_sol_mem2 = malloc(qp_sol_size2);
179
180 struct d_ocp_qp_sol qp_sol2;
181 d_ocp_qp_sol_create(&dim2, &qp_sol2, qp_sol_mem2);
182
183 /************************************************
184 * part cond arg
185 ************************************************/
186
187 int part_cond_arg_size = d_part_cond_qp_arg_memsize(dim2.N);
188 void *part_cond_arg_mem = malloc(part_cond_arg_size);
189
190 struct d_part_cond_qp_arg part_cond_arg;
191 d_part_cond_qp_arg_create(dim2.N, &part_cond_arg, part_cond_arg_mem);
192
193 d_part_cond_qp_arg_set_default(dim2.N, &part_cond_arg);
194
195 // d_set_cond_qp_ocp2ocp_arg_ric_alg(0, dim2.N, &part_cond_arg);
196
197 /************************************************
198 * ipm arg
199 ************************************************/
200
201 int ipm_arg_size = d_ocp_qp_ipm_arg_memsize(&dim);
202 void *ipm_arg_mem = malloc(ipm_arg_size);
203
204 struct d_ocp_qp_ipm_arg arg;
205 d_ocp_qp_ipm_arg_create(&dim, &arg, ipm_arg_mem);
206
207 d_ocp_qp_ipm_arg_set_default(mode, &arg);
208
209 d_ocp_qp_ipm_arg_set_mu0(&mu0, &arg);
210 d_ocp_qp_ipm_arg_set_iter_max(&iter_max, &arg);
211 d_ocp_qp_ipm_arg_set_tol_stat(&tol_stat, &arg);
212 d_ocp_qp_ipm_arg_set_tol_eq(&tol_eq, &arg);
213 d_ocp_qp_ipm_arg_set_tol_ineq(&tol_ineq, &arg);
214 d_ocp_qp_ipm_arg_set_tol_comp(&tol_comp, &arg);
215 d_ocp_qp_ipm_arg_set_reg_prim(®_prim, &arg);
216 d_ocp_qp_ipm_arg_set_warm_start(&warm_start, &arg);
217 // d_ocp_qp_ipm_arg_set_ric_alg(&ric_alg, &arg);
218
219 /************************************************
220 * part cond workspace
221 ************************************************/
222
223 int part_cond_size = d_part_cond_qp_ws_memsize(&dim, block_size, &dim2, &part_cond_arg);
224 void *part_cond_mem = malloc(part_cond_size);
225
226 struct d_part_cond_qp_ws part_cond_ws;
227 d_part_cond_qp_ws_create(&dim, block_size, &dim2, &part_cond_arg, &part_cond_ws, part_cond_mem);
228
229 /************************************************
230 * ipm workspace
231 ************************************************/
232
233 int ipm_size = d_ocp_qp_ipm_ws_memsize(&dim2, &arg);
234 void *ipm_mem = malloc(ipm_size);
235
236 struct d_ocp_qp_ipm_ws workspace;
237 d_ocp_qp_ipm_ws_create(&dim2, &arg, &workspace, ipm_mem);
238
239 /************************************************
240 * part cond
241 ************************************************/
242
243 gettimeofday(&tv0, NULL); // start
244
245 for(rep=0; rep<nrep; rep++)
246 {
247 d_part_cond_qp_cond(&qp, &qp2, &part_cond_arg, &part_cond_ws);
248 }
249
250 gettimeofday(&tv1, NULL); // stop
251
252 double time_cond = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
253
254 /************************************************
255 * ipm solver
256 ************************************************/
257
258 gettimeofday(&tv0, NULL); // start
259
260 for(rep=0; rep<nrep; rep++)
261 {
262 d_ocp_qp_ipm_solve(&qp2, &qp_sol2, &arg, &workspace);
263 d_ocp_qp_ipm_get_status(&workspace, &hpipm_status);
264 }
265
266 gettimeofday(&tv1, NULL); // stop
267
268 double time_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
269
270 /************************************************
271 * part expand
272 ************************************************/
273
274 gettimeofday(&tv0, NULL); // start
275
276 for(rep=0; rep<nrep; rep++)
277 {
278 d_part_cond_qp_expand_sol(&qp, &qp2, &qp_sol2, &qp_sol, &part_cond_arg, &part_cond_ws);
279 }
280
281 gettimeofday(&tv1, NULL); // stop
282
283 double time_expa = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
284
285 /************************************************
286 * print solution info
287 ************************************************/
288
289 printf("\nHPIPM returned with flag %i.\n", hpipm_status);
290 if(hpipm_status == 0)
291 {
292 printf("\n -> QP solved!\n");
293 }
294 else if(hpipm_status==1)
295 {
296 printf("\n -> Solver failed! Maximum number of iterations reached\n");
297 }
298 else if(hpipm_status==2)
299 {
300 printf("\n -> Solver failed! Minimum step lenght reached\n");
301 }
302 else if(hpipm_status==2)
303 {
304 printf("\n -> Solver failed! NaN in computations\n");
305 }
306 else
307 {
308 printf("\n -> Solver failed! Unknown return flag\n");
309 }
310 printf("\nAverage solution time over %i runs: %e [s]\n", nrep, time_ipm);
311 printf("\n\n");
312
313 /************************************************
314 * extract and print solution
315 ************************************************/
316
317 // u
318
319 int nu_max = nu[0];
320 for(ii=1; ii<=N; ii++)
321 if(nu[ii]>nu_max)
322 nu_max = nu[ii];
323
324 double *u = malloc(nu_max*sizeof(double));
325
326 printf("\nu = \n");
327 for(ii=0; ii<=N; ii++)
328 {
329 d_ocp_qp_sol_get_u(ii, &qp_sol, u);
330 d_print_mat(1, nu[ii], u, 1);
331 }
332
333 // x
334
335 int nx_max = nx[0];
336 for(ii=1; ii<=N; ii++)
337 if(nx[ii]>nx_max)
338 nx_max = nx[ii];
339
340 double *x = malloc(nx_max*sizeof(double));
341
342 printf("\nx = \n");
343 for(ii=0; ii<=N; ii++)
344 {
345 d_ocp_qp_sol_get_x(ii, &qp_sol, x);
346 d_print_mat(1, nx[ii], x, 1);
347 }
348
349 /************************************************
350 * print ipm statistics
351 ************************************************/
352
353 int iter; d_ocp_qp_ipm_get_iter(&workspace, &iter);
354 double res_stat; d_ocp_qp_ipm_get_max_res_stat(&workspace, &res_stat);
355 double res_eq; d_ocp_qp_ipm_get_max_res_eq(&workspace, &res_eq);
356 double res_ineq; d_ocp_qp_ipm_get_max_res_ineq(&workspace, &res_ineq);
357 double res_comp; d_ocp_qp_ipm_get_max_res_comp(&workspace, &res_comp);
358 double *stat; d_ocp_qp_ipm_get_stat(&workspace, &stat);
359 int stat_m; d_ocp_qp_ipm_get_stat_m(&workspace, &stat_m);
360
361 printf("\nipm return = %d\n", hpipm_status);
362 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);
363
364 printf("\nipm iter = %d\n", iter);
365 printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\n");
366 d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
367
368 printf("\npart cond time = %e [s]\n\n", time_cond);
369 printf("\nocp ipm time = %e [s]\n\n", time_ipm);
370 printf("\npart expa time = %e [s]\n\n", time_expa);
371 printf("\ntotal time = %e [s]\n\n", time_cond+time_ipm+time_expa);
372
373 /************************************************
374 * free memory and return
375 ************************************************/
376
377 free(dim_mem);
378 free(dim_mem2);
379 free(block_size);
380 free(qp_mem);
381 free(qp_mem2);
382 free(qp_sol_mem);
383 free(qp_sol_mem2);
384 free(part_cond_arg_mem);
385 free(ipm_arg_mem);
386 free(part_cond_mem);
387 free(ipm_mem);
388
389 free(u);
390 free(x);
391
392 return 0;
393
394 }
395
396
397