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