Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 16ce3c78abe894f77f0a95bbb88530f3f100c509 / . / ocp_qp / x_ocp_qp_sol.c
blob: 274f0f0cce8f337c608958d65ade44718890147f [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_SOL(int N, int *nx, int *nu, int *nb, int *ng)
31 {
32
33 int ii;
34
35 int nvt = 0;
36 int net = 0;
37 int nbt = 0;
38 int ngt = 0;
39 for(ii=0; ii<N; ii++)
40 {
41 nvt += nu[ii]+nx[ii];
42 net += nx[ii+1];
43 nbt += nb[ii];
44 ngt += ng[ii];
45 }
46 nvt += nu[ii]+nx[ii];
47 nbt += nb[ii];
48 ngt += ng[ii];
49
50 int size = 0;
51
52 size += (0+10*(N+1))*sizeof(struct STRVEC); // ux pi lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
53
54 size += 1*SIZE_STRVEC(nvt); // ux
55 size += 1*SIZE_STRVEC(net); // pi
56 size += 8*SIZE_STRVEC(2*nbt+2*ngt); // lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
57
58 size = (size+63)/64*64; // make multiple of typical cache line size
59 size += 64; // align to typical cache line size
60
61 return size;
62
63 }
64
65
66
67void CREATE_OCP_QP_SOL(int N, int *nx, int *nu, int *nb, int *ng, struct OCP_QP_SOL *qp_sol, void *memory)
68 {
69
70 int ii;
71
72
73 // memsize
74 qp_sol->memsize = MEMSIZE_OCP_QP_SOL(N, nx, nu, nb, ng);
75
76
77 int nvt = 0;
78 int net = 0;
79 int nbt = 0;
80 int ngt = 0;
81 for(ii=0; ii<N; ii++)
82 {
83 nvt += nu[ii]+nx[ii];
84 net += nx[ii+1];
85 nbt += nb[ii];
86 ngt += ng[ii];
87 }
88 nvt += nu[ii]+nx[ii];
89 nbt += nb[ii];
90 ngt += ng[ii];
91
92
93 // vector struct stuff
94 struct STRVEC *sv_ptr = (struct STRVEC *) memory;
95
96 qp_sol->ux = sv_ptr;
97 sv_ptr += N+1;
98 qp_sol->pi = sv_ptr;
99 sv_ptr += N+1;
100 qp_sol->lam_lb = sv_ptr;
101 sv_ptr += N+1;
102 qp_sol->lam_ub = sv_ptr;
103 sv_ptr += N+1;
104 qp_sol->lam_lg = sv_ptr;
105 sv_ptr += N+1;
106 qp_sol->lam_ug = sv_ptr;
107 sv_ptr += N+1;
108 qp_sol->t_lb = sv_ptr;
109 sv_ptr += N+1;
110 qp_sol->t_ub = sv_ptr;
111 sv_ptr += N+1;
112 qp_sol->t_lg = sv_ptr;
113 sv_ptr += N+1;
114 qp_sol->t_ug = sv_ptr;
115 sv_ptr += N+1;
116
117
118 // align to typical cache line size
119 long long l_ptr = (long long) sv_ptr;
120 l_ptr = (l_ptr+63)/64*64;
121
122
123 // double stuff
124 void *v_ptr;
125 v_ptr = (void *) l_ptr;
126
127 void *tmp_ptr;
128
129 // ux
130 tmp_ptr = v_ptr;
131 v_ptr += SIZE_STRVEC(nvt);
132 for(ii=0; ii<=N; ii++)
133 {
134 CREATE_STRVEC(nu[ii]+nx[ii], qp_sol->ux+ii, tmp_ptr);
135 tmp_ptr += (nu[ii]+nx[ii])*sizeof(REAL);
136 }
137 // pi
138 tmp_ptr = v_ptr;
139 v_ptr += SIZE_STRVEC(net);
140 for(ii=0; ii<N; ii++)
141 {
142 CREATE_STRVEC(nx[ii+1], qp_sol->pi+ii, tmp_ptr);
143 tmp_ptr += (nx[ii+1])*sizeof(REAL);
144 }
145 // lam
146 tmp_ptr = v_ptr;
147 v_ptr += SIZE_STRVEC(2*nbt+2*ngt);
148 // lam_lb
149 for(ii=0; ii<=N; ii++)
150 {
151 CREATE_STRVEC(nb[ii], qp_sol->lam_lb+ii, tmp_ptr);
152 tmp_ptr += (nb[ii])*sizeof(REAL);
153 }
154 // lam_lg
155 for(ii=0; ii<=N; ii++)
156 {
157 CREATE_STRVEC(ng[ii], qp_sol->lam_lg+ii, tmp_ptr);
158 tmp_ptr += (ng[ii])*sizeof(REAL);
159 }
160 // lam_ub
161 for(ii=0; ii<=N; ii++)
162 {
163 CREATE_STRVEC(nb[ii], qp_sol->lam_ub+ii, tmp_ptr);
164 tmp_ptr += (nb[ii])*sizeof(REAL);
165 }
166 // lam_ug
167 for(ii=0; ii<=N; ii++)
168 {
169 CREATE_STRVEC(ng[ii], qp_sol->lam_ug+ii, tmp_ptr);
170 tmp_ptr += (ng[ii])*sizeof(REAL);
171 }
172 // t
173 tmp_ptr = v_ptr;
174 v_ptr += SIZE_STRVEC(2*nbt+2*ngt);
175 // t_lb
176 for(ii=0; ii<=N; ii++)
177 {
178 CREATE_STRVEC(nb[ii], qp_sol->t_lb+ii, tmp_ptr);
179 tmp_ptr += (nb[ii])*sizeof(REAL);
180 }
181 // t_lg
182 for(ii=0; ii<=N; ii++)
183 {
184 CREATE_STRVEC(ng[ii], qp_sol->t_lg+ii, tmp_ptr);
185 tmp_ptr += (ng[ii])*sizeof(REAL);
186 }
187 // t_ub
188 for(ii=0; ii<=N; ii++)
189 {
190 CREATE_STRVEC(nb[ii], qp_sol->t_ub+ii, tmp_ptr);
191 tmp_ptr += (nb[ii])*sizeof(REAL);
192 }
193 // t_ug
194 for(ii=0; ii<=N; ii++)
195 {
196 CREATE_STRVEC(ng[ii], qp_sol->t_ug+ii, tmp_ptr);
197 tmp_ptr += (ng[ii])*sizeof(REAL);
198 }
199
200 return;
201
202 }
203
204
205
206void CVT_OCP_QP_SOL_TO_COLMAJ(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, REAL **u, REAL **x, REAL **pi, REAL **lam_lb, REAL **lam_ub, REAL **lam_lg, REAL **lam_ug)
207 {
208
209 int N = qp->N;
210 int *nx = qp->nx;
211 int *nu = qp->nu;
212 int *nb = qp->nb;
213 int *ng = qp->ng;
214
215 int ii;
216
217 for(ii=0; ii<N; ii++)
218 {
219 CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
220 CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
221 CVT_STRVEC2VEC(nx[ii+1], qp_sol->pi+ii, 0, pi[ii]);
222 CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
223 CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
224 CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
225 CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
226 }
227 CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
228 CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
229 CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
230 CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
231 CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
232 CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
233
234 return;
235
236 }
237
238
239
240void CVT_OCP_QP_SOL_TO_ROWMAJ(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, REAL **u, REAL **x, REAL **pi, REAL **lam_lb, REAL **lam_ub, REAL **lam_lg, REAL **lam_ug)
241 {
242
243 int N = qp->N;
244 int *nx = qp->nx;
245 int *nu = qp->nu;
246 int *nb = qp->nb;
247 int *ng = qp->ng;
248
249 int ii;
250
251 for(ii=0; ii<N; ii++)
252 {
253 CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
254 CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
255 CVT_STRVEC2VEC(nx[ii+1], qp_sol->pi+ii, 0, pi[ii]);
256 CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
257 CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
258 CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
259 CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
260 }
261 CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
262 CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
263 CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
264 CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
265 CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
266 CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
267
268 return;
269
270 }
271
272
273
274void CVT_OCP_QP_SOL_TO_LIBSTR(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct STRVEC *u, struct STRVEC *x, struct STRVEC *pi, struct STRVEC *lam_lb, struct STRVEC *lam_ub, struct STRVEC *lam_lg, struct STRVEC *lam_ug)
275 {
276
277 int N = qp->N;
278 int *nx = qp->nx;
279 int *nu = qp->nu;
280 int *nb = qp->nb;
281 int *ng = qp->ng;
282
283 int ii;
284
285 for(ii=0; ii<N; ii++)
286 {
287 VECCP_LIBSTR(nu[ii], qp_sol->ux+ii, 0, u+ii, 0);
288 VECCP_LIBSTR(nx[ii], qp_sol->ux+ii, nu[ii], x+ii, 0);
289 VECCP_LIBSTR(nx[ii+1], qp_sol->pi+ii, 0, pi+ii, 0);
290 VECCP_LIBSTR(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb+ii, 0);
291 VECCP_LIBSTR(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub+ii, 0);
292 VECCP_LIBSTR(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg+ii, 0);
293 VECCP_LIBSTR(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug+ii, 0);
294 }
295 VECCP_LIBSTR(nu[ii], qp_sol->ux+ii, 0, u+ii, 0);
296 VECCP_LIBSTR(nx[ii], qp_sol->ux+ii, nu[ii], x+ii, 0);
297 VECCP_LIBSTR(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb+ii, 0);
298 VECCP_LIBSTR(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub+ii, 0);
299 VECCP_LIBSTR(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg+ii, 0);
300 VECCP_LIBSTR(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug+ii, 0);
301
302 return;
303
304 }