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 "mex.h"
31 #include <stdio.h>
32 #include <stdlib.h>
33 /*#include <math.h>*/
34
35 #include "hpipm_common.h"
36 #include "hpipm_d_ocp_qp_dim.h"
37 #include "hpipm_d_ocp_qp.h"
38 #include "hpipm_d_ocp_qp_sol.h"
39 #include "hpipm_d_ocp_qp_ipm.h"
40
41
42
43
44 // z = beta*y + alpha*A*x
dgemv_n_3l(int m,int n,double alpha,double * A,int lda,double * x,double beta,double * y,double * z)45 void dgemv_n_3l(int m, int n, double alpha, double *A, int lda, double *x, double beta, double *y, double *z)
46 {
47
48 int ii, jj;
49
50 double tmp;
51
52 for(ii=0; ii<m; ii++)
53 z[ii] = beta * y[ii];
54
55 for(jj=0; jj<n; jj++)
56 {
57 tmp = alpha * x[jj];
58 for(ii=0; ii<m; ii++)
59 {
60 z[ii] += A[ii+lda*jj] * tmp;
61 }
62 }
63
64 }
65
66
67
68 // the gateway function
mexFunction(int nlhs,mxArray * plhs[],int nrhs,const mxArray * prhs[])69 void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
70 {
71
72 // get data
73 int k_max;
74 double mu0, tol, *A, *B, *b, *Q, *Qf, *R, *S, *q, *qf, *r, *x, *u, *lb, *ub, *C, *D, *lg, *ug, *CN, *lgN, *ugN, *stat, *kkk, *inf_norm_res, *pi, *lam/*, *t*/;
75
76 kkk = mxGetPr(prhs[0]);
77 k_max = (int) mxGetScalar(prhs[1]);
78 mu0 = mxGetScalar(prhs[2]);
79 tol = mxGetScalar(prhs[3]);
80 const int N = (int) mxGetScalar(prhs[4]);
81 const int nx = (int) mxGetScalar(prhs[5]);
82 const int nu = (int) mxGetScalar(prhs[6]);
83 const int nb = (int) mxGetScalar(prhs[7]);
84 const int ng = (int) mxGetScalar(prhs[8]);
85 const int ngN = (int) mxGetScalar(prhs[9]);
86 const int time_invariant = (int) mxGetScalar(prhs[10]);
87 const int free_x0 = (int) mxGetScalar(prhs[11]);
88 const int warm_start = (int) mxGetScalar(prhs[12]);
89
90 A = mxGetPr(prhs[13]);
91 B = mxGetPr(prhs[14]);
92 b = mxGetPr(prhs[15]);
93 Q = mxGetPr(prhs[16]);
94 Qf = mxGetPr(prhs[17]);
95 R = mxGetPr(prhs[18]);
96 S = mxGetPr(prhs[19]);
97 q = mxGetPr(prhs[20]);
98 qf = mxGetPr(prhs[21]);
99 r = mxGetPr(prhs[22]);
100 lb = mxGetPr(prhs[23]);
101 ub = mxGetPr(prhs[24]);
102 C = mxGetPr(prhs[25]);
103 D = mxGetPr(prhs[26]);
104 lg = mxGetPr(prhs[27]);
105 ug = mxGetPr(prhs[28]);
106 CN = mxGetPr(prhs[29]);
107 lgN = mxGetPr(prhs[30]);
108 ugN = mxGetPr(prhs[31]);
109 x = mxGetPr(prhs[32]);
110 u = mxGetPr(prhs[33]);
111 pi = mxGetPr(prhs[34]);
112 lam = mxGetPr(prhs[35]);
113 // t = mxGetPr(prhs[36]);
114 // inf_norm_res = mxGetPr(prhs[37]);
115 // stat = mxGetPr(prhs[38]);
116 inf_norm_res = mxGetPr(prhs[36]);
117 stat = mxGetPr(prhs[37]);
118
119 int kk = -1;
120
121
122
123 //
124 int ii, jj;
125 int i_tmp;
126
127
128
129 int nb0 = nb<nu ? nb : nu;
130 int nbN = nb-nu>0 ? nb-nu : 0;
131
132
133
134 int nx_v[N+1];
135 nx_v[0] = 0;
136 for(ii=1; ii<=N; ii++)
137 nx_v[ii] = nx;
138
139 int nu_v[N+1];
140 for(ii=0; ii<N; ii++)
141 nu_v[ii] = nu;
142 nu_v[N] = 0;
143
144 int nb_v[N+1];
145 nb_v[0] = nb<nu ? nb : nu;
146 for(ii=1; ii<N; ii++)
147 nb_v[ii] = nb;
148 i_tmp = nb-nu;
149 nb_v[N] = i_tmp<0 ? 0 : i_tmp;
150
151 int nbu_v[N+1];
152 for(ii=0; ii<N; ii++)
153 nbu_v[ii] = nb<nu ? nb : nu;
154 nbu_v[N] = 0;
155
156 int nbx_v[N+1];
157 for(ii=0; ii<=N; ii++)
158 {
159 i_tmp = nb_v[ii]-nbu_v[ii];
160 nbx_v[ii] = i_tmp>=0 ? i_tmp : 0;
161 }
162
163 int ng_v[N+1];
164 for(ii=0; ii<N; ii++)
165 ng_v[ii] = ng;
166 ng_v[N] = ngN;
167
168 int ns_v[N+1];
169 for(ii=0; ii<=N; ii++)
170 ns_v[ii] = 0;
171
172 int nsbx_v[N+1];
173 for(ii=0; ii<=N; ii++)
174 nsbx_v[ii] = 0;
175
176 int nsbu_v[N+1];
177 for(ii=0; ii<=N; ii++)
178 nsbu_v[ii] = 0;
179
180 int nsg_v[N+1];
181 for(ii=0; ii<=N; ii++)
182 nsg_v[ii] = 0;
183
184 int *hidxb[N+1];
185 int *ptr_idx = (int *) malloc((N+1)*nb*sizeof(int));
186 for(ii=0; ii<=N; ii++)
187 {
188 hidxb[ii] = ptr_idx+ii*nb;
189 for(jj=0; jj<nb_v[ii]; jj++)
190 hidxb[ii][jj] = jj;
191 }
192
193
194 double b0[nx];
195 dgemv_n_3l(nx, nx, 1.0, A, nx, x, 1.0, b, b0);
196
197 double r0[nu];
198 dgemv_n_3l(nu, nx, 1.0, S, nu, x, 1.0, r, r0);
199
200 double lb0[nb0];
201 for(ii=0; ii<nb0; ii++)
202 lb0[ii] = lb[ii];
203
204 double ub0[nb0];
205 for(ii=0; ii<nb0; ii++)
206 ub0[ii] = ub[ii];
207
208 double lbN[nbN];
209 double ubN[nbN];
210
211 double lg0[ng];
212 dgemv_n_3l(ng, nx, -1.0, C, ng, x, 1.0, lg, lg0);
213
214 double ug0[ng];
215 dgemv_n_3l(ng, nx, -1.0, C, ng, x, 1.0, ug, ug0);
216
217
218
219 double *hA[N];
220 double *hB[N];
221 double *hb[N];
222 double *hQ[N+1];
223 double *hS[N];
224 double *hR[N];
225 double *hq[N+1];
226 double *hr[N];
227 double *hlb[N+1];
228 double *hub[N+1];
229 double *hC[N+1];
230 double *hD[N];
231 double *hlg[N+1];
232 double *hug[N+1];
233 double *hx[N+1];
234 double *hu[N+1];
235 double *hpi[N];
236 // double *hlam[N+1];
237 // double *ht[N+1];
238 double *hlam_lb[N+1];
239 double *hlam_ub[N+1];
240 double *hlam_lg[N+1];
241 double *hlam_ug[N+1];
242
243
244
245 if(time_invariant)
246 {
247
248 for(ii=1; ii<N; ii++)
249 hA[ii] = A;
250
251 for(ii=0; ii<N; ii++)
252 hB[ii] = B;
253
254 hb[0] = b0;
255 for(ii=1; ii<N; ii++)
256 hb[ii] = b;
257
258 for(ii=1; ii<N; ii++)
259 hQ[ii] = Q;
260 hQ[N] = Qf;
261
262 for(ii=1; ii<N; ii++)
263 hS[ii] = S;
264
265 for(ii=0; ii<N; ii++)
266 hR[ii] = R;
267
268 for(ii=1; ii<N; ii++)
269 hq[ii] = q;
270 hq[N] = qf;
271
272 hr[0] = r0;
273 for(ii=1; ii<N; ii++)
274 hr[ii] = r;
275
276 for(ii=0; ii<nbN; ii++)
277 lbN[ii] = lb[nu+ii];
278
279 hlb[0] = lb0;
280 for(ii=1; ii<N; ii++)
281 hlb[ii] = lb;
282 hlb[N] = lbN;
283
284 for(ii=0; ii<nbN; ii++)
285 ubN[ii] = ub[nu+ii];
286
287 hub[0] = ub0;
288 for(ii=1; ii<N; ii++)
289 hub[ii] = ub;
290 hub[N] = ubN;
291
292 for(ii=1; ii<N; ii++)
293 hC[ii] = C;
294 hC[N] = CN;
295
296 for(ii=0; ii<N; ii++)
297 hD[ii] = D;
298
299 hlg[0] = lg0;
300 for(ii=1; ii<N; ii++)
301 hlg[ii] = lg;
302 hlg[N] = lgN;
303
304 hug[0] = ug0;
305 for(ii=1; ii<N; ii++)
306 hug[ii] = ug;
307 hug[N] = ugN;
308
309 }
310 else
311 {
312
313 for(ii=1; ii<N; ii++)
314 hA[ii] = A+ii*nx*nx;
315
316 for(ii=0; ii<N; ii++)
317 hB[ii] = B+ii*nx*nu;
318
319 hb[0] = b0;
320 for(ii=1; ii<N; ii++)
321 hb[ii] = b+ii*nx;
322
323 for(ii=1; ii<N; ii++)
324 hQ[ii] = Q+ii*nx*nx;
325 hQ[N] = Qf;
326
327 for(ii=1; ii<N; ii++)
328 hS[ii] = S+ii*nu*nx;
329
330 for(ii=0; ii<N; ii++)
331 hR[ii] = R+ii*nu*nu;
332
333 for(ii=1; ii<N; ii++)
334 hq[ii] = q+ii*nx;
335 hq[N] = qf;
336
337 hr[0] = r0;
338 for(ii=1; ii<N; ii++)
339 hr[ii] = r+ii*nu;
340
341 for(ii=0; ii<nbN; ii++)
342 lbN[ii] = lb[nu+ii];
343 hlb[0] = lb0;
344 for(ii=1; ii<N; ii++)
345 hlb[ii] = lb+ii*nb;
346 hlb[N] = lbN;
347
348 for(ii=0; ii<nbN; ii++)
349 ubN[ii] = ub[nu+ii];
350 hub[0] = ub0;
351 for(ii=1; ii<N; ii++)
352 hub[ii] = ub+ii*nb;
353 hub[N] = ubN;
354
355 for(ii=1; ii<N; ii++)
356 hC[ii] = C+ii*ng*nx;
357 hC[N] = CN;
358
359 for(ii=0; ii<N; ii++)
360 hD[ii] = D+ii*ng*nu;
361
362 hlg[0] = lg0;
363 for(ii=1; ii<N; ii++)
364 hlg[ii] = lg+ii*ng;
365 hlg[N] = lgN;
366
367 hug[0] = ug0;
368 for(ii=1; ii<N; ii++)
369 hug[ii] = ug+ii*ng;
370 hug[N] = ugN;
371
372 }
373
374 for(ii=0; ii<=N; ii++)
375 hx[ii] = x+ii*nx;
376
377 for(ii=0; ii<N; ii++)
378 hu[ii] = u+ii*nu;
379
380 for(ii=0; ii<N; ii++)
381 hpi[ii] = pi+ii*nx;
382
383 // for(ii=0; ii<=N; ii++)
384 // hlam[ii] = lam+ii*(2*nb+2*ng);
385
386 // for(ii=0; ii<=N; ii++)
387 // ht[ii] = t+ii*(2*nb+2*ng);
388
389 for(ii=0; ii<=N; ii++)
390 {
391 hlam_lb[ii] = lam+ii*nb;
392 hlam_ub[ii] = lam+(N+1)*nb+ii*nb;
393 hlam_lg[ii] = lam+2*(N+1)*nb+ii*ng;
394 // hlam_ug[ii] = lam+2*(N+1)*nb+(N+1)*ng+ii*ng;
395 hlam_ug[ii] = lam+2*(N+1)*nb+N*ng+ngN+ii*ng;
396 }
397
398 // Partial condensing horizon
399 // int N2 = N;
400
401 // int work_space_size = hpmpc_d_ip_ocp_hard_tv_work_space_size_bytes(N, nx_v, nu_v, nb_v, hidxb, ng_v, N2);
402 // void *work = malloc( work_space_size );
403
404
405
406 // call solver
407 // fortran_order_d_ip_ocp_hard_tv(&kk, k_max, mu0, tol, N, nx_v, nu_v, nb_v, hidxb, ng_v, N2, warm_start, hA, hB, hb, hQ, hS, hR, hq, hr, hlb, hub, hC, hD, hlg, hug, hx, hu, hpi, hlam, /*ht,*/ inf_norm_res, work, stat);
408
409
410
411
412 // qp dim
413 int dim_size = d_memsize_ocp_qp_dim(N);
414 void *dim_mem = malloc(dim_size);
415
416 struct d_ocp_qp_dim dim;
417 d_create_ocp_qp_dim(N, &dim, dim_mem);
418 d_cvt_int_to_ocp_qp_dim(N, nx_v, nu_v, nbx_v, nbu_v, ng_v, nsbx_v, nsbu_v, nsg_v, &dim);
419
420 // for(ii=0; ii<=N; ii++)
421 // printf("\n%d %d %d %d %d %d %d\n", nx_v[ii], nu_v[ii], nb_v[ii], nbu_v[ii], nbx_v[ii], ng_v[ii], ns_v[ii]);
422
423
424 // qp
425 int qp_size = d_memsize_ocp_qp(&dim);
426 void *qp_mem = malloc(qp_size);
427 struct d_ocp_qp qp;
428 d_create_ocp_qp(&dim, &qp, qp_mem);
429 d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hlb, hub, hC, hD, hlg, hug, NULL, NULL, NULL, NULL, NULL, NULL, NULL, &qp);
430
431
432 // qp sol
433 int qp_sol_size = d_memsize_ocp_qp_sol(&dim);
434 void *qp_sol_mem = malloc(qp_sol_size);
435 struct d_ocp_qp_sol qp_sol;
436 d_create_ocp_qp_sol(&dim, &qp_sol, qp_sol_mem);
437
438
439 // ipm arg
440 int ipm_arg_size = d_memsize_ocp_qp_ipm_arg(&dim);
441 void *ipm_arg_mem = malloc(ipm_arg_size);
442
443 struct d_ocp_qp_ipm_arg arg;
444 d_create_ocp_qp_ipm_arg(&dim, &arg, ipm_arg_mem);
445 enum hpipm_mode mode = SPEED;
446 // enum hpipm_mode mode = BALANCE;
447 // enum hpipm_mode mode = ROBUST;
448 d_set_default_ocp_qp_ipm_arg(mode, &arg);
449
450 // arg.alpha_min = 1e-12;
451 arg.res_g_max = tol;
452 arg.res_b_max = tol;
453 arg.res_d_max = tol;
454 arg.res_m_max = tol;
455 arg.mu0 = mu0;
456 arg.iter_max = k_max;
457 arg.stat_max = k_max;
458 // arg.pred_corr = 1;
459 // arg.cond_pred_corr = 1;
460
461
462 // ipm
463 int ipm_size = d_memsize_ocp_qp_ipm(&dim, &arg);
464 void *ipm_mem = malloc(ipm_size);
465
466 struct d_ocp_qp_ipm_workspace workspace;
467 d_create_ocp_qp_ipm(&dim, &arg, &workspace, ipm_mem);
468
469 // call solver
470 int hpipm_return = d_solve_ocp_qp_ipm(&qp, &qp_sol, &arg, &workspace);
471
472 // convert back solution
473 d_cvt_ocp_qp_sol_to_colmaj(&qp_sol, hu, hx, NULL, NULL, hpi, hlam_lb, hlam_ub, hlam_lg, hlam_ug, NULL, NULL);
474
475 // inf norm residual
476 inf_norm_res[0] = workspace.qp_res[0];
477 inf_norm_res[1] = workspace.qp_res[1];
478 inf_norm_res[2] = workspace.qp_res[2];
479 inf_norm_res[3] = workspace.qp_res[3];
480
481 // stat
482 for(jj=0; jj<workspace.iter; jj++)
483 for(ii=0; ii<5; ii++)
484 stat[ii+5*jj] = workspace.stat[ii+5*jj];
485
486
487
488 *kkk = (double) workspace.iter;
489
490
491 free(ptr_idx);
492 free(dim_mem);
493 free(qp_mem);
494 free(qp_sol_mem);
495 free(ipm_arg_mem);
496 free(ipm_mem);
497
498
499 return;
500
501 }
502
503