-
Notifications
You must be signed in to change notification settings - Fork 346
/
qdldl_interface.c
436 lines (344 loc) · 12.8 KB
/
qdldl_interface.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
#include "glob_opts.h"
#include "qdldl.h"
#include "qdldl_interface.h"
#ifndef EMBEDDED
#include "amd.h"
#endif
#if EMBEDDED != 1
#include "kkt.h"
#endif
#ifndef EMBEDDED
// Free LDL Factorization structure
void free_linsys_solver_qdldl(qdldl_solver *s) {
if (s) {
if (s->L) csc_spfree(s->L);
if (s->P) c_free(s->P);
if (s->Dinv) c_free(s->Dinv);
if (s->bp) c_free(s->bp);
if (s->sol) c_free(s->sol);
if (s->rho_inv_vec) c_free(s->rho_inv_vec);
// These are required for matrix updates
if (s->Pdiag_idx) c_free(s->Pdiag_idx);
if (s->KKT) csc_spfree(s->KKT);
if (s->PtoKKT) c_free(s->PtoKKT);
if (s->AtoKKT) c_free(s->AtoKKT);
if (s->rhotoKKT) c_free(s->rhotoKKT);
// QDLDL workspace
if (s->D) c_free(s->D);
if (s->etree) c_free(s->etree);
if (s->Lnz) c_free(s->Lnz);
if (s->iwork) c_free(s->iwork);
if (s->bwork) c_free(s->bwork);
if (s->fwork) c_free(s->fwork);
c_free(s);
}
}
/**
* Compute LDL factorization of matrix A
* @param A Matrix to be factorized
* @param p Private workspace
* @param nvar Number of QP variables
* @return exitstatus (0 is good)
*/
static c_int LDL_factor(csc *A, qdldl_solver * p, c_int nvar){
c_int sum_Lnz;
c_int factor_status;
// Compute elimination tree
sum_Lnz = QDLDL_etree(A->n, A->p, A->i, p->iwork, p->Lnz, p->etree);
if (sum_Lnz < 0){
// Error
#ifdef PRINTING
c_eprint("Error in KKT matrix LDL factorization when computing the elimination tree. A is not perfectly upper triangular");
#endif
return sum_Lnz;
}
// Allocate memory for Li and Lx
p->L->i = (c_int *)c_malloc(sizeof(c_int)*sum_Lnz);
p->L->x = (c_float *)c_malloc(sizeof(c_float)*sum_Lnz);
// Factor matrix
factor_status = QDLDL_factor(A->n, A->p, A->i, A->x,
p->L->p, p->L->i, p->L->x,
p->D, p->Dinv, p->Lnz,
p->etree, p->bwork, p->iwork, p->fwork);
if (factor_status < 0){
// Error
#ifdef PRINTING
c_eprint("Error in KKT matrix LDL factorization when computing the nonzero elements. There are zeros in the diagonal matrix");
#endif
return factor_status;
} else if (factor_status < nvar) {
// Error: Number of positive elements of D should be equal to nvar
#ifdef PRINTING
c_eprint("Error in KKT matrix LDL factorization when computing the nonzero elements. The problem seems to be non-convex");
#endif
return -2;
}
return 0;
}
static c_int permute_KKT(csc ** KKT, qdldl_solver * p, c_int Pnz, c_int Anz, c_int m, c_int * PtoKKT, c_int * AtoKKT, c_int * rhotoKKT){
c_float *info;
c_int amd_status;
c_int * Pinv;
csc *KKT_temp;
c_int * KtoPKPt;
c_int i; // Indexing
info = (c_float *)c_malloc(AMD_INFO * sizeof(c_float));
// Compute permutation matrix P using AMD
#ifdef DLONG
amd_status = amd_l_order((*KKT)->n, (*KKT)->p, (*KKT)->i, p->P, (c_float *)OSQP_NULL, info);
#else
amd_status = amd_order((*KKT)->n, (*KKT)->p, (*KKT)->i, p->P, (c_float *)OSQP_NULL, info);
#endif
if (amd_status < 0) return (amd_status);
// Inverse of the permutation vector
Pinv = csc_pinv(p->P, (*KKT)->n);
// Permute KKT matrix
if (!PtoKKT && !AtoKKT && !rhotoKKT){ // No vectors to be stored
// Assign values of mapping
KKT_temp = csc_symperm((*KKT), Pinv, OSQP_NULL, 1);
}
else {
// Allocate vector of mappings from unpermuted to permuted
KtoPKPt = c_malloc((*KKT)->p[(*KKT)->n] * sizeof(c_int));
KKT_temp = csc_symperm((*KKT), Pinv, KtoPKPt, 1);
// Update vectors PtoKKT, AtoKKT and rhotoKKT
if (PtoKKT){
for (i = 0; i < Pnz; i++){
PtoKKT[i] = KtoPKPt[PtoKKT[i]];
}
}
if (AtoKKT){
for (i = 0; i < Anz; i++){
AtoKKT[i] = KtoPKPt[AtoKKT[i]];
}
}
if (rhotoKKT){
for (i = 0; i < m; i++){
rhotoKKT[i] = KtoPKPt[rhotoKKT[i]];
}
}
// Cleanup vector of mapping
c_free(KtoPKPt);
}
// Cleanup
// Free previous KKT matrix and assign pointer to new one
csc_spfree((*KKT));
(*KKT) = KKT_temp;
// Free Pinv
c_free(Pinv);
// Free Amd info
c_free(info);
return 0;
}
// Initialize LDL Factorization structure
c_int init_linsys_solver_qdldl(qdldl_solver ** sp, const OSQPMatrix* P, const OSQPMatrix* A, c_float sigma, const OSQPVectorf* rho_vec, c_int polish){
// Define Variables
csc * KKT_temp; // Temporary KKT pointer
c_int i; // Loop counter
c_int n_plus_m; // Define n_plus_m dimension
c_float* rhov; //used for direct access to rho_vec data when polish=false
// Allocate private structure to store KKT factorization
qdldl_solver *s;
s = c_calloc(1, sizeof(qdldl_solver));
*sp = s;
// Size of KKT
s->n = OSQPMatrix_get_n(P);
s->m = OSQPMatrix_get_m(A);
n_plus_m = s->n + s->m;
// Sigma parameter
s->sigma = sigma;
// Polishing flag
s->polish = polish;
// Link Functions
s->solve = &solve_linsys_qdldl;
#ifndef EMBEDDED
s->free = &free_linsys_solver_qdldl;
#endif
#if EMBEDDED != 1
s->update_matrices = &update_linsys_solver_matrices_qdldl;
s->update_rho_vec = &update_linsys_solver_rho_vec_qdldl;
#endif
// Assign type
s->type = QDLDL_SOLVER;
// Set number of threads to 1 (single threaded)
s->nthreads = 1;
// Sparse matrix L (lower triangular)
// NB: We don not allocate L completely (CSC elements)
// L will be allocated during the factorization depending on the
// resulting number of elements.
s->L = c_malloc(sizeof(csc));
s->L->m = n_plus_m;
s->L->n = n_plus_m;
s->L->nz = -1;
// Diagonal matrix stored as a vector D
s->Dinv = (QDLDL_float *)c_malloc(sizeof(QDLDL_float) * n_plus_m);
s->D = (QDLDL_float *)c_malloc(sizeof(QDLDL_float) * n_plus_m);
// Permutation vector P
s->P = (QDLDL_int *)c_malloc(sizeof(QDLDL_int) * n_plus_m);
// Working vector
s->bp = (QDLDL_float *)c_malloc(sizeof(QDLDL_float) * n_plus_m);
// Solution vector
s->sol = (QDLDL_float *)c_malloc(sizeof(QDLDL_float) * n_plus_m);
// Parameter vector
s->rho_inv_vec = (c_float *)c_malloc(sizeof(c_float) * s->m);
// Elimination tree workspace
s->etree = (QDLDL_int *)c_malloc(n_plus_m * sizeof(QDLDL_int));
s->Lnz = (QDLDL_int *)c_malloc(n_plus_m * sizeof(QDLDL_int));
// Preallocate L matrix (Lx and Li are sparsity dependent)
s->L->p = (c_int *)c_malloc((n_plus_m+1) * sizeof(QDLDL_int));
// Lx and Li are sparsity dependent, so set them to
// null initially so we don't try to free them prematurely
s->L->i = OSQP_NULL;
s->L->x = OSQP_NULL;
// Preallocate workspace
s->iwork = (QDLDL_int *)c_malloc(sizeof(QDLDL_int)*(3*n_plus_m));
s->bwork = (QDLDL_bool *)c_malloc(sizeof(QDLDL_bool)*n_plus_m);
s->fwork = (QDLDL_float *)c_malloc(sizeof(QDLDL_float)*n_plus_m);
// Form and permute KKT matrix
if (polish){ // Called from polish()
// Use s->rho_inv_vec for storing param2 = vec(delta)
for (i = 0; i < s->m; i++){
s->rho_inv_vec[i] = sigma;
}
KKT_temp = form_KKT(OSQPMatrix_get_x(P),
OSQPMatrix_get_i(P),
OSQPMatrix_get_p(P),
OSQPMatrix_get_x(A),
OSQPMatrix_get_i(A),
OSQPMatrix_get_p(A),
OSQPMatrix_get_m(A),
OSQPMatrix_get_n(P),
0, sigma, s->rho_inv_vec, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL);
// Permute matrix
if (KKT_temp)
permute_KKT(&KKT_temp, s, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL);
}
else { // Called from ADMM algorithm
// Allocate vectors of indices
s->PtoKKT = c_malloc(OSQPMatrix_get_nz(P) * sizeof(c_int));
s->AtoKKT = c_malloc(OSQPMatrix_get_nz(A) * sizeof(c_int));
s->rhotoKKT = c_malloc(OSQPMatrix_get_m(A) * sizeof(c_int));
// Use p->rho_inv_vec for storing param2 = rho_inv_vec
rhov = OSQPVectorf_data(rho_vec);
for (i = 0; i < s->m; i++){
s->rho_inv_vec[i] = 1. / rhov[i];
}
KKT_temp = form_KKT(OSQPMatrix_get_x(P),
OSQPMatrix_get_i(P),
OSQPMatrix_get_p(P),
OSQPMatrix_get_x(A),
OSQPMatrix_get_i(A),
OSQPMatrix_get_p(A),
OSQPMatrix_get_m(A),
OSQPMatrix_get_n(P),
0, sigma, s->rho_inv_vec,
s->PtoKKT, s->AtoKKT,
&(s->Pdiag_idx), &(s->Pdiag_n), s->rhotoKKT);
// Permute matrix
if (KKT_temp)
permute_KKT(&KKT_temp, s, OSQPMatrix_get_nz(P), OSQPMatrix_get_nz(A), s->m, s->PtoKKT, s->AtoKKT, s->rhotoKKT);
}
// Check if matrix has been created
if (!KKT_temp){
#ifdef PRINTING
c_eprint("Error forming and permuting KKT matrix");
#endif
free_linsys_solver_qdldl(s);
*sp = OSQP_NULL;
return OSQP_LINSYS_SOLVER_INIT_ERROR;
}
// Factorize the KKT matrix
if (LDL_factor(KKT_temp, s, OSQPMatrix_get_n(P)) < 0) {
csc_spfree(KKT_temp);
free_linsys_solver_qdldl(s);
*sp = OSQP_NULL;
return OSQP_NONCVX_ERROR;
}
if (polish){ // If KKT passed, assign it to KKT_temp
// Polish, no need for KKT_temp
csc_spfree(KKT_temp);
}
else { // If not embedded option 1 copy pointer to KKT_temp. Do not free it.
s->KKT = KKT_temp;
}
// No error
return 0;
}
#endif // EMBEDDED
// Permute x = P*b using P
void permute_x(c_int n, c_float * x, const c_float * b, const c_int * P) {
c_int j;
for (j = 0 ; j < n ; j++) x[j] = b[P[j]];
}
// Permute x = P'*b using P
void permutet_x(c_int n, c_float * x, const c_float * b, const c_int * P) {
c_int j;
for (j = 0 ; j < n ; j++) x[P[j]] = b[j];
}
static void LDLSolve(c_float *x, c_float *b, const csc *L, const c_float *Dinv, const c_int *P, c_float *bp) {
/* solves P'LDL'P x = b for x */
permute_x(L->n, bp, b, P);
QDLDL_solve(L->n, L->p, L->i, L->x, Dinv, bp);
permutet_x(L->n, x, bp, P);
}
c_int solve_linsys_qdldl(qdldl_solver * s, OSQPVectorf* b) {
c_int j;
c_float* bv = OSQPVectorf_data(b);
#ifndef EMBEDDED
if (s->polish) {
/* stores solution to the KKT system in b */
LDLSolve(bv, bv, s->L, s->Dinv, s->P, s->bp);
} else {
#endif
/* stores solution to the KKT system in s->sol */
LDLSolve(s->sol, bv, s->L, s->Dinv, s->P, s->bp);
/* copy x_tilde from s->sol */
for (j = 0 ; j < s->n ; j++) {
bv[j] = s->sol[j];
}
/* compute z_tilde from b and s->sol */
for (j = 0 ; j < s->m ; j++) {
bv[j + s->n] += s->rho_inv_vec[j] * s->sol[j + s->n];
}
#ifndef EMBEDDED
}
#endif
return 0;
}
#if EMBEDDED != 1
// Update private structure with new P and A
c_int update_linsys_solver_matrices_qdldl(qdldl_solver * s, const OSQPMatrix *P, const OSQPMatrix *A) {
// Update KKT matrix with new P
update_KKT_P(s->KKT,
OSQPMatrix_get_x(P),
OSQPMatrix_get_i(P),
OSQPMatrix_get_p(P),
OSQPMatrix_get_m(A),
OSQPMatrix_get_n(P),
s->PtoKKT, s->sigma, s->Pdiag_idx, s->Pdiag_n);
// Update KKT matrix with new A
update_KKT_A(s->KKT,
OSQPMatrix_get_x(A),
OSQPMatrix_get_i(A),
OSQPMatrix_get_p(A),
OSQPMatrix_get_m(A),
OSQPMatrix_get_n(P),
s->AtoKKT);
return (QDLDL_factor(s->KKT->n, s->KKT->p, s->KKT->i, s->KKT->x,
s->L->p, s->L->i, s->L->x, s->D, s->Dinv, s->Lnz,
s->etree, s->bwork, s->iwork, s->fwork) < 0);
}
c_int update_linsys_solver_rho_vec_qdldl(qdldl_solver * s, const OSQPVectorf * rho){
c_int i;
c_float* rho_vec = OSQPVectorf_data(rho);
// Update internal rho_inv_vec
for (i = 0; i < s->m; i++){
s->rho_inv_vec[i] = 1. / rho_vec[i];
}
// Update KKT matrix with new rho_vec
update_KKT_param2(s->KKT, s->rho_inv_vec, s->rhotoKKT, s->m);
return (QDLDL_factor(s->KKT->n, s->KKT->p, s->KKT->i, s->KKT->x,
s->L->p, s->L->i, s->L->x, s->D, s->Dinv, s->Lnz,
s->etree, s->bwork, s->iwork, s->fwork) < 0);
}
#endif