Squashed 'third_party/osqp/' content from commit 33454b3e23
Change-Id: I056df0582ca06664e86554c341a94c47ab932001
git-subtree-dir: third_party/osqp
git-subtree-split: 33454b3e236f1f44193bfbbb6b8c8e71f8f04e9a
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/lin_sys/direct/pardiso/CMakeLists.txt b/lin_sys/direct/pardiso/CMakeLists.txt
new file mode 100644
index 0000000..83a3f86
--- /dev/null
+++ b/lin_sys/direct/pardiso/CMakeLists.txt
@@ -0,0 +1,14 @@
+set(pardiso_includes
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ PARENT_SCOPE
+)
+
+# Set source files
+set(
+ pardiso_sources
+ ${CMAKE_CURRENT_SOURCE_DIR}/pardiso_interface.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/pardiso_interface.c
+ ${CMAKE_CURRENT_SOURCE_DIR}/pardiso_loader.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/pardiso_loader.c
+ PARENT_SCOPE
+)
diff --git a/lin_sys/direct/pardiso/pardiso_interface.c b/lin_sys/direct/pardiso/pardiso_interface.c
new file mode 100644
index 0000000..738334a
--- /dev/null
+++ b/lin_sys/direct/pardiso/pardiso_interface.c
@@ -0,0 +1,300 @@
+#include "pardiso_interface.h"
+
+#define MKL_INT c_int
+
+// Single Dynamic library interface
+#define MKL_INTERFACE_LP64 0x0
+#define MKL_INTERFACE_ILP64 0x1
+
+// Solver Phases
+#define PARDISO_SYMBOLIC (11)
+#define PARDISO_NUMERIC (22)
+#define PARDISO_SOLVE (33)
+#define PARDISO_CLEANUP (-1)
+
+
+// Prototypes for Pardiso functions
+void pardiso(void**, // pt
+ const c_int*, // maxfct
+ const c_int*, // mnum
+ const c_int*, // mtype
+ const c_int*, // phase
+ const c_int*, // n
+ const c_float*, // a
+ const c_int*, // ia
+ const c_int*, // ja
+ c_int*, // perm
+ const c_int*, //nrhs
+ c_int*, // iparam
+ const c_int*, //msglvl
+ c_float*, // b
+ c_float*, // x
+ c_int* // error
+ );
+c_int mkl_set_interface_layer(c_int);
+c_int mkl_get_max_threads();
+
+// Free LDL Factorization structure
+void free_linsys_solver_pardiso(pardiso_solver *s) {
+ if (s) {
+
+ // Free pardiso solver using internal function
+ s->phase = PARDISO_CLEANUP;
+ pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
+ &(s->nKKT), &(s->fdum), s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
+ s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
+
+ if ( s->error != 0 ){
+#ifdef PRINTING
+ c_eprint("Error during MKL Pardiso cleanup: %d", (int)s->error);
+#endif
+ }
+ // Check each attribute of the structure and free it if it exists
+ if (s->KKT) csc_spfree(s->KKT);
+ if (s->KKT_i) c_free(s->KKT_i);
+ if (s->KKT_p) c_free(s->KKT_p);
+ 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->PtoKKT) c_free(s->PtoKKT);
+ if (s->AtoKKT) c_free(s->AtoKKT);
+ if (s->rhotoKKT) c_free(s->rhotoKKT);
+
+ c_free(s);
+
+ }
+}
+
+
+// Initialize factorization structure
+c_int init_linsys_solver_pardiso(pardiso_solver ** sp, const csc * P, const csc * A, c_float sigma, const c_float * rho_vec, c_int polish){
+ c_int i; // loop counter
+ c_int nnzKKT; // Number of nonzeros in KKT
+ // Define Variables
+ c_int n_plus_m; // n_plus_m dimension
+
+
+ // Allocate private structure to store KKT factorization
+ pardiso_solver *s;
+ s = c_calloc(1, sizeof(pardiso_solver));
+ *sp = s;
+
+ // Size of KKT
+ s->n = P->n;
+ s->m = A->m;
+ n_plus_m = s->n + s->m;
+ s->nKKT = n_plus_m;
+
+ // Sigma parameter
+ s->sigma = sigma;
+
+ // Polishing flag
+ s->polish = polish;
+
+ // Link Functions
+ s->solve = &solve_linsys_pardiso;
+ s->free = &free_linsys_solver_pardiso;
+ s->update_matrices = &update_linsys_solver_matrices_pardiso;
+ s->update_rho_vec = &update_linsys_solver_rho_vec_pardiso;
+
+ // Assign type
+ s->type = MKL_PARDISO_SOLVER;
+
+ // Working vector
+ s->bp = (c_float *)c_malloc(sizeof(c_float) * n_plus_m);
+
+ // Solution vector
+ s->sol = (c_float *)c_malloc(sizeof(c_float) * n_plus_m);
+
+ // Parameter vector
+ s->rho_inv_vec = (c_float *)c_malloc(sizeof(c_float) * n_plus_m);
+
+ // Form KKT matrix
+ if (polish){ // Called from polish()
+ // Use s->rho_inv_vec for storing param2 = vec(delta)
+ for (i = 0; i < A->m; i++){
+ s->rho_inv_vec[i] = sigma;
+ }
+
+ s->KKT = form_KKT(P, A, 1, sigma, s->rho_inv_vec, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL, OSQP_NULL);
+ }
+ else { // Called from ADMM algorithm
+
+ // Allocate vectors of indices
+ s->PtoKKT = c_malloc((P->p[P->n]) * sizeof(c_int));
+ s->AtoKKT = c_malloc((A->p[A->n]) * sizeof(c_int));
+ s->rhotoKKT = c_malloc((A->m) * sizeof(c_int));
+
+ // Use s->rho_inv_vec for storing param2 = rho_inv_vec
+ for (i = 0; i < A->m; i++){
+ s->rho_inv_vec[i] = 1. / rho_vec[i];
+ }
+
+ s->KKT = form_KKT(P, A, 1, sigma, s->rho_inv_vec,
+ s->PtoKKT, s->AtoKKT,
+ &(s->Pdiag_idx), &(s->Pdiag_n), s->rhotoKKT);
+ }
+
+ // Check if matrix has been created
+ if (!(s->KKT)) {
+#ifdef PRINTING
+ c_eprint("Error in forming KKT matrix");
+#endif
+ free_linsys_solver_pardiso(s);
+ return OSQP_LINSYS_SOLVER_INIT_ERROR;
+ } else {
+ // Adjust indexing for Pardiso
+ nnzKKT = s->KKT->p[s->KKT->m];
+ s->KKT_i = c_malloc((nnzKKT) * sizeof(c_int));
+ s->KKT_p = c_malloc((s->KKT->m + 1) * sizeof(c_int));
+
+ for(i = 0; i < nnzKKT; i++){
+ s->KKT_i[i] = s->KKT->i[i] + 1;
+ }
+ for(i = 0; i < n_plus_m+1; i++){
+ s->KKT_p[i] = s->KKT->p[i] + 1;
+ }
+
+ }
+
+ // Set MKL interface layer (Long integers if activated)
+#ifdef DLONG
+ mkl_set_interface_layer(MKL_INTERFACE_ILP64);
+#else
+ mkl_set_interface_layer(MKL_INTERFACE_LP64);
+#endif
+
+ // Set Pardiso variables
+ s->mtype = -2; // Real symmetric indefinite matrix
+ s->nrhs = 1; // Number of right hand sides
+ s->maxfct = 1; // Maximum number of numerical factorizations
+ s->mnum = 1; // Which factorization to use
+ s->msglvl = 0; // Do not print statistical information
+ s->error = 0; // Initialize error flag
+ for ( i = 0; i < 64; i++ ) {
+ s->iparm[i] = 0; // Setup Pardiso control parameters
+ s->pt[i] = 0; // Initialize the internal solver memory pointer
+ }
+ s->iparm[0] = 1; // No solver default
+ s->iparm[1] = 3; // Fill-in reordering from OpenMP
+ if (polish) {
+ s->iparm[5] = 1; // Write solution into b
+ } else {
+ s->iparm[5] = 0; // Do NOT write solution into b
+ }
+ /* s->iparm[7] = 2; // Max number of iterative refinement steps */
+ s->iparm[7] = 0; // Number of iterative refinement steps (auto, performs them only if perturbed pivots are obtained)
+ s->iparm[9] = 13; // Perturb the pivot elements with 1E-13
+ s->iparm[34] = 0; // Use Fortran-style indexing for indices
+ /* s->iparm[34] = 1; // Use C-style indexing for indices */
+
+ // Print number of threads
+ s->nthreads = mkl_get_max_threads();
+
+ // Reordering and symbolic factorization
+ s->phase = PARDISO_SYMBOLIC;
+ pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
+ &(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
+ s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
+ if ( s->error != 0 ){
+#ifdef PRINTING
+ c_eprint("Error during symbolic factorization: %d", (int)s->error);
+#endif
+ free_linsys_solver_pardiso(s);
+ *sp = OSQP_NULL;
+ return OSQP_LINSYS_SOLVER_INIT_ERROR;
+ }
+
+ // Numerical factorization
+ s->phase = PARDISO_NUMERIC;
+ pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
+ &(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
+ s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
+ if ( s->error ){
+#ifdef PRINTING
+ c_eprint("Error during numerical factorization: %d", (int)s->error);
+#endif
+ free_linsys_solver_pardiso(s);
+ *sp = OSQP_NULL;
+ return OSQP_LINSYS_SOLVER_INIT_ERROR;
+ }
+
+
+ // No error
+ return 0;
+}
+
+// Returns solution to linear system Ax = b with solution stored in b
+c_int solve_linsys_pardiso(pardiso_solver * s, c_float * b) {
+ c_int j;
+
+ // Back substitution and iterative refinement
+ s->phase = PARDISO_SOLVE;
+ pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
+ &(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
+ s->iparm, &(s->msglvl), b, s->sol, &(s->error));
+ if ( s->error != 0 ){
+#ifdef PRINTING
+ c_eprint("Error during linear system solution: %d", (int)s->error);
+#endif
+ return 1;
+ }
+
+ if (!(s->polish)) {
+ /* copy x_tilde from s->sol */
+ for (j = 0 ; j < s->n ; j++) {
+ b[j] = s->sol[j];
+ }
+
+ /* compute z_tilde from b and s->sol */
+ for (j = 0 ; j < s->m ; j++) {
+ b[j + s->n] += s->rho_inv_vec[j] * s->sol[j + s->n];
+ }
+ }
+
+ return 0;
+}
+
+// Update solver structure with new P and A
+c_int update_linsys_solver_matrices_pardiso(pardiso_solver * s, const csc *P, const csc *A) {
+
+ // Update KKT matrix with new P
+ update_KKT_P(s->KKT, P, s->PtoKKT, s->sigma, s->Pdiag_idx, s->Pdiag_n);
+
+ // Update KKT matrix with new A
+ update_KKT_A(s->KKT, A, s->AtoKKT);
+
+ // Perform numerical factorization
+ s->phase = PARDISO_NUMERIC;
+ pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
+ &(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
+ s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
+
+ // Return exit flag
+ return s->error;
+}
+
+
+c_int update_linsys_solver_rho_vec_pardiso(pardiso_solver * s, const c_float * rho_vec) {
+ c_int i;
+
+ // 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);
+
+ // Perform numerical factorization
+ s->phase = PARDISO_NUMERIC;
+ pardiso (s->pt, &(s->maxfct), &(s->mnum), &(s->mtype), &(s->phase),
+ &(s->nKKT), s->KKT->x, s->KKT_p, s->KKT_i, &(s->idum), &(s->nrhs),
+ s->iparm, &(s->msglvl), &(s->fdum), &(s->fdum), &(s->error));
+
+ // Return exit flag
+ return s->error;
+}
diff --git a/lin_sys/direct/pardiso/pardiso_interface.h b/lin_sys/direct/pardiso/pardiso_interface.h
new file mode 100644
index 0000000..c0e334d
--- /dev/null
+++ b/lin_sys/direct/pardiso/pardiso_interface.h
@@ -0,0 +1,127 @@
+#ifndef PARDISO_H
+#define PARDISO_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "lin_alg.h"
+#include "kkt.h"
+
+/**
+ * Pardiso solver structure
+ *
+ * NB: If we use Pardiso, we suppose that EMBEDDED is not enabled
+ */
+typedef struct pardiso pardiso_solver;
+
+struct pardiso {
+ enum linsys_solver_type type;
+
+ /**
+ * @name Functions
+ * @{
+ */
+ c_int (*solve)(struct pardiso * self, c_float * b);
+
+ void (*free)(struct pardiso * self); ///< Free workspace (only if desktop)
+
+ c_int (*update_matrices)(struct pardiso * self, const csc *P, const csc *A); ///< Update solver matrices
+ c_int (*update_rho_vec)(struct pardiso * self, const c_float * rho_vec); ///< Update rho_vec parameter
+
+ c_int nthreads;
+ /** @} */
+
+
+ /**
+ * @name Attributes
+ * @{
+ */
+ // Attributes
+ csc *KKT; ///< KKT matrix (in CSR format!)
+ c_int *KKT_i; ///< KKT column indices in 1-indexing for Pardiso
+ c_int *KKT_p; ///< KKT row pointers in 1-indexing for Pardiso
+ c_float *bp; ///< workspace memory for solves (rhs)
+ c_float *sol; ///< solution to the KKT system
+ c_float *rho_inv_vec; ///< parameter vector
+ c_float sigma; ///< scalar parameter
+ c_int polish; ///< polishing flag
+ c_int n; ///< number of QP variables
+ c_int m; ///< number of QP constraints
+
+ // Pardiso variables
+ void *pt[64]; ///< internal solver memory pointer pt
+ c_int iparm[64]; ///< Pardiso control parameters
+ c_int nKKT; ///< dimension of the linear system
+ c_int mtype; ///< matrix type (-2 for real and symmetric indefinite)
+ c_int nrhs; ///< number of right-hand sides (1 for our needs)
+ c_int maxfct; ///< maximum number of factors (1 for our needs)
+ c_int mnum; ///< indicates matrix for the solution phase (1 for our needs)
+ c_int phase; ///< control the execution phases of the solver
+ c_int error; ///< the error indicator (0 for no error)
+ c_int msglvl; ///< Message level information (0 for no output)
+ c_int idum; ///< dummy integer
+ c_float fdum; ///< dummy float
+
+ // These are required for matrix updates
+ c_int * Pdiag_idx, Pdiag_n; ///< index and number of diagonal elements in P
+ c_int * PtoKKT, * AtoKKT; ///< Index of elements from P and A to KKT matrix
+ c_int * rhotoKKT; ///< Index of rho places in KKT matrix
+
+ /** @} */
+};
+
+
+/**
+ * Initialize Pardiso Solver
+ *
+ * @param s Pointer to a private structure
+ * @param P Cost function matrix (upper triangular form)
+ * @param A Constraints matrix
+ * @param sigma Algorithm parameter. If polish, then sigma = delta.
+ * @param rho_vec Algorithm parameter. If polish, then rho_vec = OSQP_NULL.
+ * @param polish Flag whether we are initializing for polish or not
+ * @return Exitflag for error (0 if no errors)
+ */
+c_int init_linsys_solver_pardiso(pardiso_solver ** sp, const csc * P, const csc * A, c_float sigma, const c_float * rho_vec, c_int polish);
+
+
+/**
+ * Solve linear system and store result in b
+ * @param s Linear system solver structure
+ * @param b Right-hand side
+ * @return Exitflag
+ */
+c_int solve_linsys_pardiso(pardiso_solver * s, c_float * b);
+
+
+/**
+ * Update linear system solver matrices
+ * @param s Linear system solver structure
+ * @param P Matrix P
+ * @param A Matrix A
+ * @return Exitflag
+ */
+c_int update_linsys_solver_matrices_pardiso(pardiso_solver * s, const csc *P, const csc *A);
+
+
+/**
+ * Update rho parameter in linear system solver structure
+ * @param s Linear system solver structure
+ * @param rho_vec new rho_vec value
+ * @return exitflag
+ */
+c_int update_linsys_solver_rho_vec_pardiso(pardiso_solver * s, const c_float * rho_vec);
+
+
+/**
+ * Free linear system solver
+ * @param s linear system solver object
+ */
+void free_linsys_solver_pardiso(pardiso_solver * s);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/lin_sys/direct/pardiso/pardiso_loader.c b/lin_sys/direct/pardiso/pardiso_loader.c
new file mode 100644
index 0000000..f120e47
--- /dev/null
+++ b/lin_sys/direct/pardiso/pardiso_loader.c
@@ -0,0 +1,102 @@
+#include "lib_handler.h"
+#include "pardiso_loader.h"
+
+#include "glob_opts.h"
+#include "constants.h"
+
+#ifdef IS_WINDOWS
+#define PARDISOLIBNAME "mkl_rt." SHAREDLIBEXT
+#else
+#define PARDISOLIBNAME "libmkl_rt." SHAREDLIBEXT
+#endif
+
+typedef void (*voidfun)(void);
+
+voidfun lh_load_sym (soHandle_t h, const char *symName);
+
+
+// Interfaces for Pardiso functions
+typedef void (*pardiso_t)(void**, const c_int*, const c_int*, const c_int*,
+ const c_int*, const c_int*, const c_float*,
+ const c_int*, const c_int*, c_int*,
+ const c_int*, c_int*, const c_int*, c_float*,
+ c_float*, c_int*);
+typedef int (*mkl_set_ifl_t)(int);
+typedef int (*mkl_get_mt_t)();
+
+
+// Handlers are static variables
+static soHandle_t Pardiso_handle = OSQP_NULL;
+static pardiso_t func_pardiso = OSQP_NULL;
+static mkl_set_ifl_t func_mkl_set_interface_layer = OSQP_NULL;
+static mkl_get_mt_t func_mkl_get_max_threads = OSQP_NULL;
+
+// Wrappers for loaded Pardiso function handlers
+void pardiso(void** pt, const c_int* maxfct, const c_int* mnum,
+ const c_int* mtype, const c_int* phase, const c_int* n,
+ const c_float* a, const c_int* ia, const c_int* ja,
+ c_int* perm, const c_int* nrhs, c_int* iparm,
+ const c_int* msglvl, c_float* b, c_float* x,
+ c_int* error) {
+ if(func_pardiso){
+ // Call function pardiso only if it has been initialized
+ func_pardiso(pt, maxfct, mnum, mtype, phase, n, a, ia, ja,
+ perm, nrhs, iparm, msglvl, b, x, error);
+ }
+ else
+ {
+#ifdef PRINTING
+ c_eprint("Pardiso not loaded correctly");
+#endif
+ }
+}
+
+c_int mkl_set_interface_layer(c_int code) {
+ return (c_int)func_mkl_set_interface_layer((int)code);
+}
+
+c_int mkl_get_max_threads() {
+ return (c_int)func_mkl_get_max_threads();
+}
+
+
+c_int lh_load_pardiso(const char* libname) {
+ // DEBUG
+ // if (Pardiso_handle) return 0;
+
+ // Load Pardiso library
+ if (libname) {
+ Pardiso_handle = lh_load_lib(libname);
+ } else { /* try a default library name */
+ Pardiso_handle = lh_load_lib(PARDISOLIBNAME);
+ }
+ if (!Pardiso_handle) return 1;
+
+ // Load Pardiso functions
+ func_pardiso = (pardiso_t)lh_load_sym(Pardiso_handle, "pardiso");
+ if (!func_pardiso) return 1;
+
+ func_mkl_set_interface_layer = (mkl_set_ifl_t)lh_load_sym(Pardiso_handle,
+ "MKL_Set_Interface_Layer");
+ if (!func_mkl_set_interface_layer) return 1;
+
+ func_mkl_get_max_threads = (mkl_get_mt_t)lh_load_sym(Pardiso_handle,
+ "MKL_Get_Max_Threads");
+ if (!func_mkl_get_max_threads) return 1;
+
+ return 0;
+}
+
+c_int lh_unload_pardiso() {
+
+ if (Pardiso_handle == OSQP_NULL) return 0;
+
+ return lh_unload_lib(Pardiso_handle);
+
+ /* If multiple OSQP objects are laoded, the lines below cause a crash */
+ // Pardiso_handle = OSQP_NULL;
+ // func_pardiso = OSQP_NULL;
+ // func_mkl_set_interface_layer = OSQP_NULL;
+ // func_mkl_get_max_threads = OSQP_NULL;
+
+}
diff --git a/lin_sys/direct/pardiso/pardiso_loader.h b/lin_sys/direct/pardiso/pardiso_loader.h
new file mode 100644
index 0000000..aa96b24
--- /dev/null
+++ b/lin_sys/direct/pardiso/pardiso_loader.h
@@ -0,0 +1,29 @@
+#ifndef PARDISOLOADER_H
+#define PARDISOLOADER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+/**
+ * Tries to load a shared library with Pardiso.
+ * Return a failure if the library cannot be loaded or not all Pardiso symbols are found.
+ * @param libname The name under which the Pardiso lib can be found, or OSQP_NULL to use a default name (mkl_rt.SHAREDLIBEXT).
+ * @return Zero on success, nonzero on failure.
+ */
+c_int lh_load_pardiso(const char* libname);
+
+/**
+ * Unloads the loaded Pardiso shared library.
+ * @return Zero on success, nonzero on failure.
+ */
+c_int lh_unload_pardiso();
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /*PARADISOLOADER_H*/