Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 16ce3c78abe894f77f0a95bbb88530f3f100c509 / . / ocp_qp / x_ocp_qp.c
blob: bc5605a03c56a98fa86ee64a6c3f493ba1f2316f [file] [log] [blame]
Austin Schuh16ce3c72018-01-28 16:17:08 -0800[diff] [blame^]1/**************************************************************************************************
2* *
3* This file is part of HPIPM. *
4* *
5* HPIPM -- High Performance Interior Point Method. *
6* Copyright (C) 2017 by Gianluca Frison. *
7* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
8* All rights reserved. *
9* *
10* HPMPC is free software; you can redistribute it and/or *
11* modify it under the terms of the GNU Lesser General Public *
12* License as published by the Free Software Foundation; either *
13* version 2.1 of the License, or (at your option) any later version. *
14* *
15* HPMPC 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. *
18* See the GNU Lesser General Public License for more details. *
19* *
20* You should have received a copy of the GNU Lesser General Public *
21* License along with HPMPC; if not, write to the Free Software *
22* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
23* *
24* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
25* *
26**************************************************************************************************/
27
28
29
30int MEMSIZE_OCP_QP(int N, int *nx, int *nu, int *nb, int *ng)
31 {
32
33 int ii;
34
35 int size = 0;
36
37 size += 4*(N+1)*sizeof(int); // nx nu nb ng
38 size += (N+1)*sizeof(int *); // idxb
39 size += (1*N+2*(N+1))*sizeof(struct STRMAT); // BAbt ; RSqrq DCt
40 size += (1*N+5*(N+1))*sizeof(struct STRVEC); // b ; rq d_lb d_ub d_lg d_ug
41
42 for(ii=0; ii<N; ii++)
43 {
44 size += nb[ii]*sizeof(int); // idxb
45 size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nx[ii+1]); // BAbt
46 size += SIZE_STRVEC(nx[ii+1]); // b
47 size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // RSQrq
48 size += SIZE_STRVEC(nu[ii]+nx[ii]); // rq
49 size += SIZE_STRMAT(nu[ii]+nx[ii], ng[ii]); // DCt
50 size += 2*SIZE_STRVEC(nb[ii]); // d_lb d_ub
51 size += 2*SIZE_STRVEC(ng[ii]); // d_lg d_ug
52 }
53 ii = N;
54 size += nb[ii]*sizeof(int); // idxb
55 size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // RSQrq
56 size += SIZE_STRVEC(nu[ii]+nx[ii]); // rq
57 size += SIZE_STRMAT(nu[ii]+nx[ii], ng[ii]); // DCt
58 size += 2*SIZE_STRVEC(nb[ii]); // d_lb d_ub
59 size += 2*SIZE_STRVEC(ng[ii]); // d_lg d_ug
60
61 size = (size+63)/64*64; // make multiple of typical cache line size
62 size += 64; // align to typical cache line size
63
64 return size;
65
66 }
67
68
69
70void CREATE_OCP_QP(int N, int *nx, int *nu, int *nb, int *ng, struct OCP_QP *qp, void *memory)
71 {
72
73 int ii;
74
75
76 // memsize
77 qp->memsize = MEMSIZE_OCP_QP(N, nx, nu, nb, ng);
78
79
80 // horizon length
81 qp->N = N;
82
83
84 // int pointer stuff
85 int **ip_ptr;
86 ip_ptr = (int **) memory;
87
88 // idxb
89 qp->idxb = ip_ptr;
90 ip_ptr += N+1;
91
92
93 // matrix struct stuff
94 struct STRMAT *sm_ptr = (struct STRMAT *) ip_ptr;
95
96 // BAbt
97 qp->BAbt = sm_ptr;
98 sm_ptr += N;
99
100 // RSQrq
101 qp->RSQrq = sm_ptr;
102 sm_ptr += N+1;
103
104 // DCt
105 qp->DCt = sm_ptr;
106 sm_ptr += N+1;
107
108
109 // vector struct stuff
110 struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr;
111
112 // b
113 qp->b = sv_ptr;
114 sv_ptr += N;
115
116 // rq
117 qp->rq = sv_ptr;
118 sv_ptr += N+1;
119
120 // d_lb
121 qp->d_lb = sv_ptr;
122 sv_ptr += N+1;
123
124 // d_ub
125 qp->d_ub = sv_ptr;
126 sv_ptr += N+1;
127
128 // d_lg
129 qp->d_lg = sv_ptr;
130 sv_ptr += N+1;
131
132 // d_ug
133 qp->d_ug = sv_ptr;
134 sv_ptr += N+1;
135
136
137 // integer stuff
138 int *i_ptr;
139 i_ptr = (int *) sv_ptr;
140
141 // nx
142 qp->nx = i_ptr;
143 for(ii=0; ii<=N; ii++)
144 {
145 i_ptr[ii] = nx[ii];
146 }
147 i_ptr += N+1;
148
149 // nu
150 qp->nu = i_ptr;
151 for(ii=0; ii<=N; ii++)
152 {
153 i_ptr[ii] = nu[ii];
154 }
155 i_ptr += N+1;
156
157 // nb
158 qp->nb = i_ptr;
159 for(ii=0; ii<=N; ii++)
160 {
161 i_ptr[ii] = nb[ii];
162 }
163 i_ptr += N+1;
164
165 // ng
166 qp->ng = i_ptr;
167 for(ii=0; ii<=N; ii++)
168 {
169 i_ptr[ii] = ng[ii];
170 }
171 i_ptr += N+1;
172
173 // idxb
174 for(ii=0; ii<=N; ii++)
175 {
176 (qp->idxb)[ii] = i_ptr;
177 i_ptr += nb[ii];
178 }
179
180
181 // align to typical cache line size
182 long long l_ptr = (long long) i_ptr;
183 l_ptr = (l_ptr+63)/64*64;
184
185
186 // double stuff
187 char *c_ptr;
188 c_ptr = (char *) l_ptr;
189
190 // BAbt
191 for(ii=0; ii<N; ii++)
192 {
193 CREATE_STRMAT(nu[ii]+nx[ii]+1, nx[ii+1], qp->BAbt+ii, c_ptr);
194 c_ptr += (qp->BAbt+ii)->memory_size;
195 }
196
197 // RSQrq
198 for(ii=0; ii<=N; ii++)
199 {
200 CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], qp->RSQrq+ii, c_ptr);
201 c_ptr += (qp->RSQrq+ii)->memory_size;
202 }
203
204 // DCt
205 for(ii=0; ii<=N; ii++)
206 {
207 CREATE_STRMAT(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, c_ptr);
208 c_ptr += (qp->DCt+ii)->memory_size;
209 }
210
211 // b
212 for(ii=0; ii<N; ii++)
213 {
214 CREATE_STRVEC(nx[ii+1], qp->b+ii, c_ptr);
215 c_ptr += (qp->b+ii)->memory_size;
216 }
217
218 // rq
219 for(ii=0; ii<=N; ii++)
220 {
221 CREATE_STRVEC(nu[ii]+nx[ii], qp->rq+ii, c_ptr);
222 c_ptr += (qp->rq+ii)->memory_size;
223 }
224
225 // d_lb
226 for(ii=0; ii<=N; ii++)
227 {
228 CREATE_STRVEC(nb[ii], qp->d_lb+ii, c_ptr);
229 c_ptr += (qp->d_lb+ii)->memory_size;
230 }
231
232 // d_ub
233 for(ii=0; ii<=N; ii++)
234 {
235 CREATE_STRVEC(nb[ii], qp->d_ub+ii, c_ptr);
236 c_ptr += (qp->d_ub+ii)->memory_size;
237 }
238
239 // d_lg
240 for(ii=0; ii<=N; ii++)
241 {
242 CREATE_STRVEC(ng[ii], qp->d_lg+ii, c_ptr);
243 c_ptr += (qp->d_lg+ii)->memory_size;
244 }
245
246 // d_ug
247 for(ii=0; ii<=N; ii++)
248 {
249 CREATE_STRVEC(ng[ii], qp->d_ug+ii, c_ptr);
250 c_ptr += (qp->d_ug+ii)->memory_size;
251 }
252
253 return;
254
255 }
256
257
258
259void CAST_OCP_QP(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, struct STRMAT *BAbt, struct STRVEC *b, struct STRMAT *RSQrq, struct STRVEC *rq, struct STRMAT *DCt, struct STRVEC *d_lb, struct STRVEC *d_ub, struct STRVEC *d_lg, struct STRVEC *d_ug, struct OCP_QP *qp)
260 {
261
262 qp->N = N;
263 qp->nx = nx;
264 qp->nu = nu;
265 qp->nb = nb;
266 qp->idxb = idxb;
267 qp->ng = ng;
268 qp->BAbt = BAbt;
269 qp->b = b;
270 qp->RSQrq = RSQrq;
271 qp->rq = rq;
272 qp->DCt = DCt;
273 qp->d_lb = d_lb;
274 qp->d_ub = d_ub;
275 qp->d_lg = d_lg;
276 qp->d_ug = d_ug;
277
278 return;
279
280 }
281
282
283
284void CVT_COLMAJ_TO_OCP_QP(REAL **A, REAL **B, REAL **b, REAL **Q, REAL **S, REAL **R, REAL **q, REAL **r, int **idxb, REAL **d_lb, REAL **d_ub, REAL **C, REAL **D, REAL **d_lg, REAL **d_ug, struct OCP_QP *qp)
285 {
286
287 int N = qp->N;
288 int *nx = qp->nx;
289 int *nu = qp->nu;
290 int *nb = qp->nb;
291 int *ng = qp->ng;
292
293 int ii, jj;
294
295 for(ii=0; ii<N; ii++)
296 {
297 CVT_TRAN_MAT2STRMAT(nx[ii+1], nu[ii], B[ii], nx[ii+1], qp->BAbt+ii, 0, 0);
298 CVT_TRAN_MAT2STRMAT(nx[ii+1], nx[ii], A[ii], nx[ii+1], qp->BAbt+ii, nu[ii], 0);
299 CVT_TRAN_MAT2STRMAT(nx[ii+1], 1, b[ii], nx[ii+1], qp->BAbt+ii, nu[ii]+nx[ii], 0);
300 CVT_VEC2STRVEC(nx[ii+1], b[ii], qp->b+ii, 0);
301 }
302
303 for(ii=0; ii<=N; ii++)
304 {
305 CVT_MAT2STRMAT(nu[ii], nu[ii], R[ii], nu[ii], qp->RSQrq+ii, 0, 0);
306 CVT_TRAN_MAT2STRMAT(nu[ii], nx[ii], S[ii], nu[ii], qp->RSQrq+ii, nu[ii], 0);
307 CVT_MAT2STRMAT(nx[ii], nx[ii], Q[ii], nx[ii], qp->RSQrq+ii, nu[ii], nu[ii]);
308 CVT_TRAN_MAT2STRMAT(nu[ii], 1, r[ii], nu[ii], qp->RSQrq+ii, nu[ii]+nx[ii], 0);
309 CVT_TRAN_MAT2STRMAT(nx[ii], 1, q[ii], nx[ii], qp->RSQrq+ii, nu[ii]+nx[ii], nu[ii]);
310 CVT_VEC2STRVEC(nu[ii], r[ii], qp->rq+ii, 0);
311 CVT_VEC2STRVEC(nx[ii], q[ii], qp->rq+ii, nu[ii]);
312 }
313
314 for(ii=0; ii<=N; ii++)
315 {
316 for(jj=0; jj<nb[ii]; jj++)
317 qp->idxb[ii][jj] = idxb[ii][jj];
318 CVT_VEC2STRVEC(nb[ii], d_lb[ii], qp->d_lb+ii, 0);
319 CVT_VEC2STRVEC(nb[ii], d_ub[ii], qp->d_ub+ii, 0);
320 }
321
322 for(ii=0; ii<=N; ii++)
323 {
324 CVT_TRAN_MAT2STRMAT(ng[ii], nu[ii], D[ii], ng[ii], qp->DCt+ii, 0, 0);
325 CVT_TRAN_MAT2STRMAT(ng[ii], nx[ii], C[ii], ng[ii], qp->DCt+ii, nu[ii], 0);
326 CVT_VEC2STRVEC(ng[ii], d_lg[ii], qp->d_lg+ii, 0);
327 CVT_VEC2STRVEC(ng[ii], d_ug[ii], qp->d_ug+ii, 0);
328 }
329
330 return;
331
332 }
333
334
335
336void CVT_ROWMAJ_TO_OCP_QP(REAL **A, REAL **B, REAL **b, REAL **Q, REAL **S, REAL **R, REAL **q, REAL **r, int **idxb, REAL **d_lb, REAL **d_ub, REAL **C, REAL **D, REAL **d_lg, REAL **d_ug, struct OCP_QP *qp)
337 {
338
339 int N = qp->N;
340 int *nx = qp->nx;
341 int *nu = qp->nu;
342 int *nb = qp->nb;
343 int *ng = qp->ng;
344
345 int ii, jj;
346
347 for(ii=0; ii<N; ii++)
348 {
349 CVT_MAT2STRMAT(nu[ii], nx[ii+1], B[ii], nu[ii], qp->BAbt+ii, 0, 0);
350 CVT_MAT2STRMAT(nx[ii], nx[ii+1], A[ii], nx[ii], qp->BAbt+ii, nu[ii], 0);
351 CVT_TRAN_MAT2STRMAT(nx[ii+1], 1, b[ii], nx[ii+1], qp->BAbt+ii, nu[ii]+nx[ii], 0);
352 CVT_VEC2STRVEC(nx[ii+1], b[ii], qp->b+ii, 0);
353 }
354
355 for(ii=0; ii<=N; ii++)
356 {
357 CVT_TRAN_MAT2STRMAT(nu[ii], nu[ii], R[ii], nu[ii], qp->RSQrq+ii, 0, 0);
358 CVT_MAT2STRMAT(nx[ii], nu[ii], S[ii], nx[ii], qp->RSQrq+ii, nu[ii], 0);
359 CVT_TRAN_MAT2STRMAT(nx[ii], nx[ii], Q[ii], nx[ii], qp->RSQrq+ii, nu[ii], nu[ii]);
360 CVT_TRAN_MAT2STRMAT(nu[ii], 1, r[ii], nu[ii], qp->RSQrq+ii, nu[ii]+nx[ii], 0);
361 CVT_TRAN_MAT2STRMAT(nx[ii], 1, q[ii], nx[ii], qp->RSQrq+ii, nu[ii]+nx[ii], nu[ii]);
362 CVT_VEC2STRVEC(nu[ii], r[ii], qp->rq+ii, 0);
363 CVT_VEC2STRVEC(nx[ii], q[ii], qp->rq+ii, nu[ii]);
364 }
365
366 for(ii=0; ii<=N; ii++)
367 {
368 for(jj=0; jj<nb[ii]; jj++)
369 qp->idxb[ii][jj] = idxb[ii][jj];
370 CVT_VEC2STRVEC(nb[ii], d_lb[ii], qp->d_lb+ii, 0);
371 CVT_VEC2STRVEC(nb[ii], d_ub[ii], qp->d_ub+ii, 0);
372 }
373
374 for(ii=0; ii<=N; ii++)
375 {
376 CVT_MAT2STRMAT(nu[ii], ng[ii], D[ii], nu[ii], qp->DCt+ii, 0, 0);
377 CVT_MAT2STRMAT(nx[ii], ng[ii], C[ii], nx[ii], qp->DCt+ii, nu[ii], 0);
378 CVT_VEC2STRVEC(ng[ii], d_lg[ii], qp->d_lg+ii, 0);
379 CVT_VEC2STRVEC(ng[ii], d_ug[ii], qp->d_ug+ii, 0);
380 }
381
382 return;
383
384 }
385
386
387
388void COPY_OCP_QP(struct OCP_QP *qp_in, struct OCP_QP *qp_out)
389 {
390
391 int N = qp_in->N;
392 int *nx = qp_in->nx;
393 int *nu = qp_in->nu;
394 int *nb = qp_in->nb;
395 int *ng = qp_in->ng;
396
397 int ii, jj;
398
399#if defined(RUNTIME_CHECKS)
400 if(qp_out->N != qp_in->N)
401 {
402 printf("\nError : x_copy_ocp_qp : qp_out->N != qp_out->N : %d != %d\n\n", qp_out->N, qp_in->N);
403 exit(1);
404 }
405 for(ii=0; ii<=N; ii++)
406 {
407 if(qp_out->nx[ii] != qp_in->nx[ii])
408 {
409 printf("\nError : x_copy_ocp_qp : qp_out->nx[%d] != qp_out->nx[%d] : %d != %d\n\n", ii, ii, qp_out->nx[ii], qp_in->nx[ii]);
410 exit(1);
411 }
412 if(qp_out->nu[ii] != qp_in->nu[ii])
413 {
414 printf("\nError : x_copy_ocp_qp : qp_out->nu[%d] != qp_out->nu[%d] : %d != %d\n\n", ii, ii, qp_out->nu[ii], qp_in->nu[ii]);
415 exit(1);
416 }
417 if(qp_out->nb[ii] != qp_in->nb[ii])
418 {
419 printf("\nError : x_copy_ocp_qp : qp_out->nb[%d] != qp_out->nb[%d] : %d != %d\n\n", ii, ii, qp_out->nb[ii], qp_in->nb[ii]);
420 exit(1);
421 }
422 if(qp_out->ng[ii] != qp_in->ng[ii])
423 {
424 printf("\nError : x_copy_ocp_qp : qp_out->ng[%d] != qp_out->ng[%d] : %d != %d\n\n", ii, ii, qp_out->ng[ii], qp_in->ng[ii]);
425 exit(1);
426 }
427 }
428#endif
429
430 for(ii=0; ii<N; ii++)
431 {
432 for(jj=0; jj<qp_in->nb[ii]; jj++) qp_out->idxb[ii][jj] = qp_in->idxb[ii][jj];
433 GECP_LIBSTR(nx[ii]+nu[ii]+1, nx[ii+1], qp_in->BAbt+ii, 0, 0, qp_out->BAbt+ii, 0, 0);
434 VECCP_LIBSTR(nx[ii+1], qp_in->b+ii, 0, qp_out->b+ii, 0);
435 GECP_LIBSTR(nx[ii]+nu[ii]+1, nu[ii]+nx[ii], qp_in->RSQrq+ii, 0, 0, qp_out->RSQrq+ii, 0, 0);
436 VECCP_LIBSTR(nu[ii]+nx[ii], qp_in->rq+ii, 0, qp_out->rq+ii, 0);
437 GECP_LIBSTR(nx[ii]+nu[ii], ng[ii], qp_in->DCt+ii, 0, 0, qp_out->DCt+ii, 0, 0);
438 VECCP_LIBSTR(nb[ii], qp_in->d_lb+ii, 0, qp_out->d_lb+ii, 0);
439 VECCP_LIBSTR(nb[ii], qp_in->d_ub+ii, 0, qp_out->d_ub+ii, 0);
440 VECCP_LIBSTR(ng[ii], qp_in->d_lg+ii, 0, qp_out->d_lg+ii, 0);
441 VECCP_LIBSTR(ng[ii], qp_in->d_ug+ii, 0, qp_out->d_ug+ii, 0);
442 }
443
444 return;
445
446 }
447
448