Gitiles
Code Review Sign In
realtimeroboticsgroup.org / RealtimeRoboticsGroup / test / 59a86d621dd5604f6ab68c4cb15c552e8926cf4e / . / third_party / hpipm / cond / d_cond.c
blob: 299efa55fdddd2e39f5748c1a8a0a509250aeb22 [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#include <math.h>
29#include <stdlib.h>
30#include <stdio.h>
31
32#include <blasfeo_target.h>
33#include <blasfeo_common.h>
34#include <blasfeo_d_blas.h>
35#include <blasfeo_d_aux.h>
36
37#include "../include/hpipm_d_ocp_qp.h"
38#include "../include/hpipm_d_ocp_qp_sol.h"
39#include "../include/hpipm_d_dense_qp.h"
40#include "../include/hpipm_d_dense_qp_sol.h"
41#include "../include/hpipm_d_cond.h"
42#include "../include/hpipm_d_cond_aux.h"
43
44
45
46void d_compute_qp_size_ocp2dense(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, int *nvc, int *nec, int *nbc, int *ngc)
47 {
48
49 int ii, jj;
50
51 nvc[0] = 0;
52 nec[0] = 0;
53 nbc[0] = 0;
54 ngc[0] = 0;
55
56 // first stage
57 nvc[0] += nx[0]+nu[0];
58 nbc[0] += nb[0];
59 ngc[0] += ng[0];
60 // remaining stages
61 for(ii=1; ii<=N; ii++)
62 {
63 nvc[0] += nu[ii];
64 for(jj=0; jj<nb[ii]; jj++)
65 {
66 if(idxb[ii][jj]<nu[ii]) // input constraint
67 {
68 nbc[0]++;
69 }
70 else // state constraint
71 {
72 ngc[0]++;
73 }
74 }
75 ngc[0] += ng[ii];
76 }
77
78 return;
79
80 }
81
82
83
84int d_memsize_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp) // XXX + args for algorithm type ???
85 {
86
87 int ii;
88
89 int N = ocp_qp->N;
90 int *nx = ocp_qp->nx;
91 int *nu = ocp_qp->nu;
92 int *nb = ocp_qp->nb;
93 int *ng = ocp_qp->ng;
94
95 // compute core qp size and max size
96 int nvt = 0;
97 int net = 0;
98 int nbt = 0;
99 int ngt = 0;
100 int nxM = 0;
101 int nuM = 0;
102 int nbM = 0;
103 int ngM = 0;
104
105 for(ii=0; ii<N; ii++)
106 {
107 nvt += nx[ii]+nu[ii];
108 net += nx[ii+1];
109 nbt += nb[ii];
110 ngt += ng[ii];
111 nxM = nx[ii]>nxM ? nx[ii] : nxM;
112 nuM = nu[ii]>nuM ? nu[ii] : nuM;
113 nbM = nb[ii]>nbM ? nb[ii] : nbM;
114 ngM = ng[ii]>ngM ? ng[ii] : ngM;
115 }
116 ii = N;
117 nvt += nx[ii]+nu[ii];
118 nbt += nb[ii];
119 ngt += ng[ii];
120 nxM = nx[ii]>nxM ? nx[ii] : nxM;
121 nuM = nu[ii]>nuM ? nu[ii] : nuM;
122 nbM = nb[ii]>nbM ? nb[ii] : nbM;
123 ngM = ng[ii]>ngM ? ng[ii] : ngM;
124
125 int size = 0;
126
127 size += (2+2*(N+1))*sizeof(struct d_strmat); // Gamma L Lx AL
128 size += (2+1*(N+1))*sizeof(struct d_strvec); // Gammab tmp_ngM tmp_nuxM
129
130 int nu_tmp = 0;
131 for(ii=0; ii<N; ii++)
132 {
133 nu_tmp += nu[ii];
134 size += d_size_strmat(nu_tmp+nx[0]+1, nx[ii+1]); // Gamma
135 }
136 for(ii=0; ii<=N; ii++)
137 size += d_size_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // L
138 size += d_size_strmat(nxM+1, nxM); // Lx
139 size += d_size_strmat(nuM+nxM+1, nxM); // AL
140 for(ii=0; ii<N; ii++)
141 size += 1*d_size_strvec(nx[ii+1]); // Gammab
142 size += d_size_strvec(ngM); // tmp_ngM
143 size += 1*d_size_strvec(nuM+nxM); // tmp_nuxM
144 size += 1*d_size_strvec(ngM); // tmp_ngM
145
146 size = (size+63)/64*64; // make multiple of typical cache line size
147 size += 1*64; // align once to typical cache line size
148
149 return size;
150
151 }
152
153
154
155void d_create_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws, void *mem)
156 {
157
158 int ii;
159
160 int N = ocp_qp->N;
161 int *nx = ocp_qp->nx;
162 int *nu = ocp_qp->nu;
163 int *nb = ocp_qp->nb;
164 int *ng = ocp_qp->ng;
165
166 // compute core qp size and max size
167 int nvt = 0;
168 int net = 0;
169 int nbt = 0;
170 int ngt = 0;
171 int nxM = 0;
172 int nuM = 0;
173 int nbM = 0;
174 int ngM = 0;
175
176 for(ii=0; ii<N; ii++)
177 {
178 nvt += nx[ii]+nu[ii];
179 net += nx[ii+1];
180 nbt += nb[ii];
181 ngt += ng[ii];
182 nxM = nx[ii]>nxM ? nx[ii] : nxM;
183 nuM = nu[ii]>nuM ? nu[ii] : nuM;
184 nbM = nb[ii]>nbM ? nb[ii] : nbM;
185 ngM = ng[ii]>ngM ? ng[ii] : ngM;
186 }
187 ii = N;
188 nvt += nx[ii]+nu[ii];
189 nbt += nb[ii];
190 ngt += ng[ii];
191 nxM = nx[ii]>nxM ? nx[ii] : nxM;
192 nuM = nu[ii]>nuM ? nu[ii] : nuM;
193 nbM = nb[ii]>nbM ? nb[ii] : nbM;
194 ngM = ng[ii]>ngM ? ng[ii] : ngM;
195
196
197 // matrix struct
198 struct d_strmat *sm_ptr = (struct d_strmat *) mem;
199
200 cond_ws->Gamma = sm_ptr;
201 sm_ptr += N+1;
202 cond_ws->L = sm_ptr;
203 sm_ptr += N+1;
204 cond_ws->Lx = sm_ptr;
205 sm_ptr += 1;
206 cond_ws->AL = sm_ptr;
207 sm_ptr += 1;
208
209
210 // vector struct
211 struct d_strvec *sv_ptr = (struct d_strvec *) sm_ptr;
212
213 cond_ws->Gammab = sv_ptr;
214 sv_ptr += N+1;
215 cond_ws->tmp_ngM = sv_ptr;
216 sv_ptr += 1;
217 cond_ws->tmp_nuxM = sv_ptr;
218 sv_ptr += 1;
219
220
221 // align to typicl cache line size
222 size_t s_ptr = (size_t) sv_ptr;
223 s_ptr = (s_ptr+63)/64*64;
224
225
226 // void stuf
227 char *c_ptr = (char *) s_ptr;
228 char *c_tmp;
229
230 int nu_tmp = 0;
231 for(ii=0; ii<N; ii++)
232 {
233 nu_tmp += nu[ii];
234 d_create_strmat(nu_tmp+nx[0]+1, nx[ii+1], cond_ws->Gamma+ii, c_ptr);
235 c_ptr += (cond_ws->Gamma+ii)->memory_size;
236 }
237 for(ii=0; ii<=N; ii++)
238 {
239 d_create_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], cond_ws->L+ii, c_ptr);
240 c_ptr += (cond_ws->L+ii)->memory_size;
241 }
242 d_create_strmat(nxM+1, nxM, cond_ws->Lx, c_ptr);
243 c_ptr += cond_ws->Lx->memory_size;
244 d_create_strmat(nuM+nxM+1, nxM, cond_ws->AL, c_ptr);
245 c_ptr += cond_ws->AL->memory_size;
246 for(ii=0; ii<N; ii++)
247 {
248 d_create_strvec(nx[ii+1], cond_ws->Gammab+ii, c_ptr);
249 c_ptr += (cond_ws->Gammab+ii)->memory_size;
250 }
251 d_create_strvec(ngM, cond_ws->tmp_ngM, c_ptr);
252 c_ptr += cond_ws->tmp_ngM->memory_size;
253 c_tmp = c_ptr;
254 d_create_strvec(nuM+nxM, cond_ws->tmp_nuxM, c_ptr);
255 c_ptr += cond_ws->tmp_nuxM->memory_size;
256
257 cond_ws->memsize = d_memsize_cond_qp_ocp2dense(ocp_qp, dense_qp);
258
259 return;
260
261 }
262
263
264
265void d_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws)
266 {
267
268 d_compute_Gamma(ocp_qp, cond_ws);
269
270 d_cond_RSQrq_N2nx3(ocp_qp, dense_qp->Hg, dense_qp->g, cond_ws);
271
272 d_cond_DCtd(ocp_qp, dense_qp->idxb, dense_qp->d_lb, dense_qp->d_ub, dense_qp->Ct, dense_qp->d_lg, dense_qp->d_ug, cond_ws);
273
274 return;
275
276 }
277
278
279
280void d_expand_sol_dense2ocp(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_ocp2dense_workspace *cond_ws)
281 {
282
283 d_expand_sol(ocp_qp, dense_qp_sol, ocp_qp_sol->ux, ocp_qp_sol->pi, ocp_qp_sol->lam_lb, ocp_qp_sol->lam_ub, ocp_qp_sol->lam_lg, ocp_qp_sol->lam_ug, ocp_qp_sol->t_lb, ocp_qp_sol->t_ub, ocp_qp_sol->t_lg, ocp_qp_sol->t_ug, cond_ws->tmp_nuxM, cond_ws->tmp_ngM);
284
285 return;
286
287 }