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/src/CMakeLists.txt b/src/CMakeLists.txt
new file mode 100644
index 0000000..02bf7f1
--- /dev/null
+++ b/src/CMakeLists.txt
@@ -0,0 +1,47 @@
+# Add the OSQP sources
+set(
+ osqp_src
+ "${CMAKE_CURRENT_SOURCE_DIR}/auxil.c"
+ "${CMAKE_CURRENT_SOURCE_DIR}/error.c"
+ "${CMAKE_CURRENT_SOURCE_DIR}/lin_alg.c"
+ "${CMAKE_CURRENT_SOURCE_DIR}/osqp.c"
+ "${CMAKE_CURRENT_SOURCE_DIR}/proj.c"
+ "${CMAKE_CURRENT_SOURCE_DIR}/scaling.c"
+ "${CMAKE_CURRENT_SOURCE_DIR}/util.c"
+)
+
+# Add the KKT update only in normal mode and matrix-updating embedded mode (not mode 1)
+if (NOT (EMBEDDED EQUAL 1))
+ list(
+ APPEND
+ osqp_src
+ "${CMAKE_CURRENT_SOURCE_DIR}/kkt.c"
+ )
+endif()
+
+# Add more files that should only be in non-embedded code
+if (NOT DEFINED EMBEDDED)
+ list(
+ APPEND
+ osqp_src
+ "${CMAKE_CURRENT_SOURCE_DIR}/cs.c"
+ "${CMAKE_CURRENT_SOURCE_DIR}/polish.c"
+ "${CMAKE_CURRENT_SOURCE_DIR}/lin_sys.c"
+ )
+endif()
+
+# Add the ctrl-c handler if enabled
+if (CTRLC)
+ list(
+ APPEND
+ osqp_src
+ "${CMAKE_CURRENT_SOURCE_DIR}/ctrlc.c"
+ )
+endif()
+
+# Pass the source list up to the main CMakeLists scope
+set(
+ osqp_src
+ "${osqp_src}"
+ PARENT_SCOPE
+)
diff --git a/src/auxil.c b/src/auxil.c
new file mode 100644
index 0000000..89c7de7
--- /dev/null
+++ b/src/auxil.c
@@ -0,0 +1,1067 @@
+#include "osqp.h" // For OSQP rho update
+#include "auxil.h"
+#include "proj.h"
+#include "lin_alg.h"
+#include "constants.h"
+#include "scaling.h"
+#include "util.h"
+
+/***********************************************************
+* Auxiliary functions needed to compute ADMM iterations * *
+***********************************************************/
+#if EMBEDDED != 1
+c_float compute_rho_estimate(OSQPWorkspace *work) {
+ c_int n, m; // Dimensions
+ c_float pri_res, dua_res; // Primal and dual residuals
+ c_float pri_res_norm, dua_res_norm; // Normalization for the residuals
+ c_float temp_res_norm; // Temporary residual norm
+ c_float rho_estimate; // Rho estimate value
+
+ // Get problem dimensions
+ n = work->data->n;
+ m = work->data->m;
+
+ // Get primal and dual residuals
+ pri_res = vec_norm_inf(work->z_prev, m);
+ dua_res = vec_norm_inf(work->x_prev, n);
+
+ // Normalize primal residual
+ pri_res_norm = vec_norm_inf(work->z, m); // ||z||
+ temp_res_norm = vec_norm_inf(work->Ax, m); // ||Ax||
+ pri_res_norm = c_max(pri_res_norm, temp_res_norm); // max (||z||,||Ax||)
+ pri_res /= (pri_res_norm + OSQP_DIVISION_TOL); // Normalize primal
+ // residual (prevent 0
+ // division)
+
+ // Normalize dual residual
+ dua_res_norm = vec_norm_inf(work->data->q, n); // ||q||
+ temp_res_norm = vec_norm_inf(work->Aty, n); // ||A' y||
+ dua_res_norm = c_max(dua_res_norm, temp_res_norm);
+ temp_res_norm = vec_norm_inf(work->Px, n); // ||P x||
+ dua_res_norm = c_max(dua_res_norm, temp_res_norm); // max(||q||,||A' y||,||P
+ // x||)
+ dua_res /= (dua_res_norm + OSQP_DIVISION_TOL); // Normalize dual residual
+ // (prevent 0 division)
+
+
+ // Return rho estimate
+ rho_estimate = work->settings->rho * c_sqrt(pri_res / dua_res);
+ rho_estimate = c_min(c_max(rho_estimate, RHO_MIN), RHO_MAX); // Constrain
+ // rho values
+ return rho_estimate;
+}
+
+c_int adapt_rho(OSQPWorkspace *work) {
+ c_int exitflag; // Exitflag
+ c_float rho_new; // New rho value
+
+ exitflag = 0; // Initialize exitflag to 0
+
+ // Compute new rho
+ rho_new = compute_rho_estimate(work);
+
+ // Set rho estimate in info
+ work->info->rho_estimate = rho_new;
+
+ // Check if the new rho is large or small enough and update it in case
+ if ((rho_new > work->settings->rho * work->settings->adaptive_rho_tolerance) ||
+ (rho_new < work->settings->rho / work->settings->adaptive_rho_tolerance)) {
+ exitflag = osqp_update_rho(work, rho_new);
+ work->info->rho_updates += 1;
+ }
+
+ return exitflag;
+}
+
+void set_rho_vec(OSQPWorkspace *work) {
+ c_int i;
+
+ work->settings->rho = c_min(c_max(work->settings->rho, RHO_MIN), RHO_MAX);
+
+ for (i = 0; i < work->data->m; i++) {
+ if ((work->data->l[i] < -OSQP_INFTY * MIN_SCALING) &&
+ (work->data->u[i] > OSQP_INFTY * MIN_SCALING)) {
+ // Loose bounds
+ work->constr_type[i] = -1;
+ work->rho_vec[i] = RHO_MIN;
+ } else if (work->data->u[i] - work->data->l[i] < RHO_TOL) {
+ // Equality constraints
+ work->constr_type[i] = 1;
+ work->rho_vec[i] = RHO_EQ_OVER_RHO_INEQ * work->settings->rho;
+ } else {
+ // Inequality constraints
+ work->constr_type[i] = 0;
+ work->rho_vec[i] = work->settings->rho;
+ }
+ work->rho_inv_vec[i] = 1. / work->rho_vec[i];
+ }
+}
+
+c_int update_rho_vec(OSQPWorkspace *work) {
+ c_int i, exitflag, constr_type_changed;
+
+ exitflag = 0;
+ constr_type_changed = 0;
+
+ for (i = 0; i < work->data->m; i++) {
+ if ((work->data->l[i] < -OSQP_INFTY * MIN_SCALING) &&
+ (work->data->u[i] > OSQP_INFTY * MIN_SCALING)) {
+ // Loose bounds
+ if (work->constr_type[i] != -1) {
+ work->constr_type[i] = -1;
+ work->rho_vec[i] = RHO_MIN;
+ work->rho_inv_vec[i] = 1. / RHO_MIN;
+ constr_type_changed = 1;
+ }
+ } else if (work->data->u[i] - work->data->l[i] < RHO_TOL) {
+ // Equality constraints
+ if (work->constr_type[i] != 1) {
+ work->constr_type[i] = 1;
+ work->rho_vec[i] = RHO_EQ_OVER_RHO_INEQ * work->settings->rho;
+ work->rho_inv_vec[i] = 1. / work->rho_vec[i];
+ constr_type_changed = 1;
+ }
+ } else {
+ // Inequality constraints
+ if (work->constr_type[i] != 0) {
+ work->constr_type[i] = 0;
+ work->rho_vec[i] = work->settings->rho;
+ work->rho_inv_vec[i] = 1. / work->settings->rho;
+ constr_type_changed = 1;
+ }
+ }
+ }
+
+ // Update rho_vec in KKT matrix if constraints type has changed
+ if (constr_type_changed == 1) {
+ exitflag = work->linsys_solver->update_rho_vec(work->linsys_solver,
+ work->rho_vec);
+ }
+
+ return exitflag;
+}
+
+#endif // EMBEDDED != 1
+
+
+void swap_vectors(c_float **a, c_float **b) {
+ c_float *temp;
+
+ temp = *b;
+ *b = *a;
+ *a = temp;
+}
+
+void cold_start(OSQPWorkspace *work) {
+ vec_set_scalar(work->x, 0., work->data->n);
+ vec_set_scalar(work->z, 0., work->data->m);
+ vec_set_scalar(work->y, 0., work->data->m);
+}
+
+static void compute_rhs(OSQPWorkspace *work) {
+ c_int i; // Index
+
+ for (i = 0; i < work->data->n; i++) {
+ // Cycle over part related to x variables
+ work->xz_tilde[i] = work->settings->sigma * work->x_prev[i] -
+ work->data->q[i];
+ }
+
+ for (i = 0; i < work->data->m; i++) {
+ // Cycle over dual variable in the first step (nu)
+ work->xz_tilde[i + work->data->n] = work->z_prev[i] - work->rho_inv_vec[i] *
+ work->y[i];
+ }
+}
+
+void update_xz_tilde(OSQPWorkspace *work) {
+ // Compute right-hand side
+ compute_rhs(work);
+
+ // Solve linear system
+ work->linsys_solver->solve(work->linsys_solver, work->xz_tilde);
+}
+
+void update_x(OSQPWorkspace *work) {
+ c_int i;
+
+ // update x
+ for (i = 0; i < work->data->n; i++) {
+ work->x[i] = work->settings->alpha * work->xz_tilde[i] +
+ ((c_float)1.0 - work->settings->alpha) * work->x_prev[i];
+ }
+
+ // update delta_x
+ for (i = 0; i < work->data->n; i++) {
+ work->delta_x[i] = work->x[i] - work->x_prev[i];
+ }
+}
+
+void update_z(OSQPWorkspace *work) {
+ c_int i;
+
+ // update z
+ for (i = 0; i < work->data->m; i++) {
+ work->z[i] = work->settings->alpha * work->xz_tilde[i + work->data->n] +
+ ((c_float)1.0 - work->settings->alpha) * work->z_prev[i] +
+ work->rho_inv_vec[i] * work->y[i];
+ }
+
+ // project z
+ project(work, work->z);
+}
+
+void update_y(OSQPWorkspace *work) {
+ c_int i; // Index
+
+ for (i = 0; i < work->data->m; i++) {
+ work->delta_y[i] = work->rho_vec[i] *
+ (work->settings->alpha *
+ work->xz_tilde[i + work->data->n] +
+ ((c_float)1.0 - work->settings->alpha) * work->z_prev[i] -
+ work->z[i]);
+ work->y[i] += work->delta_y[i];
+ }
+}
+
+c_float compute_obj_val(OSQPWorkspace *work, c_float *x) {
+ c_float obj_val;
+
+ obj_val = quad_form(work->data->P, x) +
+ vec_prod(work->data->q, x, work->data->n);
+
+ if (work->settings->scaling) {
+ obj_val *= work->scaling->cinv;
+ }
+
+ return obj_val;
+}
+
+c_float compute_pri_res(OSQPWorkspace *work, c_float *x, c_float *z) {
+ // NB: Use z_prev as working vector
+ // pr = Ax - z
+
+ mat_vec(work->data->A, x, work->Ax, 0); // Ax
+ vec_add_scaled(work->z_prev, work->Ax, z, work->data->m, -1);
+
+ // If scaling active -> rescale residual
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ return vec_scaled_norm_inf(work->scaling->Einv, work->z_prev, work->data->m);
+ }
+
+ // Return norm of the residual
+ return vec_norm_inf(work->z_prev, work->data->m);
+}
+
+c_float compute_pri_tol(OSQPWorkspace *work, c_float eps_abs, c_float eps_rel) {
+ c_float max_rel_eps, temp_rel_eps;
+
+ // max_rel_eps = max(||z||, ||A x||)
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ // ||Einv * z||
+ max_rel_eps =
+ vec_scaled_norm_inf(work->scaling->Einv, work->z, work->data->m);
+
+ // ||Einv * A * x||
+ temp_rel_eps = vec_scaled_norm_inf(work->scaling->Einv,
+ work->Ax,
+ work->data->m);
+
+ // Choose maximum
+ max_rel_eps = c_max(max_rel_eps, temp_rel_eps);
+ } else { // No unscaling required
+ // ||z||
+ max_rel_eps = vec_norm_inf(work->z, work->data->m);
+
+ // ||A * x||
+ temp_rel_eps = vec_norm_inf(work->Ax, work->data->m);
+
+ // Choose maximum
+ max_rel_eps = c_max(max_rel_eps, temp_rel_eps);
+ }
+
+ // eps_prim
+ return eps_abs + eps_rel * max_rel_eps;
+}
+
+c_float compute_dua_res(OSQPWorkspace *work, c_float *x, c_float *y) {
+ // NB: Use x_prev as temporary vector
+ // NB: Only upper triangular part of P is stored.
+ // dr = q + A'*y + P*x
+
+ // dr = q
+ prea_vec_copy(work->data->q, work->x_prev, work->data->n);
+
+ // P * x (upper triangular part)
+ mat_vec(work->data->P, x, work->Px, 0);
+
+ // P' * x (lower triangular part with no diagonal)
+ mat_tpose_vec(work->data->P, x, work->Px, 1, 1);
+
+ // dr += P * x (full P matrix)
+ vec_add_scaled(work->x_prev, work->x_prev, work->Px, work->data->n, 1);
+
+ // dr += A' * y
+ if (work->data->m > 0) {
+ mat_tpose_vec(work->data->A, y, work->Aty, 0, 0);
+ vec_add_scaled(work->x_prev, work->x_prev, work->Aty, work->data->n, 1);
+ }
+
+ // If scaling active -> rescale residual
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ return work->scaling->cinv * vec_scaled_norm_inf(work->scaling->Dinv,
+ work->x_prev,
+ work->data->n);
+ }
+
+ return vec_norm_inf(work->x_prev, work->data->n);
+}
+
+c_float compute_dua_tol(OSQPWorkspace *work, c_float eps_abs, c_float eps_rel) {
+ c_float max_rel_eps, temp_rel_eps;
+
+ // max_rel_eps = max(||q||, ||A' y|, ||P x||)
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ // || Dinv q||
+ max_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv,
+ work->data->q,
+ work->data->n);
+
+ // || Dinv A' y ||
+ temp_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv,
+ work->Aty,
+ work->data->n);
+ max_rel_eps = c_max(max_rel_eps, temp_rel_eps);
+
+ // || Dinv P x||
+ temp_rel_eps = vec_scaled_norm_inf(work->scaling->Dinv,
+ work->Px,
+ work->data->n);
+ max_rel_eps = c_max(max_rel_eps, temp_rel_eps);
+
+ // Multiply by cinv
+ max_rel_eps *= work->scaling->cinv;
+ } else { // No scaling required
+ // ||q||
+ max_rel_eps = vec_norm_inf(work->data->q, work->data->n);
+
+ // ||A'*y||
+ temp_rel_eps = vec_norm_inf(work->Aty, work->data->n);
+ max_rel_eps = c_max(max_rel_eps, temp_rel_eps);
+
+ // ||P*x||
+ temp_rel_eps = vec_norm_inf(work->Px, work->data->n);
+ max_rel_eps = c_max(max_rel_eps, temp_rel_eps);
+ }
+
+ // eps_dual
+ return eps_abs + eps_rel * max_rel_eps;
+}
+
+c_int is_primal_infeasible(OSQPWorkspace *work, c_float eps_prim_inf) {
+ // This function checks for the primal infeasibility termination criteria.
+ //
+ // 1) A' * delta_y < eps * ||delta_y||
+ //
+ // 2) u'*max(delta_y, 0) + l'*min(delta_y, 0) < eps * ||delta_y||
+ //
+
+ c_int i; // Index for loops
+ c_float norm_delta_y;
+ c_float ineq_lhs = 0.0;
+
+ // Project delta_y onto the polar of the recession cone of [l,u]
+ for (i = 0; i < work->data->m; i++) {
+ if (work->data->u[i] > OSQP_INFTY * MIN_SCALING) { // Infinite upper bound
+ if (work->data->l[i] < -OSQP_INFTY * MIN_SCALING) { // Infinite lower bound
+ // Both bounds infinite
+ work->delta_y[i] = 0.0;
+ } else {
+ // Only upper bound infinite
+ work->delta_y[i] = c_min(work->delta_y[i], 0.0);
+ }
+ } else if (work->data->l[i] < -OSQP_INFTY * MIN_SCALING) { // Infinite lower bound
+ // Only lower bound infinite
+ work->delta_y[i] = c_max(work->delta_y[i], 0.0);
+ }
+ }
+
+ // Compute infinity norm of delta_y (unscale if necessary)
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ // Use work->Adelta_x as temporary vector
+ vec_ew_prod(work->scaling->E, work->delta_y, work->Adelta_x, work->data->m);
+ norm_delta_y = vec_norm_inf(work->Adelta_x, work->data->m);
+ } else {
+ norm_delta_y = vec_norm_inf(work->delta_y, work->data->m);
+ }
+
+ if (norm_delta_y > OSQP_DIVISION_TOL) {
+
+ for (i = 0; i < work->data->m; i++) {
+ ineq_lhs += work->data->u[i] * c_max(work->delta_y[i], 0) + \
+ work->data->l[i] * c_min(work->delta_y[i], 0);
+ }
+
+ // Check if the condition is satisfied: ineq_lhs < -eps
+ if (ineq_lhs < eps_prim_inf * norm_delta_y) {
+ // Compute and return ||A'delta_y|| < eps_prim_inf
+ mat_tpose_vec(work->data->A, work->delta_y, work->Atdelta_y, 0, 0);
+
+ // Unscale if necessary
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ vec_ew_prod(work->scaling->Dinv,
+ work->Atdelta_y,
+ work->Atdelta_y,
+ work->data->n);
+ }
+
+ return vec_norm_inf(work->Atdelta_y, work->data->n) < eps_prim_inf * norm_delta_y;
+ }
+ }
+
+ // Conditions not satisfied -> not primal infeasible
+ return 0;
+}
+
+c_int is_dual_infeasible(OSQPWorkspace *work, c_float eps_dual_inf) {
+ // This function checks for the scaled dual infeasibility termination
+ // criteria.
+ //
+ // 1) q * delta_x < eps * || delta_x ||
+ //
+ // 2) ||P * delta_x || < eps * || delta_x ||
+ //
+ // 3) -> (A * delta_x)_i > -eps * || delta_x ||, l_i != -inf
+ // -> (A * delta_x)_i < eps * || delta_x ||, u_i != inf
+ //
+
+
+ c_int i; // Index for loops
+ c_float norm_delta_x;
+ c_float cost_scaling;
+
+ // Compute norm of delta_x
+ if (work->settings->scaling && !work->settings->scaled_termination) { // Unscale
+ // if
+ // necessary
+ norm_delta_x = vec_scaled_norm_inf(work->scaling->D,
+ work->delta_x,
+ work->data->n);
+ cost_scaling = work->scaling->c;
+ } else {
+ norm_delta_x = vec_norm_inf(work->delta_x, work->data->n);
+ cost_scaling = 1.0;
+ }
+
+ // Prevent 0 division || delta_x || > 0
+ if (norm_delta_x > OSQP_DIVISION_TOL) {
+ // Normalize delta_x by its norm
+
+ /* vec_mult_scalar(work->delta_x, 1./norm_delta_x, work->data->n); */
+
+ // Check first if q'*delta_x < 0
+ if (vec_prod(work->data->q, work->delta_x, work->data->n) <
+ cost_scaling * eps_dual_inf * norm_delta_x) {
+ // Compute product P * delta_x (NB: P is store in upper triangular form)
+ mat_vec(work->data->P, work->delta_x, work->Pdelta_x, 0);
+ mat_tpose_vec(work->data->P, work->delta_x, work->Pdelta_x, 1, 1);
+
+ // Scale if necessary
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ vec_ew_prod(work->scaling->Dinv,
+ work->Pdelta_x,
+ work->Pdelta_x,
+ work->data->n);
+ }
+
+ // Check if || P * delta_x || = 0
+ if (vec_norm_inf(work->Pdelta_x, work->data->n) <
+ cost_scaling * eps_dual_inf * norm_delta_x) {
+ // Compute A * delta_x
+ mat_vec(work->data->A, work->delta_x, work->Adelta_x, 0);
+
+ // Scale if necessary
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ vec_ew_prod(work->scaling->Einv,
+ work->Adelta_x,
+ work->Adelta_x,
+ work->data->m);
+ }
+
+ // De Morgan Law Applied to dual infeasibility conditions for A * x
+ // NB: Note that MIN_SCALING is used to adjust the infinity value
+ // in case the problem is scaled.
+ for (i = 0; i < work->data->m; i++) {
+ if (((work->data->u[i] < OSQP_INFTY * MIN_SCALING) &&
+ (work->Adelta_x[i] > eps_dual_inf * norm_delta_x)) ||
+ ((work->data->l[i] > -OSQP_INFTY * MIN_SCALING) &&
+ (work->Adelta_x[i] < -eps_dual_inf * norm_delta_x))) {
+ // At least one condition not satisfied -> not dual infeasible
+ return 0;
+ }
+ }
+
+ // All conditions passed -> dual infeasible
+ return 1;
+ }
+ }
+ }
+
+ // Conditions not satisfied -> not dual infeasible
+ return 0;
+}
+
+c_int has_solution(OSQPInfo * info){
+
+ return ((info->status_val != OSQP_PRIMAL_INFEASIBLE) &&
+ (info->status_val != OSQP_PRIMAL_INFEASIBLE_INACCURATE) &&
+ (info->status_val != OSQP_DUAL_INFEASIBLE) &&
+ (info->status_val != OSQP_DUAL_INFEASIBLE_INACCURATE) &&
+ (info->status_val != OSQP_NON_CVX));
+
+}
+
+void store_solution(OSQPWorkspace *work) {
+#ifndef EMBEDDED
+ c_float norm_vec;
+#endif /* ifndef EMBEDDED */
+
+ if (has_solution(work->info)) {
+ prea_vec_copy(work->x, work->solution->x, work->data->n); // primal
+ prea_vec_copy(work->y, work->solution->y, work->data->m); // dual
+
+ // Unscale solution if scaling has been performed
+ if (work->settings->scaling)
+ unscale_solution(work);
+ } else {
+ // No solution present. Solution is NaN
+ vec_set_scalar(work->solution->x, OSQP_NAN, work->data->n);
+ vec_set_scalar(work->solution->y, OSQP_NAN, work->data->m);
+
+#ifndef EMBEDDED
+
+ // Normalize infeasibility certificates if embedded is off
+ // NB: It requires a division
+ if ((work->info->status_val == OSQP_PRIMAL_INFEASIBLE) ||
+ ((work->info->status_val == OSQP_PRIMAL_INFEASIBLE_INACCURATE))) {
+ norm_vec = vec_norm_inf(work->delta_y, work->data->m);
+ vec_mult_scalar(work->delta_y, 1. / norm_vec, work->data->m);
+ }
+
+ if ((work->info->status_val == OSQP_DUAL_INFEASIBLE) ||
+ ((work->info->status_val == OSQP_DUAL_INFEASIBLE_INACCURATE))) {
+ norm_vec = vec_norm_inf(work->delta_x, work->data->n);
+ vec_mult_scalar(work->delta_x, 1. / norm_vec, work->data->n);
+ }
+
+#endif /* ifndef EMBEDDED */
+
+ // Cold start iterates to 0 for next runs (they cannot start from NaN)
+ cold_start(work);
+ }
+}
+
+void update_info(OSQPWorkspace *work,
+ c_int iter,
+ c_int compute_objective,
+ c_int polish) {
+ c_float *x, *z, *y; // Allocate pointers to variables
+ c_float *obj_val, *pri_res, *dua_res; // objective value, residuals
+
+#ifdef PROFILING
+ c_float *run_time; // Execution time
+#endif /* ifdef PROFILING */
+
+#ifndef EMBEDDED
+
+ if (polish) {
+ x = work->pol->x;
+ y = work->pol->y;
+ z = work->pol->z;
+ obj_val = &work->pol->obj_val;
+ pri_res = &work->pol->pri_res;
+ dua_res = &work->pol->dua_res;
+# ifdef PROFILING
+ run_time = &work->info->polish_time;
+# endif /* ifdef PROFILING */
+ } else {
+#endif // EMBEDDED
+ x = work->x;
+ y = work->y;
+ z = work->z;
+ obj_val = &work->info->obj_val;
+ pri_res = &work->info->pri_res;
+ dua_res = &work->info->dua_res;
+ work->info->iter = iter; // Update iteration number
+#ifdef PROFILING
+ run_time = &work->info->solve_time;
+#endif /* ifdef PROFILING */
+#ifndef EMBEDDED
+}
+
+#endif /* ifndef EMBEDDED */
+
+
+ // Compute the objective if needed
+ if (compute_objective) {
+ *obj_val = compute_obj_val(work, x);
+ }
+
+ // Compute primal residual
+ if (work->data->m == 0) {
+ // No constraints -> Always primal feasible
+ *pri_res = 0.;
+ } else {
+ *pri_res = compute_pri_res(work, x, z);
+ }
+
+ // Compute dual residual
+ *dua_res = compute_dua_res(work, x, y);
+
+ // Update timing
+#ifdef PROFILING
+ *run_time = osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+#ifdef PRINTING
+ work->summary_printed = 0; // The just updated info have not been printed
+#endif /* ifdef PRINTING */
+}
+
+
+void reset_info(OSQPInfo *info) {
+#ifdef PROFILING
+
+ // Initialize info values.
+ info->solve_time = 0.0; // Solve time to zero
+# ifndef EMBEDDED
+ info->polish_time = 0.0; // Polish time to zero
+# endif /* ifndef EMBEDDED */
+
+ // NB: We do not reset the setup_time because it is performed only once
+#endif /* ifdef PROFILING */
+
+ update_status(info, OSQP_UNSOLVED); // Problem is unsolved
+
+#if EMBEDDED != 1
+ info->rho_updates = 0; // Rho updates are now 0
+#endif /* if EMBEDDED != 1 */
+}
+
+void update_status(OSQPInfo *info, c_int status_val) {
+ // Update status value
+ info->status_val = status_val;
+
+ // Update status string depending on status val
+ if (status_val == OSQP_SOLVED) c_strcpy(info->status, "solved");
+
+ if (status_val == OSQP_SOLVED_INACCURATE) c_strcpy(info->status,
+ "solved inaccurate");
+ else if (status_val == OSQP_PRIMAL_INFEASIBLE) c_strcpy(info->status,
+ "primal infeasible");
+ else if (status_val == OSQP_PRIMAL_INFEASIBLE_INACCURATE) c_strcpy(info->status,
+ "primal infeasible inaccurate");
+ else if (status_val == OSQP_UNSOLVED) c_strcpy(info->status, "unsolved");
+ else if (status_val == OSQP_DUAL_INFEASIBLE) c_strcpy(info->status,
+ "dual infeasible");
+ else if (status_val == OSQP_DUAL_INFEASIBLE_INACCURATE) c_strcpy(info->status,
+ "dual infeasible inaccurate");
+ else if (status_val == OSQP_MAX_ITER_REACHED) c_strcpy(info->status,
+ "maximum iterations reached");
+#ifdef PROFILING
+ else if (status_val == OSQP_TIME_LIMIT_REACHED) c_strcpy(info->status,
+ "run time limit reached");
+#endif /* ifdef PROFILING */
+ else if (status_val == OSQP_SIGINT) c_strcpy(info->status, "interrupted");
+
+ else if (status_val == OSQP_NON_CVX) c_strcpy(info->status, "problem non convex");
+
+}
+
+c_int check_termination(OSQPWorkspace *work, c_int approximate) {
+ c_float eps_prim, eps_dual, eps_prim_inf, eps_dual_inf;
+ c_int exitflag;
+ c_int prim_res_check, dual_res_check, prim_inf_check, dual_inf_check;
+ c_float eps_abs, eps_rel;
+
+ // Initialize variables to 0
+ exitflag = 0;
+ prim_res_check = 0; dual_res_check = 0;
+ prim_inf_check = 0; dual_inf_check = 0;
+
+ // Initialize tolerances
+ eps_abs = work->settings->eps_abs;
+ eps_rel = work->settings->eps_rel;
+ eps_prim_inf = work->settings->eps_prim_inf;
+ eps_dual_inf = work->settings->eps_dual_inf;
+
+ // If residuals are too large, the problem is probably non convex
+ if ((work->info->pri_res > OSQP_INFTY) ||
+ (work->info->dua_res > OSQP_INFTY)){
+ // Looks like residuals are diverging. Probably the problem is non convex!
+ // Terminate and report it
+ update_status(work->info, OSQP_NON_CVX);
+ work->info->obj_val = OSQP_NAN;
+ return 1;
+ }
+
+ // If approximate solution required, increase tolerances by 10
+ if (approximate) {
+ eps_abs *= 10;
+ eps_rel *= 10;
+ eps_prim_inf *= 10;
+ eps_dual_inf *= 10;
+ }
+
+ // Check residuals
+ if (work->data->m == 0) {
+ prim_res_check = 1; // No constraints -> Primal feasibility always satisfied
+ }
+ else {
+ // Compute primal tolerance
+ eps_prim = compute_pri_tol(work, eps_abs, eps_rel);
+
+ // Primal feasibility check
+ if (work->info->pri_res < eps_prim) {
+ prim_res_check = 1;
+ } else {
+ // Primal infeasibility check
+ prim_inf_check = is_primal_infeasible(work, eps_prim_inf);
+ }
+ } // End check if m == 0
+
+ // Compute dual tolerance
+ eps_dual = compute_dua_tol(work, eps_abs, eps_rel);
+
+ // Dual feasibility check
+ if (work->info->dua_res < eps_dual) {
+ dual_res_check = 1;
+ } else {
+ // Check dual infeasibility
+ dual_inf_check = is_dual_infeasible(work, eps_dual_inf);
+ }
+
+ // Compare checks to determine solver status
+ if (prim_res_check && dual_res_check) {
+ // Update final information
+ if (approximate) {
+ update_status(work->info, OSQP_SOLVED_INACCURATE);
+ } else {
+ update_status(work->info, OSQP_SOLVED);
+ }
+ exitflag = 1;
+ }
+ else if (prim_inf_check) {
+ // Update final information
+ if (approximate) {
+ update_status(work->info, OSQP_PRIMAL_INFEASIBLE_INACCURATE);
+ } else {
+ update_status(work->info, OSQP_PRIMAL_INFEASIBLE);
+ }
+
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ // Update infeasibility certificate
+ vec_ew_prod(work->scaling->E, work->delta_y, work->delta_y, work->data->m);
+ }
+ work->info->obj_val = OSQP_INFTY;
+ exitflag = 1;
+ }
+ else if (dual_inf_check) {
+ // Update final information
+ if (approximate) {
+ update_status(work->info, OSQP_DUAL_INFEASIBLE_INACCURATE);
+ } else {
+ update_status(work->info, OSQP_DUAL_INFEASIBLE);
+ }
+
+ if (work->settings->scaling && !work->settings->scaled_termination) {
+ // Update infeasibility certificate
+ vec_ew_prod(work->scaling->D, work->delta_x, work->delta_x, work->data->n);
+ }
+ work->info->obj_val = -OSQP_INFTY;
+ exitflag = 1;
+ }
+
+ return exitflag;
+}
+
+
+#ifndef EMBEDDED
+
+c_int validate_data(const OSQPData *data) {
+ c_int j, ptr;
+
+ if (!data) {
+# ifdef PRINTING
+ c_eprint("Missing data");
+# endif
+ return 1;
+ }
+
+ if (!(data->P)) {
+# ifdef PRINTING
+ c_eprint("Missing matrix P");
+# endif
+ return 1;
+ }
+
+ if (!(data->A)) {
+# ifdef PRINTING
+ c_eprint("Missing matrix A");
+# endif
+ return 1;
+ }
+
+ if (!(data->q)) {
+# ifdef PRINTING
+ c_eprint("Missing vector q");
+# endif
+ return 1;
+ }
+
+ // General dimensions Tests
+ if ((data->n <= 0) || (data->m < 0)) {
+# ifdef PRINTING
+ c_eprint("n must be positive and m nonnegative; n = %i, m = %i",
+ (int)data->n, (int)data->m);
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Matrix P
+ if (data->P->m != data->n) {
+# ifdef PRINTING
+ c_eprint("P does not have dimension n x n with n = %i", (int)data->n);
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (data->P->m != data->P->n) {
+# ifdef PRINTING
+ c_eprint("P is not square");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ for (j = 0; j < data->n; j++) { // COLUMN
+ for (ptr = data->P->p[j]; ptr < data->P->p[j + 1]; ptr++) {
+ if (data->P->i[ptr] > j) { // if ROW > COLUMN
+# ifdef PRINTING
+ c_eprint("P is not upper triangular");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+ }
+ }
+
+ // Matrix A
+ if ((data->A->m != data->m) || (data->A->n != data->n)) {
+# ifdef PRINTING
+ c_eprint("A does not have dimension %i x %i", (int)data->m, (int)data->n);
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Lower and upper bounds
+ for (j = 0; j < data->m; j++) {
+ if (data->l[j] > data->u[j]) {
+# ifdef PRINTING
+ c_eprint("Lower bound at index %d is greater than upper bound: %.4e > %.4e",
+ (int)j, data->l[j], data->u[j]);
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+ }
+
+ // TODO: Complete with other checks
+
+ return 0;
+}
+
+c_int validate_linsys_solver(c_int linsys_solver) {
+ if ((linsys_solver != QDLDL_SOLVER) &&
+ (linsys_solver != MKL_PARDISO_SOLVER)) {
+ return 1;
+ }
+
+ // TODO: Add more solvers in case
+
+ // Valid solver
+ return 0;
+}
+
+c_int validate_settings(const OSQPSettings *settings) {
+ if (!settings) {
+# ifdef PRINTING
+ c_eprint("Missing settings!");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->scaling < 0) {
+# ifdef PRINTING
+ c_eprint("scaling must be nonnegative");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if ((settings->adaptive_rho != 0) && (settings->adaptive_rho != 1)) {
+# ifdef PRINTING
+ c_eprint("adaptive_rho must be either 0 or 1");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->adaptive_rho_interval < 0) {
+# ifdef PRINTING
+ c_eprint("adaptive_rho_interval must be nonnegative");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+# ifdef PROFILING
+
+ if (settings->adaptive_rho_fraction <= 0) {
+# ifdef PRINTING
+ c_eprint("adaptive_rho_fraction must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+# endif /* ifdef PROFILING */
+
+ if (settings->adaptive_rho_tolerance < 1.0) {
+# ifdef PRINTING
+ c_eprint("adaptive_rho_tolerance must be >= 1");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->polish_refine_iter < 0) {
+# ifdef PRINTING
+ c_eprint("polish_refine_iter must be nonnegative");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->rho <= 0.0) {
+# ifdef PRINTING
+ c_eprint("rho must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->sigma <= 0.0) {
+# ifdef PRINTING
+ c_eprint("sigma must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->delta <= 0.0) {
+# ifdef PRINTING
+ c_eprint("delta must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->max_iter <= 0) {
+# ifdef PRINTING
+ c_eprint("max_iter must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->eps_abs < 0.0) {
+# ifdef PRINTING
+ c_eprint("eps_abs must be nonnegative");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->eps_rel < 0.0) {
+# ifdef PRINTING
+ c_eprint("eps_rel must be nonnegative");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if ((settings->eps_rel == 0.0) &&
+ (settings->eps_abs == 0.0)) {
+# ifdef PRINTING
+ c_eprint("at least one of eps_abs and eps_rel must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->eps_prim_inf <= 0.0) {
+# ifdef PRINTING
+ c_eprint("eps_prim_inf must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->eps_dual_inf <= 0.0) {
+# ifdef PRINTING
+ c_eprint("eps_dual_inf must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if ((settings->alpha <= 0.0) ||
+ (settings->alpha >= 2.0)) {
+# ifdef PRINTING
+ c_eprint("alpha must be strictly between 0 and 2");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (validate_linsys_solver(settings->linsys_solver)) {
+# ifdef PRINTING
+ c_eprint("linsys_solver not recognized");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if ((settings->verbose != 0) &&
+ (settings->verbose != 1)) {
+# ifdef PRINTING
+ c_eprint("verbose must be either 0 or 1");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if ((settings->scaled_termination != 0) &&
+ (settings->scaled_termination != 1)) {
+# ifdef PRINTING
+ c_eprint("scaled_termination must be either 0 or 1");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if (settings->check_termination < 0) {
+# ifdef PRINTING
+ c_eprint("check_termination must be nonnegative");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ if ((settings->warm_start != 0) &&
+ (settings->warm_start != 1)) {
+# ifdef PRINTING
+ c_eprint("warm_start must be either 0 or 1");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+# ifdef PROFILING
+
+ if (settings->time_limit < 0.0) {
+# ifdef PRINTING
+ c_eprint("time_limit must be nonnegative\n");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+# endif /* ifdef PROFILING */
+
+ return 0;
+}
+
+#endif // #ifndef EMBEDDED
diff --git a/src/cs.c b/src/cs.c
new file mode 100644
index 0000000..a678ceb
--- /dev/null
+++ b/src/cs.c
@@ -0,0 +1,318 @@
+#include "cs.h"
+
+
+static void* csc_malloc(c_int n, c_int size) {
+ return c_malloc(n * size);
+}
+
+static void* csc_calloc(c_int n, c_int size) {
+ return c_calloc(n, size);
+}
+
+csc* csc_matrix(c_int m, c_int n, c_int nzmax, c_float *x, c_int *i, c_int *p)
+{
+ csc *M = (csc *)c_malloc(sizeof(csc));
+
+ if (!M) return OSQP_NULL;
+
+ M->m = m;
+ M->n = n;
+ M->nz = -1;
+ M->nzmax = nzmax;
+ M->x = x;
+ M->i = i;
+ M->p = p;
+ return M;
+}
+
+csc* csc_spalloc(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet) {
+ csc *A = csc_calloc(1, sizeof(csc)); /* allocate the csc struct */
+
+ if (!A) return OSQP_NULL; /* out of memory */
+
+ A->m = m; /* define dimensions and nzmax */
+ A->n = n;
+ A->nzmax = nzmax = c_max(nzmax, 1);
+ A->nz = triplet ? 0 : -1; /* allocate triplet or comp.col */
+ A->p = csc_malloc(triplet ? nzmax : n + 1, sizeof(c_int));
+ A->i = csc_malloc(nzmax, sizeof(c_int));
+ A->x = values ? csc_malloc(nzmax, sizeof(c_float)) : OSQP_NULL;
+ if (!A->p || !A->i || (values && !A->x)){
+ csc_spfree(A);
+ return OSQP_NULL;
+ } else return A;
+}
+
+void csc_spfree(csc *A) {
+ if (A){
+ if (A->p) c_free(A->p);
+ if (A->i) c_free(A->i);
+ if (A->x) c_free(A->x);
+ c_free(A);
+ }
+}
+
+csc* triplet_to_csc(const csc *T, c_int *TtoC) {
+ c_int m, n, nz, p, k, *Cp, *Ci, *w, *Ti, *Tj;
+ c_float *Cx, *Tx;
+ csc *C;
+
+ m = T->m;
+ n = T->n;
+ Ti = T->i;
+ Tj = T->p;
+ Tx = T->x;
+ nz = T->nz;
+ C = csc_spalloc(m, n, nz, Tx != OSQP_NULL, 0); /* allocate result */
+ w = csc_calloc(n, sizeof(c_int)); /* get workspace */
+
+ if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */
+
+ Cp = C->p;
+ Ci = C->i;
+ Cx = C->x;
+
+ for (k = 0; k < nz; k++) w[Tj[k]]++; /* column counts */
+ csc_cumsum(Cp, w, n); /* column pointers */
+
+ for (k = 0; k < nz; k++) {
+ Ci[p = w[Tj[k]]++] = Ti[k]; /* A(i,j) is the pth entry in C */
+
+ if (Cx) {
+ Cx[p] = Tx[k];
+
+ if (TtoC != OSQP_NULL) TtoC[k] = p; // Assign vector of indices
+ }
+ }
+ return csc_done(C, w, OSQP_NULL, 1); /* success; free w and return C */
+}
+
+csc* triplet_to_csr(const csc *T, c_int *TtoC) {
+ c_int m, n, nz, p, k, *Cp, *Cj, *w, *Ti, *Tj;
+ c_float *Cx, *Tx;
+ csc *C;
+
+ m = T->m;
+ n = T->n;
+ Ti = T->i;
+ Tj = T->p;
+ Tx = T->x;
+ nz = T->nz;
+ C = csc_spalloc(m, n, nz, Tx != OSQP_NULL, 0); /* allocate result */
+ w = csc_calloc(m, sizeof(c_int)); /* get workspace */
+
+ if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */
+
+ Cp = C->p;
+ Cj = C->i;
+ Cx = C->x;
+
+ for (k = 0; k < nz; k++) w[Ti[k]]++; /* row counts */
+ csc_cumsum(Cp, w, m); /* row pointers */
+
+ for (k = 0; k < nz; k++) {
+ Cj[p = w[Ti[k]]++] = Tj[k]; /* A(i,j) is the pth entry in C */
+
+ if (Cx) {
+ Cx[p] = Tx[k];
+
+ if (TtoC != OSQP_NULL) TtoC[k] = p; // Assign vector of indices
+ }
+ }
+ return csc_done(C, w, OSQP_NULL, 1); /* success; free w and return C */
+}
+
+c_int csc_cumsum(c_int *p, c_int *c, c_int n) {
+ c_int i, nz = 0;
+
+ if (!p || !c) return -1; /* check inputs */
+
+ for (i = 0; i < n; i++)
+ {
+ p[i] = nz;
+ nz += c[i];
+ c[i] = p[i];
+ }
+ p[n] = nz;
+ return nz; /* return sum (c [0..n-1]) */
+}
+
+c_int* csc_pinv(c_int const *p, c_int n) {
+ c_int k, *pinv;
+
+ if (!p) return OSQP_NULL; /* p = OSQP_NULL denotes identity */
+
+ pinv = csc_malloc(n, sizeof(c_int)); /* allocate result */
+
+ if (!pinv) return OSQP_NULL; /* out of memory */
+
+ for (k = 0; k < n; k++) pinv[p[k]] = k; /* invert the permutation */
+ return pinv; /* return result */
+}
+
+csc* csc_symperm(const csc *A, const c_int *pinv, c_int *AtoC, c_int values) {
+ c_int i, j, p, q, i2, j2, n, *Ap, *Ai, *Cp, *Ci, *w;
+ c_float *Cx, *Ax;
+ csc *C;
+
+ n = A->n;
+ Ap = A->p;
+ Ai = A->i;
+ Ax = A->x;
+ C = csc_spalloc(n, n, Ap[n], values && (Ax != OSQP_NULL),
+ 0); /* alloc result*/
+ w = csc_calloc(n, sizeof(c_int)); /* get workspace */
+
+ if (!C || !w) return csc_done(C, w, OSQP_NULL, 0); /* out of memory */
+
+ Cp = C->p;
+ Ci = C->i;
+ Cx = C->x;
+
+ for (j = 0; j < n; j++) /* count entries in each column of C */
+ {
+ j2 = pinv ? pinv[j] : j; /* column j of A is column j2 of C */
+
+ for (p = Ap[j]; p < Ap[j + 1]; p++) {
+ i = Ai[p];
+
+ if (i > j) continue; /* skip lower triangular part of A */
+ i2 = pinv ? pinv[i] : i; /* row i of A is row i2 of C */
+ w[c_max(i2, j2)]++; /* column count of C */
+ }
+ }
+ csc_cumsum(Cp, w, n); /* compute column pointers of C */
+
+ for (j = 0; j < n; j++) {
+ j2 = pinv ? pinv[j] : j; /* column j of A is column j2 of C */
+
+ for (p = Ap[j]; p < Ap[j + 1]; p++) {
+ i = Ai[p];
+
+ if (i > j) continue; /* skip lower triangular
+ part of A*/
+ i2 = pinv ? pinv[i] : i; /* row i of A is row i2
+ of C */
+ Ci[q = w[c_max(i2, j2)]++] = c_min(i2, j2);
+
+ if (Cx) Cx[q] = Ax[p];
+
+ if (AtoC) { // If vector AtoC passed, store values of the mappings
+ AtoC[p] = q;
+ }
+ }
+ }
+ return csc_done(C, w, OSQP_NULL, 1); /* success; free workspace, return C */
+}
+
+csc* copy_csc_mat(const csc *A) {
+ csc *B = csc_spalloc(A->m, A->n, A->p[A->n], 1, 0);
+
+ if (!B) return OSQP_NULL;
+
+ prea_int_vec_copy(A->p, B->p, A->n + 1);
+ prea_int_vec_copy(A->i, B->i, A->p[A->n]);
+ prea_vec_copy(A->x, B->x, A->p[A->n]);
+
+ return B;
+}
+
+void prea_copy_csc_mat(const csc *A, csc *B) {
+ prea_int_vec_copy(A->p, B->p, A->n + 1);
+ prea_int_vec_copy(A->i, B->i, A->p[A->n]);
+ prea_vec_copy(A->x, B->x, A->p[A->n]);
+
+ B->nzmax = A->nzmax;
+}
+
+csc* csc_done(csc *C, void *w, void *x, c_int ok) {
+ c_free(w); /* free workspace */
+ c_free(x);
+ if (ok) return C;
+ else {
+ csc_spfree(C);
+ return OSQP_NULL;
+ }
+}
+
+csc* csc_to_triu(csc *M) {
+ csc *M_trip; // Matrix in triplet format
+ csc *M_triu; // Resulting upper triangular matrix
+ c_int nnzorigM; // Number of nonzeros from original matrix M
+ c_int nnzmaxM; // Estimated maximum number of elements of upper triangular M
+ c_int n; // Dimension of M
+ c_int ptr, i, j; // Counters for (i,j) and index in M
+ c_int z_M = 0; // Counter for elements in M_trip
+
+
+ // Check if matrix is square
+ if (M->m != M->n) {
+#ifdef PRINTING
+ c_eprint("Matrix M not square");
+#endif /* ifdef PRINTING */
+ return OSQP_NULL;
+ }
+ n = M->n;
+
+ // Get number of nonzeros full M
+ nnzorigM = M->p[n];
+
+ // Estimate nnzmaxM
+ // Number of nonzero elements in original M + diagonal part.
+ // -> Full matrix M as input: estimate is half the number of total elements +
+ // diagonal = .5 * (nnzorigM + n)
+ // -> Upper triangular matrix M as input: estimate is the number of total
+ // elements + diagonal = nnzorigM + n
+ // The maximum between the two is nnzorigM + n
+ nnzmaxM = nnzorigM + n;
+
+ // OLD
+ // nnzmaxM = n*(n+1)/2; // Full upper triangular matrix (This version
+ // allocates too much memory!)
+ // nnzmaxM = .5 * (nnzorigM + n); // half of the total elements + diagonal
+
+ // Allocate M_trip
+ M_trip = csc_spalloc(n, n, nnzmaxM, 1, 1); // Triplet format
+
+ if (!M_trip) {
+#ifdef PRINTING
+ c_eprint("Upper triangular matrix extraction failed (out of memory)");
+#endif /* ifdef PRINTING */
+ return OSQP_NULL;
+ }
+
+ // Fill M_trip with only elements in M which are in the upper triangular
+ for (j = 0; j < n; j++) { // Cycle over columns
+ for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) {
+ // Get row index
+ i = M->i[ptr];
+
+ // Assign element only if in the upper triangular
+ if (i <= j) {
+ // c_print("\nM(%i, %i) = %.4f", M->i[ptr], j, M->x[ptr]);
+
+ M_trip->i[z_M] = i;
+ M_trip->p[z_M] = j;
+ M_trip->x[z_M] = M->x[ptr];
+
+ // Increase counter for the number of elements
+ z_M++;
+ }
+ }
+ }
+
+ // Set number of nonzeros
+ M_trip->nz = z_M;
+
+ // Convert triplet matrix to csc format
+ M_triu = triplet_to_csc(M_trip, OSQP_NULL);
+
+ // Assign number of nonzeros of full matrix to triu M
+ M_triu->nzmax = nnzmaxM;
+
+ // Cleanup and return result
+ csc_spfree(M_trip);
+
+ // Return matrix in triplet form
+ return M_triu;
+}
diff --git a/src/ctrlc.c b/src/ctrlc.c
new file mode 100644
index 0000000..a1fe2ea
--- /dev/null
+++ b/src/ctrlc.c
@@ -0,0 +1,80 @@
+/*
+ * Implements signal handling (ctrl-c) for OSQP.
+ *
+ * Under Windows, we use SetConsoleCtrlHandler.
+ * Under Unix systems, we use sigaction.
+ * For Mex files, we use utSetInterruptEnabled/utIsInterruptPending.
+ *
+ */
+
+#include "ctrlc.h"
+
+#if defined MATLAB
+
+static int istate;
+
+void osqp_start_interrupt_listener(void) {
+ istate = utSetInterruptEnabled(1);
+}
+
+void osqp_end_interrupt_listener(void) {
+ utSetInterruptEnabled(istate);
+}
+
+int osqp_is_interrupted(void) {
+ return utIsInterruptPending();
+}
+
+#elif defined IS_WINDOWS
+
+static int int_detected;
+static BOOL WINAPI handle_ctrlc(DWORD dwCtrlType) {
+ if (dwCtrlType != CTRL_C_EVENT) return FALSE;
+
+ int_detected = 1;
+ return TRUE;
+}
+
+void osqp_start_interrupt_listener(void) {
+ int_detected = 0;
+ SetConsoleCtrlHandler(handle_ctrlc, TRUE);
+}
+
+void osqp_end_interrupt_listener(void) {
+ SetConsoleCtrlHandler(handle_ctrlc, FALSE);
+}
+
+int osqp_is_interrupted(void) {
+ return int_detected;
+}
+
+#else /* Unix */
+
+# include <signal.h>
+static int int_detected;
+struct sigaction oact;
+static void handle_ctrlc(int dummy) {
+ int_detected = dummy ? dummy : -1;
+}
+
+void osqp_start_interrupt_listener(void) {
+ struct sigaction act;
+
+ int_detected = 0;
+ act.sa_flags = 0;
+ sigemptyset(&act.sa_mask);
+ act.sa_handler = handle_ctrlc;
+ sigaction(SIGINT, &act, &oact);
+}
+
+void osqp_end_interrupt_listener(void) {
+ struct sigaction act;
+
+ sigaction(SIGINT, &oact, &act);
+}
+
+int osqp_is_interrupted(void) {
+ return int_detected;
+}
+
+#endif /* END IF IS_MATLAB / WINDOWS */
diff --git a/src/error.c b/src/error.c
new file mode 100644
index 0000000..e0c069e
--- /dev/null
+++ b/src/error.c
@@ -0,0 +1,21 @@
+#include "error.h"
+
+const char *OSQP_ERROR_MESSAGE[] = {
+ "Problem data validation.",
+ "Solver settings validation.",
+ "Linear system solver not available.\nTried to obtain it from shared library.",
+ "Linear system solver initialization.",
+ "KKT matrix factorization.\nThe problem seems to be non-convex.",
+ "Memory allocation.",
+ "Solver workspace not initialized.",
+};
+
+
+c_int _osqp_error(enum osqp_error_type error_code,
+ const char * function_name) {
+# ifdef PRINTING
+ c_print("ERROR in %s: %s\n", function_name, OSQP_ERROR_MESSAGE[error_code-1]);
+# endif
+ return (c_int)error_code;
+}
+
diff --git a/src/kkt.c b/src/kkt.c
new file mode 100644
index 0000000..f89aea0
--- /dev/null
+++ b/src/kkt.c
@@ -0,0 +1,224 @@
+#include "kkt.h"
+
+#ifndef EMBEDDED
+
+
+csc* form_KKT(const csc *P,
+ const csc *A,
+ c_int format,
+ c_float param1,
+ c_float *param2,
+ c_int *PtoKKT,
+ c_int *AtoKKT,
+ c_int **Pdiag_idx,
+ c_int *Pdiag_n,
+ c_int *param2toKKT) {
+ c_int nKKT, nnzKKTmax; // Size, number of nonzeros and max number of nonzeros
+ // in KKT matrix
+ csc *KKT_trip, *KKT; // KKT matrix in triplet format and CSC format
+ c_int ptr, i, j; // Counters for elements (i,j) and index pointer
+ c_int zKKT = 0; // Counter for total number of elements in P and in
+ // KKT
+ c_int *KKT_TtoC; // Pointer to vector mapping from KKT in triplet form
+ // to CSC
+
+ // Get matrix dimensions
+ nKKT = P->m + A->m;
+
+ // Get maximum number of nonzero elements (only upper triangular part)
+ nnzKKTmax = P->p[P->n] + // Number of elements in P
+ P->m + // Number of elements in param1 * I
+ A->p[A->n] + // Number of nonzeros in A
+ A->m; // Number of elements in - diag(param2)
+
+ // Preallocate KKT matrix in triplet format
+ KKT_trip = csc_spalloc(nKKT, nKKT, nnzKKTmax, 1, 1);
+
+ if (!KKT_trip) return OSQP_NULL; // Failed to preallocate matrix
+
+ // Allocate vector of indices on the diagonal. Worst case it has m elements
+ if (Pdiag_idx != OSQP_NULL) {
+ (*Pdiag_idx) = c_malloc(P->m * sizeof(c_int));
+ *Pdiag_n = 0; // Set 0 diagonal elements to start
+ }
+
+ // Allocate Triplet matrices
+ // P + param1 I
+ for (j = 0; j < P->n; j++) { // cycle over columns
+ // No elements in column j => add diagonal element param1
+ if (P->p[j] == P->p[j + 1]) {
+ KKT_trip->i[zKKT] = j;
+ KKT_trip->p[zKKT] = j;
+ KKT_trip->x[zKKT] = param1;
+ zKKT++;
+ }
+
+ for (ptr = P->p[j]; ptr < P->p[j + 1]; ptr++) { // cycle over rows
+ // Get current row
+ i = P->i[ptr];
+
+ // Add element of P
+ KKT_trip->i[zKKT] = i;
+ KKT_trip->p[zKKT] = j;
+ KKT_trip->x[zKKT] = P->x[ptr];
+
+ if (PtoKKT != OSQP_NULL) PtoKKT[ptr] = zKKT; // Update index from P to
+ // KKTtrip
+
+ if (i == j) { // P has a diagonal element,
+ // add param1
+ KKT_trip->x[zKKT] += param1;
+
+ // If index vector pointer supplied -> Store the index
+ if (Pdiag_idx != OSQP_NULL) {
+ (*Pdiag_idx)[*Pdiag_n] = ptr;
+ (*Pdiag_n)++;
+ }
+ }
+ zKKT++;
+
+ // Add diagonal param1 in case
+ if ((i < j) && // Diagonal element not reached
+ (ptr + 1 == P->p[j + 1])) { // last element of column j
+ // Add diagonal element param1
+ KKT_trip->i[zKKT] = j;
+ KKT_trip->p[zKKT] = j;
+ KKT_trip->x[zKKT] = param1;
+ zKKT++;
+ }
+ }
+ }
+
+ if (Pdiag_idx != OSQP_NULL) {
+ // Realloc Pdiag_idx so that it contains exactly *Pdiag_n diagonal elements
+ (*Pdiag_idx) = c_realloc((*Pdiag_idx), (*Pdiag_n) * sizeof(c_int));
+ }
+
+
+ // A' at top right
+ for (j = 0; j < A->n; j++) { // Cycle over columns of A
+ for (ptr = A->p[j]; ptr < A->p[j + 1]; ptr++) {
+ KKT_trip->p[zKKT] = P->m + A->i[ptr]; // Assign column index from
+ // row index of A
+ KKT_trip->i[zKKT] = j; // Assign row index from
+ // column index of A
+ KKT_trip->x[zKKT] = A->x[ptr]; // Assign A value element
+
+ if (AtoKKT != OSQP_NULL) AtoKKT[ptr] = zKKT; // Update index from A to
+ // KKTtrip
+ zKKT++;
+ }
+ }
+
+ // - diag(param2) at bottom right
+ for (j = 0; j < A->m; j++) {
+ KKT_trip->i[zKKT] = j + P->n;
+ KKT_trip->p[zKKT] = j + P->n;
+ KKT_trip->x[zKKT] = -param2[j];
+
+ if (param2toKKT != OSQP_NULL) param2toKKT[j] = zKKT; // Update index from
+ // param2 to KKTtrip
+ zKKT++;
+ }
+
+ // Allocate number of nonzeros
+ KKT_trip->nz = zKKT;
+
+ // Convert triplet matrix to csc format
+ if (!PtoKKT && !AtoKKT && !param2toKKT) {
+ // If no index vectors passed, do not store KKT mapping from Trip to CSC/CSR
+ if (format == 0) KKT = triplet_to_csc(KKT_trip, OSQP_NULL);
+ else KKT = triplet_to_csr(KKT_trip, OSQP_NULL);
+ }
+ else {
+ // Allocate vector of indices from triplet to csc
+ KKT_TtoC = c_malloc((zKKT) * sizeof(c_int));
+
+ if (!KKT_TtoC) {
+ // Error in allocating KKT_TtoC vector
+ csc_spfree(KKT_trip);
+ c_free(*Pdiag_idx);
+ return OSQP_NULL;
+ }
+
+ // Store KKT mapping from Trip to CSC/CSR
+ if (format == 0)
+ KKT = triplet_to_csc(KKT_trip, KKT_TtoC);
+ else
+ KKT = triplet_to_csr(KKT_trip, KKT_TtoC);
+
+ // Update vectors of indices from P, A, param2 to KKT (now in CSC format)
+ if (PtoKKT != OSQP_NULL) {
+ for (i = 0; i < P->p[P->n]; i++) {
+ PtoKKT[i] = KKT_TtoC[PtoKKT[i]];
+ }
+ }
+
+ if (AtoKKT != OSQP_NULL) {
+ for (i = 0; i < A->p[A->n]; i++) {
+ AtoKKT[i] = KKT_TtoC[AtoKKT[i]];
+ }
+ }
+
+ if (param2toKKT != OSQP_NULL) {
+ for (i = 0; i < A->m; i++) {
+ param2toKKT[i] = KKT_TtoC[param2toKKT[i]];
+ }
+ }
+
+ // Free mapping
+ c_free(KKT_TtoC);
+ }
+
+ // Clean matrix in triplet format and return result
+ csc_spfree(KKT_trip);
+
+ return KKT;
+}
+
+#endif /* ifndef EMBEDDED */
+
+
+#if EMBEDDED != 1
+
+void update_KKT_P(csc *KKT,
+ const csc *P,
+ const c_int *PtoKKT,
+ const c_float param1,
+ const c_int *Pdiag_idx,
+ const c_int Pdiag_n) {
+ c_int i, j; // Iterations
+
+ // Update elements of KKT using P
+ for (i = 0; i < P->p[P->n]; i++) {
+ KKT->x[PtoKKT[i]] = P->x[i];
+ }
+
+ // Update diagonal elements of KKT by adding sigma
+ for (i = 0; i < Pdiag_n; i++) {
+ j = Pdiag_idx[i]; // Extract index of the element on the
+ // diagonal
+ KKT->x[PtoKKT[j]] += param1;
+ }
+}
+
+void update_KKT_A(csc *KKT, const csc *A, const c_int *AtoKKT) {
+ c_int i; // Iterations
+
+ // Update elements of KKT using A
+ for (i = 0; i < A->p[A->n]; i++) {
+ KKT->x[AtoKKT[i]] = A->x[i];
+ }
+}
+
+void update_KKT_param2(csc *KKT, const c_float *param2,
+ const c_int *param2toKKT, const c_int m) {
+ c_int i; // Iterations
+
+ // Update elements of KKT using param2
+ for (i = 0; i < m; i++) {
+ KKT->x[param2toKKT[i]] = -param2[i];
+ }
+}
+
+#endif // EMBEDDED != 1
diff --git a/src/lin_alg.c b/src/lin_alg.c
new file mode 100644
index 0000000..ba896f5
--- /dev/null
+++ b/src/lin_alg.c
@@ -0,0 +1,413 @@
+#include "lin_alg.h"
+
+
+/* VECTOR FUNCTIONS ----------------------------------------------------------*/
+
+
+void vec_add_scaled(c_float *c,
+ const c_float *a,
+ const c_float *b,
+ c_int n,
+ c_float sc) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ c[i] = a[i] + sc * b[i];
+ }
+}
+
+c_float vec_scaled_norm_inf(const c_float *S, const c_float *v, c_int l) {
+ c_int i;
+ c_float abs_Sv_i;
+ c_float max = 0.0;
+
+ for (i = 0; i < l; i++) {
+ abs_Sv_i = c_absval(S[i] * v[i]);
+
+ if (abs_Sv_i > max) max = abs_Sv_i;
+ }
+ return max;
+}
+
+c_float vec_norm_inf(const c_float *v, c_int l) {
+ c_int i;
+ c_float abs_v_i;
+ c_float max = 0.0;
+
+ for (i = 0; i < l; i++) {
+ abs_v_i = c_absval(v[i]);
+
+ if (abs_v_i > max) max = abs_v_i;
+ }
+ return max;
+}
+
+c_float vec_norm_inf_diff(const c_float *a, const c_float *b, c_int l) {
+ c_float nmDiff = 0.0, tmp;
+ c_int i;
+
+ for (i = 0; i < l; i++) {
+ tmp = c_absval(a[i] - b[i]);
+
+ if (tmp > nmDiff) nmDiff = tmp;
+ }
+ return nmDiff;
+}
+
+c_float vec_mean(const c_float *a, c_int n) {
+ c_float mean = 0.0;
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ mean += a[i];
+ }
+ mean /= (c_float)n;
+
+ return mean;
+}
+
+void int_vec_set_scalar(c_int *a, c_int sc, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = sc;
+ }
+}
+
+void vec_set_scalar(c_float *a, c_float sc, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = sc;
+ }
+}
+
+void vec_add_scalar(c_float *a, c_float sc, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] += sc;
+ }
+}
+
+void vec_mult_scalar(c_float *a, c_float sc, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] *= sc;
+ }
+}
+
+#ifndef EMBEDDED
+c_float* vec_copy(c_float *a, c_int n) {
+ c_float *b;
+ c_int i;
+
+ b = c_malloc(n * sizeof(c_float));
+ if (!b) return OSQP_NULL;
+
+ for (i = 0; i < n; i++) {
+ b[i] = a[i];
+ }
+
+ return b;
+}
+
+#endif // end EMBEDDED
+
+
+void prea_int_vec_copy(const c_int *a, c_int *b, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ b[i] = a[i];
+ }
+}
+
+void prea_vec_copy(const c_float *a, c_float *b, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ b[i] = a[i];
+ }
+}
+
+void vec_ew_recipr(const c_float *a, c_float *b, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ b[i] = (c_float)1.0 / a[i];
+ }
+}
+
+c_float vec_prod(const c_float *a, const c_float *b, c_int n) {
+ c_float prod = 0.0;
+ c_int i; // Index
+
+ for (i = 0; i < n; i++) {
+ prod += a[i] * b[i];
+ }
+
+ return prod;
+}
+
+void vec_ew_prod(const c_float *a, const c_float *b, c_float *c, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ c[i] = b[i] * a[i];
+ }
+}
+
+#if EMBEDDED != 1
+void vec_ew_sqrt(c_float *a, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = c_sqrt(a[i]);
+ }
+}
+
+void vec_ew_max(c_float *a, c_int n, c_float max_val) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = c_max(a[i], max_val);
+ }
+}
+
+void vec_ew_min(c_float *a, c_int n, c_float min_val) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ a[i] = c_min(a[i], min_val);
+ }
+}
+
+void vec_ew_max_vec(const c_float *a, const c_float *b, c_float *c, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ c[i] = c_max(a[i], b[i]);
+ }
+}
+
+void vec_ew_min_vec(const c_float *a, const c_float *b, c_float *c, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ c[i] = c_min(a[i], b[i]);
+ }
+}
+
+#endif // EMBEDDED != 1
+
+
+/* MATRIX FUNCTIONS ----------------------------------------------------------*/
+
+/* multiply scalar to matrix */
+void mat_mult_scalar(csc *A, c_float sc) {
+ c_int i, nnzA;
+
+ nnzA = A->p[A->n];
+
+ for (i = 0; i < nnzA; i++) {
+ A->x[i] *= sc;
+ }
+}
+
+void mat_premult_diag(csc *A, const c_float *d) {
+ c_int j, i;
+
+ for (j = 0; j < A->n; j++) { // Cycle over columns
+ for (i = A->p[j]; i < A->p[j + 1]; i++) { // Cycle every row in the column
+ A->x[i] *= d[A->i[i]]; // Scale by corresponding element
+ // of d for row i
+ }
+ }
+}
+
+void mat_postmult_diag(csc *A, const c_float *d) {
+ c_int j, i;
+
+ for (j = 0; j < A->n; j++) { // Cycle over columns j
+ for (i = A->p[j]; i < A->p[j + 1]; i++) { // Cycle every row i in column j
+ A->x[i] *= d[j]; // Scale by corresponding element
+ // of d for column j
+ }
+ }
+}
+
+void mat_vec(const csc *A, const c_float *x, c_float *y, c_int plus_eq) {
+ c_int i, j;
+
+ if (!plus_eq) {
+ // y = 0
+ for (i = 0; i < A->m; i++) {
+ y[i] = 0;
+ }
+ }
+
+ // if A is empty
+ if (A->p[A->n] == 0) {
+ return;
+ }
+
+ if (plus_eq == -1) {
+ // y -= A*x
+ for (j = 0; j < A->n; j++) {
+ for (i = A->p[j]; i < A->p[j + 1]; i++) {
+ y[A->i[i]] -= A->x[i] * x[j];
+ }
+ }
+ } else {
+ // y += A*x
+ for (j = 0; j < A->n; j++) {
+ for (i = A->p[j]; i < A->p[j + 1]; i++) {
+ y[A->i[i]] += A->x[i] * x[j];
+ }
+ }
+ }
+}
+
+void mat_tpose_vec(const csc *A, const c_float *x, c_float *y,
+ c_int plus_eq, c_int skip_diag) {
+ c_int i, j, k;
+
+ if (!plus_eq) {
+ // y = 0
+ for (i = 0; i < A->n; i++) {
+ y[i] = 0;
+ }
+ }
+
+ // if A is empty
+ if (A->p[A->n] == 0) {
+ return;
+ }
+
+ if (plus_eq == -1) {
+ // y -= A*x
+ if (skip_diag) {
+ for (j = 0; j < A->n; j++) {
+ for (k = A->p[j]; k < A->p[j + 1]; k++) {
+ i = A->i[k];
+ y[j] -= i == j ? 0 : A->x[k] * x[i];
+ }
+ }
+ } else {
+ for (j = 0; j < A->n; j++) {
+ for (k = A->p[j]; k < A->p[j + 1]; k++) {
+ y[j] -= A->x[k] * x[A->i[k]];
+ }
+ }
+ }
+ } else {
+ // y += A*x
+ if (skip_diag) {
+ for (j = 0; j < A->n; j++) {
+ for (k = A->p[j]; k < A->p[j + 1]; k++) {
+ i = A->i[k];
+ y[j] += i == j ? 0 : A->x[k] * x[i];
+ }
+ }
+ } else {
+ for (j = 0; j < A->n; j++) {
+ for (k = A->p[j]; k < A->p[j + 1]; k++) {
+ y[j] += A->x[k] * x[A->i[k]];
+ }
+ }
+ }
+ }
+}
+
+#if EMBEDDED != 1
+void mat_inf_norm_cols(const csc *M, c_float *E) {
+ c_int j, ptr;
+
+ // Initialize zero max elements
+ for (j = 0; j < M->n; j++) {
+ E[j] = 0.;
+ }
+
+ // Compute maximum across columns
+ for (j = 0; j < M->n; j++) {
+ for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) {
+ E[j] = c_max(c_absval(M->x[ptr]), E[j]);
+ }
+ }
+}
+
+void mat_inf_norm_rows(const csc *M, c_float *E) {
+ c_int i, j, ptr;
+
+ // Initialize zero max elements
+ for (j = 0; j < M->m; j++) {
+ E[j] = 0.;
+ }
+
+ // Compute maximum across rows
+ for (j = 0; j < M->n; j++) {
+ for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) {
+ i = M->i[ptr];
+ E[i] = c_max(c_absval(M->x[ptr]), E[i]);
+ }
+ }
+}
+
+void mat_inf_norm_cols_sym_triu(const csc *M, c_float *E) {
+ c_int i, j, ptr;
+ c_float abs_x;
+
+ // Initialize zero max elements
+ for (j = 0; j < M->n; j++) {
+ E[j] = 0.;
+ }
+
+ // Compute maximum across columns
+ // Note that element (i, j) contributes to
+ // -> Column j (as expected in any matrices)
+ // -> Column i (which is equal to row i for symmetric matrices)
+ for (j = 0; j < M->n; j++) {
+ for (ptr = M->p[j]; ptr < M->p[j + 1]; ptr++) {
+ i = M->i[ptr];
+ abs_x = c_absval(M->x[ptr]);
+ E[j] = c_max(abs_x, E[j]);
+
+ if (i != j) {
+ E[i] = c_max(abs_x, E[i]);
+ }
+ }
+ }
+}
+
+#endif /* if EMBEDDED != 1 */
+
+
+c_float quad_form(const csc *P, const c_float *x) {
+ c_float quad_form = 0.;
+ c_int i, j, ptr; // Pointers to iterate over
+ // matrix: (i,j) a element
+ // pointer
+
+ for (j = 0; j < P->n; j++) { // Iterate over columns
+ for (ptr = P->p[j]; ptr < P->p[j + 1]; ptr++) { // Iterate over rows
+ i = P->i[ptr]; // Row index
+
+ if (i == j) { // Diagonal element
+ quad_form += (c_float).5 * P->x[ptr] * x[i] * x[i];
+ }
+ else if (i < j) { // Off-diagonal element
+ quad_form += P->x[ptr] * x[i] * x[j];
+ }
+ else { // Element in lower diagonal
+ // part
+#ifdef PRINTING
+ c_eprint("quad_form matrix is not upper triangular");
+#endif /* ifdef PRINTING */
+ return OSQP_NULL;
+ }
+ }
+ }
+ return quad_form;
+}
diff --git a/src/lin_sys.c b/src/lin_sys.c
new file mode 100644
index 0000000..fe617d3
--- /dev/null
+++ b/src/lin_sys.c
@@ -0,0 +1,75 @@
+#include "lin_sys.h"
+
+#include "qdldl_interface.h" // Include only this solver in the same directory
+
+const char *LINSYS_SOLVER_NAME[] = {
+ "qdldl", "mkl pardiso"
+};
+
+#ifdef ENABLE_MKL_PARDISO
+# include "pardiso_interface.h"
+# include "pardiso_loader.h"
+#endif /* ifdef ENABLE_MKL_PARDISO */
+
+// Load linear system solver shared library
+c_int load_linsys_solver(enum linsys_solver_type linsys_solver) {
+ switch (linsys_solver) {
+ case QDLDL_SOLVER:
+
+ // We do not load QDLDL solver. We have the source.
+ return 0;
+
+# ifdef ENABLE_MKL_PARDISO
+ case MKL_PARDISO_SOLVER:
+
+ // Load Pardiso library
+ return lh_load_pardiso(OSQP_NULL);
+
+# endif /* ifdef ENABLE_MKL_PARDISO */
+ default: // QDLDL
+ return 0;
+ }
+}
+
+// Unload linear system solver shared library
+c_int unload_linsys_solver(enum linsys_solver_type linsys_solver) {
+ switch (linsys_solver) {
+ case QDLDL_SOLVER:
+
+ // We do not load QDLDL solver. We have the source.
+ return 0;
+
+# ifdef ENABLE_MKL_PARDISO
+ case MKL_PARDISO_SOLVER:
+
+ // Unload Pardiso library
+ return lh_unload_pardiso();
+
+# endif /* ifdef ENABLE_MKL_PARDISO */
+ default: // QDLDL
+ return 0;
+ }
+}
+
+// Initialize linear system solver structure
+// NB: Only the upper triangular part of P is stuffed!
+c_int init_linsys_solver(LinSysSolver **s,
+ const csc *P,
+ const csc *A,
+ c_float sigma,
+ const c_float *rho_vec,
+ enum linsys_solver_type linsys_solver,
+ c_int polish) {
+ switch (linsys_solver) {
+ case QDLDL_SOLVER:
+ return init_linsys_solver_qdldl((qdldl_solver **)s, P, A, sigma, rho_vec, polish);
+
+# ifdef ENABLE_MKL_PARDISO
+ case MKL_PARDISO_SOLVER:
+ return init_linsys_solver_pardiso((pardiso_solver **)s, P, A, sigma, rho_vec, polish);
+
+# endif /* ifdef ENABLE_MKL_PARDISO */
+ default: // QDLDL
+ return init_linsys_solver_qdldl((qdldl_solver **)s, P, A, sigma, rho_vec, polish);
+ }
+}
diff --git a/src/osqp.c b/src/osqp.c
new file mode 100644
index 0000000..20cb1af
--- /dev/null
+++ b/src/osqp.c
@@ -0,0 +1,1617 @@
+#include "osqp.h"
+#include "auxil.h"
+#include "util.h"
+#include "scaling.h"
+#include "glob_opts.h"
+#include "error.h"
+
+
+#ifndef EMBEDDED
+# include "polish.h"
+#endif /* ifndef EMBEDDED */
+
+#ifdef CTRLC
+# include "ctrlc.h"
+#endif /* ifdef CTRLC */
+
+#ifndef EMBEDDED
+# include "lin_sys.h"
+#endif /* ifndef EMBEDDED */
+
+/**********************
+* Main API Functions *
+**********************/
+void osqp_set_default_settings(OSQPSettings *settings) {
+
+ settings->rho = (c_float)RHO; /* ADMM step */
+ settings->sigma = (c_float)SIGMA; /* ADMM step */
+ settings->scaling = SCALING; /* heuristic problem scaling */
+#if EMBEDDED != 1
+ settings->adaptive_rho = ADAPTIVE_RHO;
+ settings->adaptive_rho_interval = ADAPTIVE_RHO_INTERVAL;
+ settings->adaptive_rho_tolerance = (c_float)ADAPTIVE_RHO_TOLERANCE;
+
+# ifdef PROFILING
+ settings->adaptive_rho_fraction = (c_float)ADAPTIVE_RHO_FRACTION;
+# endif /* ifdef PROFILING */
+#endif /* if EMBEDDED != 1 */
+
+ settings->max_iter = MAX_ITER; /* maximum iterations to
+ take */
+ settings->eps_abs = (c_float)EPS_ABS; /* absolute convergence
+ tolerance */
+ settings->eps_rel = (c_float)EPS_REL; /* relative convergence
+ tolerance */
+ settings->eps_prim_inf = (c_float)EPS_PRIM_INF; /* primal infeasibility
+ tolerance */
+ settings->eps_dual_inf = (c_float)EPS_DUAL_INF; /* dual infeasibility
+ tolerance */
+ settings->alpha = (c_float)ALPHA; /* relaxation parameter */
+ settings->linsys_solver = LINSYS_SOLVER; /* relaxation parameter */
+
+#ifndef EMBEDDED
+ settings->delta = DELTA; /* regularization parameter
+ for polish */
+ settings->polish = POLISH; /* ADMM solution polish: 1
+ */
+ settings->polish_refine_iter = POLISH_REFINE_ITER; /* iterative refinement
+ steps in polish */
+ settings->verbose = VERBOSE; /* print output */
+#endif /* ifndef EMBEDDED */
+
+ settings->scaled_termination = SCALED_TERMINATION; /* Evaluate scaled
+ termination criteria*/
+ settings->check_termination = CHECK_TERMINATION; /* Interval for evaluating
+ termination criteria */
+ settings->warm_start = WARM_START; /* warm starting */
+
+#ifdef PROFILING
+ settings->time_limit = TIME_LIMIT;
+#endif /* ifdef PROFILING */
+}
+
+#ifndef EMBEDDED
+
+
+c_int osqp_setup(OSQPWorkspace** workp, const OSQPData *data, const OSQPSettings *settings) {
+ c_int exitflag;
+
+ OSQPWorkspace * work;
+
+ // Validate data
+ if (validate_data(data)) return osqp_error(OSQP_DATA_VALIDATION_ERROR);
+
+ // Validate settings
+ if (validate_settings(settings)) return osqp_error(OSQP_SETTINGS_VALIDATION_ERROR);
+
+ // Allocate empty workspace
+ work = c_calloc(1, sizeof(OSQPWorkspace));
+ if (!(work)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ *workp = work;
+
+ // Start and allocate directly timer
+# ifdef PROFILING
+ work->timer = c_malloc(sizeof(OSQPTimer));
+ if (!(work->timer)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ osqp_tic(work->timer);
+# endif /* ifdef PROFILING */
+
+ // Copy problem data into workspace
+ work->data = c_malloc(sizeof(OSQPData));
+ if (!(work->data)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ work->data->n = data->n;
+ work->data->m = data->m;
+
+ // Cost function
+ work->data->P = copy_csc_mat(data->P);
+ work->data->q = vec_copy(data->q, data->n);
+ if (!(work->data->P) || !(work->data->q)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Constraints
+ work->data->A = copy_csc_mat(data->A);
+ if (!(work->data->A)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ work->data->l = vec_copy(data->l, data->m);
+ work->data->u = vec_copy(data->u, data->m);
+ if ( data->m && (!(work->data->l) || !(work->data->u)) )
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Vectorized rho parameter
+ work->rho_vec = c_malloc(data->m * sizeof(c_float));
+ work->rho_inv_vec = c_malloc(data->m * sizeof(c_float));
+ if ( data->m && (!(work->rho_vec) || !(work->rho_inv_vec)) )
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Type of constraints
+ work->constr_type = c_calloc(data->m, sizeof(c_int));
+ if (data->m && !(work->constr_type)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Allocate internal solver variables (ADMM steps)
+ work->x = c_calloc(data->n, sizeof(c_float));
+ work->z = c_calloc(data->m, sizeof(c_float));
+ work->xz_tilde = c_calloc(data->n + data->m, sizeof(c_float));
+ work->x_prev = c_calloc(data->n, sizeof(c_float));
+ work->z_prev = c_calloc(data->m, sizeof(c_float));
+ work->y = c_calloc(data->m, sizeof(c_float));
+ if (!(work->x) || !(work->xz_tilde) || !(work->x_prev))
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ if ( data->m && (!(work->z) || !(work->z_prev) || !(work->y)) )
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Initialize variables x, y, z to 0
+ cold_start(work);
+
+ // Primal and dual residuals variables
+ work->Ax = c_calloc(data->m, sizeof(c_float));
+ work->Px = c_calloc(data->n, sizeof(c_float));
+ work->Aty = c_calloc(data->n, sizeof(c_float));
+
+ // Primal infeasibility variables
+ work->delta_y = c_calloc(data->m, sizeof(c_float));
+ work->Atdelta_y = c_calloc(data->n, sizeof(c_float));
+
+ // Dual infeasibility variables
+ work->delta_x = c_calloc(data->n, sizeof(c_float));
+ work->Pdelta_x = c_calloc(data->n, sizeof(c_float));
+ work->Adelta_x = c_calloc(data->m, sizeof(c_float));
+
+ if (!(work->Px) || !(work->Aty) || !(work->Atdelta_y) ||
+ !(work->delta_x) || !(work->Pdelta_x))
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ if ( data->m && (!(work->Ax) || !(work->delta_y) || !(work->Adelta_x)) )
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Copy settings
+ work->settings = copy_settings(settings);
+ if (!(work->settings)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Perform scaling
+ if (settings->scaling) {
+ // Allocate scaling structure
+ work->scaling = c_malloc(sizeof(OSQPScaling));
+ if (!(work->scaling)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ work->scaling->D = c_malloc(data->n * sizeof(c_float));
+ work->scaling->Dinv = c_malloc(data->n * sizeof(c_float));
+ work->scaling->E = c_malloc(data->m * sizeof(c_float));
+ work->scaling->Einv = c_malloc(data->m * sizeof(c_float));
+ if (!(work->scaling->D) || !(work->scaling->Dinv))
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ if ( data->m && (!(work->scaling->E) || !(work->scaling->Einv)) )
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+
+ // Allocate workspace variables used in scaling
+ work->D_temp = c_malloc(data->n * sizeof(c_float));
+ work->D_temp_A = c_malloc(data->n * sizeof(c_float));
+ work->E_temp = c_malloc(data->m * sizeof(c_float));
+ // if (!(work->D_temp) || !(work->D_temp_A) || !(work->E_temp))
+ // return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ if (!(work->D_temp) || !(work->D_temp_A)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ if (data->m && !(work->E_temp)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Scale data
+ scale_data(work);
+ } else {
+ work->scaling = OSQP_NULL;
+ work->D_temp = OSQP_NULL;
+ work->D_temp_A = OSQP_NULL;
+ work->E_temp = OSQP_NULL;
+ }
+
+ // Set type of constraints
+ set_rho_vec(work);
+
+ // Load linear system solver
+ if (load_linsys_solver(work->settings->linsys_solver)) return osqp_error(OSQP_LINSYS_SOLVER_LOAD_ERROR);
+
+ // Initialize linear system solver structure
+ exitflag = init_linsys_solver(&(work->linsys_solver), work->data->P, work->data->A,
+ work->settings->sigma, work->rho_vec,
+ work->settings->linsys_solver, 0);
+
+ if (exitflag) {
+ return osqp_error(exitflag);
+ }
+
+ // Initialize active constraints structure
+ work->pol = c_malloc(sizeof(OSQPPolish));
+ if (!(work->pol)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ work->pol->Alow_to_A = c_malloc(data->m * sizeof(c_int));
+ work->pol->Aupp_to_A = c_malloc(data->m * sizeof(c_int));
+ work->pol->A_to_Alow = c_malloc(data->m * sizeof(c_int));
+ work->pol->A_to_Aupp = c_malloc(data->m * sizeof(c_int));
+ work->pol->x = c_malloc(data->n * sizeof(c_float));
+ work->pol->z = c_malloc(data->m * sizeof(c_float));
+ work->pol->y = c_malloc(data->m * sizeof(c_float));
+ if (!(work->pol->x)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ if ( data->m && (!(work->pol->Alow_to_A) || !(work->pol->Aupp_to_A) ||
+ !(work->pol->A_to_Alow) || !(work->pol->A_to_Aupp) ||
+ !(work->pol->z) || !(work->pol->y)) )
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Allocate solution
+ work->solution = c_calloc(1, sizeof(OSQPSolution));
+ if (!(work->solution)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ work->solution->x = c_calloc(1, data->n * sizeof(c_float));
+ work->solution->y = c_calloc(1, data->m * sizeof(c_float));
+ if (!(work->solution->x)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ if (data->m && !(work->solution->y)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+
+ // Allocate and initialize information
+ work->info = c_calloc(1, sizeof(OSQPInfo));
+ if (!(work->info)) return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ work->info->status_polish = 0; // Polishing not performed
+ update_status(work->info, OSQP_UNSOLVED);
+# ifdef PROFILING
+ work->info->solve_time = 0.0; // Solve time to zero
+ work->info->update_time = 0.0; // Update time to zero
+ work->info->polish_time = 0.0; // Polish time to zero
+ work->info->run_time = 0.0; // Total run time to zero
+ work->info->setup_time = osqp_toc(work->timer); // Update timer information
+
+ work->first_run = 1;
+ work->clear_update_time = 0;
+ work->rho_update_from_solve = 0;
+# endif /* ifdef PROFILING */
+ work->info->rho_updates = 0; // Rho updates set to 0
+ work->info->rho_estimate = work->settings->rho; // Best rho estimate
+
+ // Print header
+# ifdef PRINTING
+ if (work->settings->verbose) print_setup_header(work);
+ work->summary_printed = 0; // Initialize last summary to not printed
+# endif /* ifdef PRINTING */
+
+
+ // If adaptive rho and automatic interval, but profiling disabled, we need to
+ // set the interval to a default value
+# ifndef PROFILING
+ if (work->settings->adaptive_rho && !work->settings->adaptive_rho_interval) {
+ if (work->settings->check_termination) {
+ // If check_termination is enabled, we set it to a multiple of the check
+ // termination interval
+ work->settings->adaptive_rho_interval = ADAPTIVE_RHO_MULTIPLE_TERMINATION *
+ work->settings->check_termination;
+ } else {
+ // If check_termination is disabled we set it to a predefined fix number
+ work->settings->adaptive_rho_interval = ADAPTIVE_RHO_FIXED;
+ }
+ }
+# endif /* ifndef PROFILING */
+
+ // Return exit flag
+ return 0;
+}
+
+#endif // #ifndef EMBEDDED
+
+
+c_int osqp_solve(OSQPWorkspace *work) {
+
+ c_int exitflag;
+ c_int iter;
+ c_int compute_cost_function; // Boolean: compute the cost function in the loop or not
+ c_int can_check_termination; // Boolean: check termination or not
+
+#ifdef PROFILING
+ c_float temp_run_time; // Temporary variable to store current run time
+#endif /* ifdef PROFILING */
+
+#ifdef PRINTING
+ c_int can_print; // Boolean whether you can print
+#endif /* ifdef PRINTING */
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+#ifdef PROFILING
+ if (work->clear_update_time == 1)
+ work->info->update_time = 0.0;
+ work->rho_update_from_solve = 1;
+#endif /* ifdef PROFILING */
+
+ // Initialize variables
+ exitflag = 0;
+ can_check_termination = 0;
+#ifdef PRINTING
+ can_print = work->settings->verbose;
+#endif /* ifdef PRINTING */
+#ifdef PRINTING
+ compute_cost_function = work->settings->verbose; // Compute cost function only
+ // if verbose is on
+#else /* ifdef PRINTING */
+ compute_cost_function = 0; // Never compute cost
+ // function during the
+ // iterations if no printing
+ // enabled
+#endif /* ifdef PRINTING */
+
+
+
+#ifdef PROFILING
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+
+#ifdef PRINTING
+
+ if (work->settings->verbose) {
+ // Print Header for every column
+ print_header();
+ }
+#endif /* ifdef PRINTING */
+
+#ifdef CTRLC
+
+ // initialize Ctrl-C support
+ osqp_start_interrupt_listener();
+#endif /* ifdef CTRLC */
+
+ // Initialize variables (cold start or warm start depending on settings)
+ if (!work->settings->warm_start) cold_start(work); // If not warm start ->
+ // set x, z, y to zero
+
+ // Main ADMM algorithm
+ for (iter = 1; iter <= work->settings->max_iter; iter++) {
+ // Update x_prev, z_prev (preallocated, no malloc)
+ swap_vectors(&(work->x), &(work->x_prev));
+ swap_vectors(&(work->z), &(work->z_prev));
+
+ /* ADMM STEPS */
+ /* Compute \tilde{x}^{k+1}, \tilde{z}^{k+1} */
+ update_xz_tilde(work);
+
+ /* Compute x^{k+1} */
+ update_x(work);
+
+ /* Compute z^{k+1} */
+ update_z(work);
+
+ /* Compute y^{k+1} */
+ update_y(work);
+
+ /* End of ADMM Steps */
+
+#ifdef CTRLC
+
+ // Check the interrupt signal
+ if (osqp_is_interrupted()) {
+ update_status(work->info, OSQP_SIGINT);
+# ifdef PRINTING
+ c_print("Solver interrupted\n");
+# endif /* ifdef PRINTING */
+ exitflag = 1;
+ goto exit;
+ }
+#endif /* ifdef CTRLC */
+
+#ifdef PROFILING
+
+ // Check if solver time_limit is enabled. In case, check if the current
+ // run time is more than the time_limit option.
+ if (work->first_run) {
+ temp_run_time = work->info->setup_time + osqp_toc(work->timer);
+ }
+ else {
+ temp_run_time = work->info->update_time + osqp_toc(work->timer);
+ }
+
+ if (work->settings->time_limit &&
+ (temp_run_time >= work->settings->time_limit)) {
+ update_status(work->info, OSQP_TIME_LIMIT_REACHED);
+# ifdef PRINTING
+ if (work->settings->verbose) c_print("run time limit reached\n");
+ can_print = 0; // Not printing at this iteration
+# endif /* ifdef PRINTING */
+ break;
+ }
+#endif /* ifdef PROFILING */
+
+
+ // Can we check for termination ?
+ can_check_termination = work->settings->check_termination &&
+ (iter % work->settings->check_termination == 0);
+
+#ifdef PRINTING
+
+ // Can we print ?
+ can_print = work->settings->verbose &&
+ ((iter % PRINT_INTERVAL == 0) || (iter == 1));
+
+ if (can_check_termination || can_print) { // Update status in either of
+ // these cases
+ // Update information
+ update_info(work, iter, compute_cost_function, 0);
+
+ if (can_print) {
+ // Print summary
+ print_summary(work);
+ }
+
+ if (can_check_termination) {
+ // Check algorithm termination
+ if (check_termination(work, 0)) {
+ // Terminate algorithm
+ break;
+ }
+ }
+ }
+#else /* ifdef PRINTING */
+
+ if (can_check_termination) {
+ // Update information and compute also objective value
+ update_info(work, iter, compute_cost_function, 0);
+
+ // Check algorithm termination
+ if (check_termination(work, 0)) {
+ // Terminate algorithm
+ break;
+ }
+ }
+#endif /* ifdef PRINTING */
+
+
+#if EMBEDDED != 1
+# ifdef PROFILING
+
+ // If adaptive rho with automatic interval, check if the solve time is a
+ // certain fraction
+ // of the setup time.
+ if (work->settings->adaptive_rho && !work->settings->adaptive_rho_interval) {
+ // Check time
+ if (osqp_toc(work->timer) >
+ work->settings->adaptive_rho_fraction * work->info->setup_time) {
+ // Enough time has passed. We now get the number of iterations between
+ // the updates.
+ if (work->settings->check_termination) {
+ // If check_termination is enabled, we round the number of iterations
+ // between
+ // rho updates to the closest multiple of check_termination
+ work->settings->adaptive_rho_interval = (c_int)c_roundmultiple(iter,
+ work->settings->check_termination);
+ } else {
+ // If check_termination is disabled, we round the number of iterations
+ // between
+ // updates to the closest multiple of the default check_termination
+ // interval.
+ work->settings->adaptive_rho_interval = (c_int)c_roundmultiple(iter,
+ CHECK_TERMINATION);
+ }
+
+ // Make sure the interval is not 0 and at least check_termination times
+ work->settings->adaptive_rho_interval = c_max(
+ work->settings->adaptive_rho_interval,
+ work->settings->check_termination);
+ } // If time condition is met
+ } // If adaptive rho enabled and interval set to auto
+# else // PROFILING
+ if (work->settings->adaptive_rho && !work->settings->adaptive_rho_interval) {
+ // Set adaptive_rho_interval to constant value
+ if (work->settings->check_termination) {
+ // If check_termination is enabled, we set it to a multiple of the check
+ // termination interval
+ work->settings->adaptive_rho_interval = ADAPTIVE_RHO_MULTIPLE_TERMINATION *
+ work->settings->check_termination;
+ } else {
+ // If check_termination is disabled we set it to a predefined fix number
+ work->settings->adaptive_rho_interval = ADAPTIVE_RHO_FIXED;
+ }
+ }
+# endif /* ifdef PROFILING */
+
+ // Adapt rho
+ if (work->settings->adaptive_rho &&
+ work->settings->adaptive_rho_interval &&
+ (iter % work->settings->adaptive_rho_interval == 0)) {
+ // Update info with the residuals if it hasn't been done before
+# ifdef PRINTING
+
+ if (!can_check_termination && !can_print) {
+ // Information has not been computed neither for termination or printing
+ // reasons
+ update_info(work, iter, compute_cost_function, 0);
+ }
+# else /* ifdef PRINTING */
+
+ if (!can_check_termination) {
+ // Information has not been computed before for termination check
+ update_info(work, iter, compute_cost_function, 0);
+ }
+# endif /* ifdef PRINTING */
+
+ // Actually update rho
+ if (adapt_rho(work)) {
+# ifdef PRINTING
+ c_eprint("Failed rho update");
+# endif // PRINTING
+ exitflag = 1;
+ goto exit;
+ }
+ }
+#endif // EMBEDDED != 1
+
+ } // End of ADMM for loop
+
+
+ // Update information and check termination condition if it hasn't been done
+ // during last iteration (max_iter reached or check_termination disabled)
+ if (!can_check_termination) {
+ /* Update information */
+#ifdef PRINTING
+
+ if (!can_print) {
+ // Update info only if it hasn't been updated before for printing
+ // reasons
+ update_info(work, iter - 1, compute_cost_function, 0);
+ }
+#else /* ifdef PRINTING */
+
+ // If no printing is enabled, update info directly
+ update_info(work, iter - 1, compute_cost_function, 0);
+#endif /* ifdef PRINTING */
+
+#ifdef PRINTING
+
+ /* Print summary */
+ if (work->settings->verbose && !work->summary_printed) print_summary(work);
+#endif /* ifdef PRINTING */
+
+ /* Check whether a termination criterion is triggered */
+ check_termination(work, 0);
+ }
+
+ // Compute objective value in case it was not
+ // computed during the iterations
+ if (!compute_cost_function && has_solution(work->info)){
+ work->info->obj_val = compute_obj_val(work, work->x);
+ }
+
+
+#ifdef PRINTING
+ /* Print summary for last iteration */
+ if (work->settings->verbose && !work->summary_printed) {
+ print_summary(work);
+ }
+#endif /* ifdef PRINTING */
+
+ /* if max iterations reached, change status accordingly */
+ if (work->info->status_val == OSQP_UNSOLVED) {
+ if (!check_termination(work, 1)) { // Try to check for approximate
+ update_status(work->info, OSQP_MAX_ITER_REACHED);
+ }
+ }
+
+#ifdef PROFILING
+ /* if time-limit reached check termination and update status accordingly */
+ if (work->info->status_val == OSQP_TIME_LIMIT_REACHED) {
+ if (!check_termination(work, 1)) { // Try for approximate solutions
+ update_status(work->info, OSQP_TIME_LIMIT_REACHED); /* Change update status back to OSQP_TIME_LIMIT_REACHED */
+ }
+ }
+#endif /* ifdef PROFILING */
+
+
+#if EMBEDDED != 1
+ /* Update rho estimate */
+ work->info->rho_estimate = compute_rho_estimate(work);
+#endif /* if EMBEDDED != 1 */
+
+ /* Update solve time */
+#ifdef PROFILING
+ work->info->solve_time = osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+
+#ifndef EMBEDDED
+ // Polish the obtained solution
+ if (work->settings->polish && (work->info->status_val == OSQP_SOLVED))
+ polish(work);
+#endif /* ifndef EMBEDDED */
+
+#ifdef PROFILING
+ /* Update total time */
+ if (work->first_run) {
+ // total time: setup + solve + polish
+ work->info->run_time = work->info->setup_time +
+ work->info->solve_time +
+ work->info->polish_time;
+ } else {
+ // total time: update + solve + polish
+ work->info->run_time = work->info->update_time +
+ work->info->solve_time +
+ work->info->polish_time;
+ }
+
+ // Indicate that the solve function has already been executed
+ if (work->first_run) work->first_run = 0;
+
+ // Indicate that the update_time should be set to zero
+ work->clear_update_time = 1;
+
+ // Indicate that osqp_update_rho is not called from osqp_solve
+ work->rho_update_from_solve = 0;
+#endif /* ifdef PROFILING */
+
+#ifdef PRINTING
+ /* Print final footer */
+ if (work->settings->verbose) print_footer(work->info, work->settings->polish);
+#endif /* ifdef PRINTING */
+
+ // Store solution
+ store_solution(work);
+
+
+// Define exit flag for quitting function
+#if defined(PROFILING) || defined(CTRLC) || EMBEDDED != 1
+exit:
+#endif /* if defined(PROFILING) || defined(CTRLC) || EMBEDDED != 1 */
+
+#ifdef CTRLC
+ // Restore previous signal handler
+ osqp_end_interrupt_listener();
+#endif /* ifdef CTRLC */
+
+ return exitflag;
+}
+
+
+#ifndef EMBEDDED
+
+c_int osqp_cleanup(OSQPWorkspace *work) {
+ c_int exitflag = 0;
+
+ if (work) { // If workspace has been allocated
+ // Free Data
+ if (work->data) {
+ if (work->data->P) csc_spfree(work->data->P);
+ if (work->data->A) csc_spfree(work->data->A);
+ if (work->data->q) c_free(work->data->q);
+ if (work->data->l) c_free(work->data->l);
+ if (work->data->u) c_free(work->data->u);
+ c_free(work->data);
+ }
+
+ // Free scaling variables
+ if (work->scaling){
+ if (work->scaling->D) c_free(work->scaling->D);
+ if (work->scaling->Dinv) c_free(work->scaling->Dinv);
+ if (work->scaling->E) c_free(work->scaling->E);
+ if (work->scaling->Einv) c_free(work->scaling->Einv);
+ c_free(work->scaling);
+ }
+
+ // Free temp workspace variables for scaling
+ if (work->D_temp) c_free(work->D_temp);
+ if (work->D_temp_A) c_free(work->D_temp_A);
+ if (work->E_temp) c_free(work->E_temp);
+
+ // Free linear system solver structure
+ if (work->linsys_solver) {
+ if (work->linsys_solver->free) {
+ work->linsys_solver->free(work->linsys_solver);
+ }
+ }
+
+ // Unload linear system solver after free
+ if (work->settings) {
+ exitflag = unload_linsys_solver(work->settings->linsys_solver);
+ }
+
+#ifndef EMBEDDED
+ // Free active constraints structure
+ if (work->pol) {
+ if (work->pol->Alow_to_A) c_free(work->pol->Alow_to_A);
+ if (work->pol->Aupp_to_A) c_free(work->pol->Aupp_to_A);
+ if (work->pol->A_to_Alow) c_free(work->pol->A_to_Alow);
+ if (work->pol->A_to_Aupp) c_free(work->pol->A_to_Aupp);
+ if (work->pol->x) c_free(work->pol->x);
+ if (work->pol->z) c_free(work->pol->z);
+ if (work->pol->y) c_free(work->pol->y);
+ c_free(work->pol);
+ }
+#endif /* ifndef EMBEDDED */
+
+ // Free other Variables
+ if (work->rho_vec) c_free(work->rho_vec);
+ if (work->rho_inv_vec) c_free(work->rho_inv_vec);
+#if EMBEDDED != 1
+ if (work->constr_type) c_free(work->constr_type);
+#endif
+ if (work->x) c_free(work->x);
+ if (work->z) c_free(work->z);
+ if (work->xz_tilde) c_free(work->xz_tilde);
+ if (work->x_prev) c_free(work->x_prev);
+ if (work->z_prev) c_free(work->z_prev);
+ if (work->y) c_free(work->y);
+ if (work->Ax) c_free(work->Ax);
+ if (work->Px) c_free(work->Px);
+ if (work->Aty) c_free(work->Aty);
+ if (work->delta_y) c_free(work->delta_y);
+ if (work->Atdelta_y) c_free(work->Atdelta_y);
+ if (work->delta_x) c_free(work->delta_x);
+ if (work->Pdelta_x) c_free(work->Pdelta_x);
+ if (work->Adelta_x) c_free(work->Adelta_x);
+
+ // Free Settings
+ if (work->settings) c_free(work->settings);
+
+ // Free solution
+ if (work->solution) {
+ if (work->solution->x) c_free(work->solution->x);
+ if (work->solution->y) c_free(work->solution->y);
+ c_free(work->solution);
+ }
+
+ // Free information
+ if (work->info) c_free(work->info);
+
+# ifdef PROFILING
+ // Free timer
+ if (work->timer) c_free(work->timer);
+# endif /* ifdef PROFILING */
+
+ // Free work
+ c_free(work);
+ }
+
+ return exitflag;
+}
+
+#endif // #ifndef EMBEDDED
+
+
+/************************
+* Update problem data *
+************************/
+c_int osqp_update_lin_cost(OSQPWorkspace *work, const c_float *q_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+#ifdef PROFILING
+ if (work->clear_update_time == 1) {
+ work->clear_update_time = 0;
+ work->info->update_time = 0.0;
+ }
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+ // Replace q by the new vector
+ prea_vec_copy(q_new, work->data->q, work->data->n);
+
+ // Scaling
+ if (work->settings->scaling) {
+ vec_ew_prod(work->scaling->D, work->data->q, work->data->q, work->data->n);
+ vec_mult_scalar(work->data->q, work->scaling->c, work->data->n);
+ }
+
+ // Reset solver information
+ reset_info(work->info);
+
+#ifdef PROFILING
+ work->info->update_time += osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+ return 0;
+}
+
+c_int osqp_update_bounds(OSQPWorkspace *work,
+ const c_float *l_new,
+ const c_float *u_new) {
+ c_int i, exitflag = 0;
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+#ifdef PROFILING
+ if (work->clear_update_time == 1) {
+ work->clear_update_time = 0;
+ work->info->update_time = 0.0;
+ }
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+ // Check if lower bound is smaller than upper bound
+ for (i = 0; i < work->data->m; i++) {
+ if (l_new[i] > u_new[i]) {
+#ifdef PRINTING
+ c_eprint("lower bound must be lower than or equal to upper bound");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+ }
+
+ // Replace l and u by the new vectors
+ prea_vec_copy(l_new, work->data->l, work->data->m);
+ prea_vec_copy(u_new, work->data->u, work->data->m);
+
+ // Scaling
+ if (work->settings->scaling) {
+ vec_ew_prod(work->scaling->E, work->data->l, work->data->l, work->data->m);
+ vec_ew_prod(work->scaling->E, work->data->u, work->data->u, work->data->m);
+ }
+
+ // Reset solver information
+ reset_info(work->info);
+
+#if EMBEDDED != 1
+ // Update rho_vec and refactor if constraints type changes
+ exitflag = update_rho_vec(work);
+#endif // EMBEDDED != 1
+
+#ifdef PROFILING
+ work->info->update_time += osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+ return exitflag;
+}
+
+c_int osqp_update_lower_bound(OSQPWorkspace *work, const c_float *l_new) {
+ c_int i, exitflag = 0;
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+#ifdef PROFILING
+ if (work->clear_update_time == 1) {
+ work->clear_update_time = 0;
+ work->info->update_time = 0.0;
+ }
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+ // Replace l by the new vector
+ prea_vec_copy(l_new, work->data->l, work->data->m);
+
+ // Scaling
+ if (work->settings->scaling) {
+ vec_ew_prod(work->scaling->E, work->data->l, work->data->l, work->data->m);
+ }
+
+ // Check if lower bound is smaller than upper bound
+ for (i = 0; i < work->data->m; i++) {
+ if (work->data->l[i] > work->data->u[i]) {
+#ifdef PRINTING
+ c_eprint("upper bound must be greater than or equal to lower bound");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+ }
+
+ // Reset solver information
+ reset_info(work->info);
+
+#if EMBEDDED != 1
+ // Update rho_vec and refactor if constraints type changes
+ exitflag = update_rho_vec(work);
+#endif // EMBEDDED ! =1
+
+#ifdef PROFILING
+ work->info->update_time += osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+ return exitflag;
+}
+
+c_int osqp_update_upper_bound(OSQPWorkspace *work, const c_float *u_new) {
+ c_int i, exitflag = 0;
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+#ifdef PROFILING
+ if (work->clear_update_time == 1) {
+ work->clear_update_time = 0;
+ work->info->update_time = 0.0;
+ }
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+ // Replace u by the new vector
+ prea_vec_copy(u_new, work->data->u, work->data->m);
+
+ // Scaling
+ if (work->settings->scaling) {
+ vec_ew_prod(work->scaling->E, work->data->u, work->data->u, work->data->m);
+ }
+
+ // Check if upper bound is greater than lower bound
+ for (i = 0; i < work->data->m; i++) {
+ if (work->data->u[i] < work->data->l[i]) {
+#ifdef PRINTING
+ c_eprint("lower bound must be lower than or equal to upper bound");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+ }
+
+ // Reset solver information
+ reset_info(work->info);
+
+#if EMBEDDED != 1
+ // Update rho_vec and refactor if constraints type changes
+ exitflag = update_rho_vec(work);
+#endif // EMBEDDED != 1
+
+#ifdef PROFILING
+ work->info->update_time += osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+ return exitflag;
+}
+
+c_int osqp_warm_start(OSQPWorkspace *work, const c_float *x, const c_float *y) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Update warm_start setting to true
+ if (!work->settings->warm_start) work->settings->warm_start = 1;
+
+ // Copy primal and dual variables into the iterates
+ prea_vec_copy(x, work->x, work->data->n);
+ prea_vec_copy(y, work->y, work->data->m);
+
+ // Scale iterates
+ if (work->settings->scaling) {
+ vec_ew_prod(work->scaling->Dinv, work->x, work->x, work->data->n);
+ vec_ew_prod(work->scaling->Einv, work->y, work->y, work->data->m);
+ vec_mult_scalar(work->y, work->scaling->c, work->data->m);
+ }
+
+ // Compute Ax = z and store it in z
+ mat_vec(work->data->A, work->x, work->z, 0);
+
+ return 0;
+}
+
+c_int osqp_warm_start_x(OSQPWorkspace *work, const c_float *x) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Update warm_start setting to true
+ if (!work->settings->warm_start) work->settings->warm_start = 1;
+
+ // Copy primal variable into the iterate x
+ prea_vec_copy(x, work->x, work->data->n);
+
+ // Scale iterate
+ if (work->settings->scaling) {
+ vec_ew_prod(work->scaling->Dinv, work->x, work->x, work->data->n);
+ }
+
+ // Compute Ax = z and store it in z
+ mat_vec(work->data->A, work->x, work->z, 0);
+
+ return 0;
+}
+
+c_int osqp_warm_start_y(OSQPWorkspace *work, const c_float *y) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Update warm_start setting to true
+ if (!work->settings->warm_start) work->settings->warm_start = 1;
+
+ // Copy dual variable into the iterate y
+ prea_vec_copy(y, work->y, work->data->m);
+
+ // Scale iterate
+ if (work->settings->scaling) {
+ vec_ew_prod(work->scaling->Einv, work->y, work->y, work->data->m);
+ vec_mult_scalar(work->y, work->scaling->c, work->data->m);
+ }
+
+ return 0;
+}
+
+
+#if EMBEDDED != 1
+
+c_int osqp_update_P(OSQPWorkspace *work,
+ const c_float *Px_new,
+ const c_int *Px_new_idx,
+ c_int P_new_n) {
+ c_int i; // For indexing
+ c_int exitflag; // Exit flag
+ c_int nnzP; // Number of nonzeros in P
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+#ifdef PROFILING
+ if (work->clear_update_time == 1) {
+ work->clear_update_time = 0;
+ work->info->update_time = 0.0;
+ }
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+ nnzP = work->data->P->p[work->data->P->n];
+
+ if (Px_new_idx) { // Passing the index of elements changed
+ // Check if number of elements is less or equal than the total number of
+ // nonzeros in P
+ if (P_new_n > nnzP) {
+# ifdef PRINTING
+ c_eprint("new number of elements (%i) greater than elements in P (%i)",
+ (int)P_new_n,
+ (int)nnzP);
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+ }
+
+ if (work->settings->scaling) {
+ // Unscale data
+ unscale_data(work);
+ }
+
+ // Update P elements
+ if (Px_new_idx) { // Change only Px_new_idx
+ for (i = 0; i < P_new_n; i++) {
+ work->data->P->x[Px_new_idx[i]] = Px_new[i];
+ }
+ }
+ else // Change whole P
+ {
+ for (i = 0; i < nnzP; i++) {
+ work->data->P->x[i] = Px_new[i];
+ }
+ }
+
+ if (work->settings->scaling) {
+ // Scale data
+ scale_data(work);
+ }
+
+ // Update linear system structure with new data
+ exitflag = work->linsys_solver->update_matrices(work->linsys_solver,
+ work->data->P,
+ work->data->A);
+
+ // Reset solver information
+ reset_info(work->info);
+
+# ifdef PRINTING
+
+ if (exitflag < 0) {
+ c_eprint("new KKT matrix is not quasidefinite");
+ }
+# endif /* ifdef PRINTING */
+
+#ifdef PROFILING
+ work->info->update_time += osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+ return exitflag;
+}
+
+
+c_int osqp_update_A(OSQPWorkspace *work,
+ const c_float *Ax_new,
+ const c_int *Ax_new_idx,
+ c_int A_new_n) {
+ c_int i; // For indexing
+ c_int exitflag; // Exit flag
+ c_int nnzA; // Number of nonzeros in A
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+#ifdef PROFILING
+ if (work->clear_update_time == 1) {
+ work->clear_update_time = 0;
+ work->info->update_time = 0.0;
+ }
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+ nnzA = work->data->A->p[work->data->A->n];
+
+ if (Ax_new_idx) { // Passing the index of elements changed
+ // Check if number of elements is less or equal than the total number of
+ // nonzeros in A
+ if (A_new_n > nnzA) {
+# ifdef PRINTING
+ c_eprint("new number of elements (%i) greater than elements in A (%i)",
+ (int)A_new_n,
+ (int)nnzA);
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+ }
+
+ if (work->settings->scaling) {
+ // Unscale data
+ unscale_data(work);
+ }
+
+ // Update A elements
+ if (Ax_new_idx) { // Change only Ax_new_idx
+ for (i = 0; i < A_new_n; i++) {
+ work->data->A->x[Ax_new_idx[i]] = Ax_new[i];
+ }
+ }
+ else { // Change whole A
+ for (i = 0; i < nnzA; i++) {
+ work->data->A->x[i] = Ax_new[i];
+ }
+ }
+
+ if (work->settings->scaling) {
+ // Scale data
+ scale_data(work);
+ }
+
+ // Update linear system structure with new data
+ exitflag = work->linsys_solver->update_matrices(work->linsys_solver,
+ work->data->P,
+ work->data->A);
+
+ // Reset solver information
+ reset_info(work->info);
+
+# ifdef PRINTING
+
+ if (exitflag < 0) {
+ c_eprint("new KKT matrix is not quasidefinite");
+ }
+# endif /* ifdef PRINTING */
+
+#ifdef PROFILING
+ work->info->update_time += osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+ return exitflag;
+}
+
+
+c_int osqp_update_P_A(OSQPWorkspace *work,
+ const c_float *Px_new,
+ const c_int *Px_new_idx,
+ c_int P_new_n,
+ const c_float *Ax_new,
+ const c_int *Ax_new_idx,
+ c_int A_new_n) {
+ c_int i; // For indexing
+ c_int exitflag; // Exit flag
+ c_int nnzP, nnzA; // Number of nonzeros in P and A
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+#ifdef PROFILING
+ if (work->clear_update_time == 1) {
+ work->clear_update_time = 0;
+ work->info->update_time = 0.0;
+ }
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+ nnzP = work->data->P->p[work->data->P->n];
+ nnzA = work->data->A->p[work->data->A->n];
+
+
+ if (Px_new_idx) { // Passing the index of elements changed
+ // Check if number of elements is less or equal than the total number of
+ // nonzeros in P
+ if (P_new_n > nnzP) {
+# ifdef PRINTING
+ c_eprint("new number of elements (%i) greater than elements in P (%i)",
+ (int)P_new_n,
+ (int)nnzP);
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+ }
+
+
+ if (Ax_new_idx) { // Passing the index of elements changed
+ // Check if number of elements is less or equal than the total number of
+ // nonzeros in A
+ if (A_new_n > nnzA) {
+# ifdef PRINTING
+ c_eprint("new number of elements (%i) greater than elements in A (%i)",
+ (int)A_new_n,
+ (int)nnzA);
+# endif /* ifdef PRINTING */
+ return 2;
+ }
+ }
+
+ if (work->settings->scaling) {
+ // Unscale data
+ unscale_data(work);
+ }
+
+ // Update P elements
+ if (Px_new_idx) { // Change only Px_new_idx
+ for (i = 0; i < P_new_n; i++) {
+ work->data->P->x[Px_new_idx[i]] = Px_new[i];
+ }
+ }
+ else // Change whole P
+ {
+ for (i = 0; i < nnzP; i++) {
+ work->data->P->x[i] = Px_new[i];
+ }
+ }
+
+ // Update A elements
+ if (Ax_new_idx) { // Change only Ax_new_idx
+ for (i = 0; i < A_new_n; i++) {
+ work->data->A->x[Ax_new_idx[i]] = Ax_new[i];
+ }
+ }
+ else { // Change whole A
+ for (i = 0; i < nnzA; i++) {
+ work->data->A->x[i] = Ax_new[i];
+ }
+ }
+
+ if (work->settings->scaling) {
+ // Scale data
+ scale_data(work);
+ }
+
+ // Update linear system structure with new data
+ exitflag = work->linsys_solver->update_matrices(work->linsys_solver,
+ work->data->P,
+ work->data->A);
+
+ // Reset solver information
+ reset_info(work->info);
+
+# ifdef PRINTING
+
+ if (exitflag < 0) {
+ c_eprint("new KKT matrix is not quasidefinite");
+ }
+# endif /* ifdef PRINTING */
+
+#ifdef PROFILING
+ work->info->update_time += osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+ return exitflag;
+}
+
+c_int osqp_update_rho(OSQPWorkspace *work, c_float rho_new) {
+ c_int exitflag, i;
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check value of rho
+ if (rho_new <= 0) {
+# ifdef PRINTING
+ c_eprint("rho must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+#ifdef PROFILING
+ if (work->rho_update_from_solve == 0) {
+ if (work->clear_update_time == 1) {
+ work->clear_update_time = 0;
+ work->info->update_time = 0.0;
+ }
+ osqp_tic(work->timer); // Start timer
+ }
+#endif /* ifdef PROFILING */
+
+ // Update rho in settings
+ work->settings->rho = c_min(c_max(rho_new, RHO_MIN), RHO_MAX);
+
+ // Update rho_vec and rho_inv_vec
+ for (i = 0; i < work->data->m; i++) {
+ if (work->constr_type[i] == 0) {
+ // Inequalities
+ work->rho_vec[i] = work->settings->rho;
+ work->rho_inv_vec[i] = 1. / work->settings->rho;
+ }
+ else if (work->constr_type[i] == 1) {
+ // Equalities
+ work->rho_vec[i] = RHO_EQ_OVER_RHO_INEQ * work->settings->rho;
+ work->rho_inv_vec[i] = 1. / work->rho_vec[i];
+ }
+ }
+
+ // Update rho_vec in KKT matrix
+ exitflag = work->linsys_solver->update_rho_vec(work->linsys_solver,
+ work->rho_vec);
+
+#ifdef PROFILING
+ if (work->rho_update_from_solve == 0)
+ work->info->update_time += osqp_toc(work->timer);
+#endif /* ifdef PROFILING */
+
+ return exitflag;
+}
+
+#endif // EMBEDDED != 1
+
+/****************************
+* Update problem settings *
+****************************/
+c_int osqp_update_max_iter(OSQPWorkspace *work, c_int max_iter_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that max_iter is positive
+ if (max_iter_new <= 0) {
+#ifdef PRINTING
+ c_eprint("max_iter must be positive");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update max_iter
+ work->settings->max_iter = max_iter_new;
+
+ return 0;
+}
+
+c_int osqp_update_eps_abs(OSQPWorkspace *work, c_float eps_abs_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that eps_abs is positive
+ if (eps_abs_new < 0.) {
+#ifdef PRINTING
+ c_eprint("eps_abs must be nonnegative");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update eps_abs
+ work->settings->eps_abs = eps_abs_new;
+
+ return 0;
+}
+
+c_int osqp_update_eps_rel(OSQPWorkspace *work, c_float eps_rel_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that eps_rel is positive
+ if (eps_rel_new < 0.) {
+#ifdef PRINTING
+ c_eprint("eps_rel must be nonnegative");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update eps_rel
+ work->settings->eps_rel = eps_rel_new;
+
+ return 0;
+}
+
+c_int osqp_update_eps_prim_inf(OSQPWorkspace *work, c_float eps_prim_inf_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that eps_prim_inf is positive
+ if (eps_prim_inf_new < 0.) {
+#ifdef PRINTING
+ c_eprint("eps_prim_inf must be nonnegative");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update eps_prim_inf
+ work->settings->eps_prim_inf = eps_prim_inf_new;
+
+ return 0;
+}
+
+c_int osqp_update_eps_dual_inf(OSQPWorkspace *work, c_float eps_dual_inf_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that eps_dual_inf is positive
+ if (eps_dual_inf_new < 0.) {
+#ifdef PRINTING
+ c_eprint("eps_dual_inf must be nonnegative");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update eps_dual_inf
+ work->settings->eps_dual_inf = eps_dual_inf_new;
+
+
+ return 0;
+}
+
+c_int osqp_update_alpha(OSQPWorkspace *work, c_float alpha_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that alpha is between 0 and 2
+ if ((alpha_new <= 0.) || (alpha_new >= 2.)) {
+#ifdef PRINTING
+ c_eprint("alpha must be between 0 and 2");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update alpha
+ work->settings->alpha = alpha_new;
+
+ return 0;
+}
+
+c_int osqp_update_warm_start(OSQPWorkspace *work, c_int warm_start_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that warm_start is either 0 or 1
+ if ((warm_start_new != 0) && (warm_start_new != 1)) {
+#ifdef PRINTING
+ c_eprint("warm_start should be either 0 or 1");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update warm_start
+ work->settings->warm_start = warm_start_new;
+
+ return 0;
+}
+
+c_int osqp_update_scaled_termination(OSQPWorkspace *work, c_int scaled_termination_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that scaled_termination is either 0 or 1
+ if ((scaled_termination_new != 0) && (scaled_termination_new != 1)) {
+#ifdef PRINTING
+ c_eprint("scaled_termination should be either 0 or 1");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update scaled_termination
+ work->settings->scaled_termination = scaled_termination_new;
+
+ return 0;
+}
+
+c_int osqp_update_check_termination(OSQPWorkspace *work, c_int check_termination_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that check_termination is nonnegative
+ if (check_termination_new < 0) {
+#ifdef PRINTING
+ c_eprint("check_termination should be nonnegative");
+#endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update check_termination
+ work->settings->check_termination = check_termination_new;
+
+ return 0;
+}
+
+#ifndef EMBEDDED
+
+c_int osqp_update_delta(OSQPWorkspace *work, c_float delta_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that delta is positive
+ if (delta_new <= 0.) {
+# ifdef PRINTING
+ c_eprint("delta must be positive");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update delta
+ work->settings->delta = delta_new;
+
+ return 0;
+}
+
+c_int osqp_update_polish(OSQPWorkspace *work, c_int polish_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that polish is either 0 or 1
+ if ((polish_new != 0) && (polish_new != 1)) {
+# ifdef PRINTING
+ c_eprint("polish should be either 0 or 1");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update polish
+ work->settings->polish = polish_new;
+
+# ifdef PROFILING
+
+ // Reset polish time to zero
+ work->info->polish_time = 0.0;
+# endif /* ifdef PROFILING */
+
+ return 0;
+}
+
+c_int osqp_update_polish_refine_iter(OSQPWorkspace *work, c_int polish_refine_iter_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that polish_refine_iter is nonnegative
+ if (polish_refine_iter_new < 0) {
+# ifdef PRINTING
+ c_eprint("polish_refine_iter must be nonnegative");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update polish_refine_iter
+ work->settings->polish_refine_iter = polish_refine_iter_new;
+
+ return 0;
+}
+
+c_int osqp_update_verbose(OSQPWorkspace *work, c_int verbose_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that verbose is either 0 or 1
+ if ((verbose_new != 0) && (verbose_new != 1)) {
+# ifdef PRINTING
+ c_eprint("verbose should be either 0 or 1");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update verbose
+ work->settings->verbose = verbose_new;
+
+ return 0;
+}
+
+#endif // EMBEDDED
+
+#ifdef PROFILING
+
+c_int osqp_update_time_limit(OSQPWorkspace *work, c_float time_limit_new) {
+
+ // Check if workspace has been initialized
+ if (!work) return osqp_error(OSQP_WORKSPACE_NOT_INIT_ERROR);
+
+ // Check that time_limit is nonnegative
+ if (time_limit_new < 0.) {
+# ifdef PRINTING
+ c_print("time_limit must be nonnegative\n");
+# endif /* ifdef PRINTING */
+ return 1;
+ }
+
+ // Update time_limit
+ work->settings->time_limit = time_limit_new;
+
+ return 0;
+}
+#endif /* ifdef PROFILING */
diff --git a/src/polish.c b/src/polish.c
new file mode 100644
index 0000000..55df72d
--- /dev/null
+++ b/src/polish.c
@@ -0,0 +1,350 @@
+#include "polish.h"
+#include "lin_alg.h"
+#include "util.h"
+#include "auxil.h"
+#include "lin_sys.h"
+#include "kkt.h"
+#include "proj.h"
+#include "error.h"
+
+/**
+ * Form reduced matrix A that contains only rows that are active at the
+ * solution.
+ * Ared = vstack[Alow, Aupp]
+ * Active constraints are guessed from the primal and dual solution returned by
+ * the ADMM.
+ * @param work Workspace
+ * @return Number of rows in Ared, negative if error
+ */
+static c_int form_Ared(OSQPWorkspace *work) {
+ c_int j, ptr;
+ c_int Ared_nnz = 0;
+
+ // Initialize counters for active constraints
+ work->pol->n_low = 0;
+ work->pol->n_upp = 0;
+
+ /* Guess which linear constraints are lower-active, upper-active and free
+ * A_to_Alow[j] = -1 (if j-th row of A is not inserted in Alow)
+ * A_to_Alow[j] = i (if j-th row of A is inserted at i-th row of Alow)
+ * Aupp is formed in the equivalent way.
+ * Ared is formed by stacking vertically Alow and Aupp.
+ */
+ for (j = 0; j < work->data->m; j++) {
+ if (work->z[j] - work->data->l[j] < -work->y[j]) { // lower-active
+ work->pol->Alow_to_A[work->pol->n_low] = j;
+ work->pol->A_to_Alow[j] = work->pol->n_low++;
+ } else {
+ work->pol->A_to_Alow[j] = -1;
+ }
+ }
+
+ for (j = 0; j < work->data->m; j++) {
+ if (work->data->u[j] - work->z[j] < work->y[j]) { // upper-active
+ work->pol->Aupp_to_A[work->pol->n_upp] = j;
+ work->pol->A_to_Aupp[j] = work->pol->n_upp++;
+ } else {
+ work->pol->A_to_Aupp[j] = -1;
+ }
+ }
+
+ // Check if there are no active constraints
+ if (work->pol->n_low + work->pol->n_upp == 0) {
+ // Form empty Ared
+ work->pol->Ared = csc_spalloc(0, work->data->n, 0, 1, 0);
+ if (!(work->pol->Ared)) return -1;
+ int_vec_set_scalar(work->pol->Ared->p, 0, work->data->n + 1);
+ return 0; // mred = 0
+ }
+
+ // Count number of elements in Ared
+ for (j = 0; j < work->data->A->p[work->data->A->n]; j++) {
+ if ((work->pol->A_to_Alow[work->data->A->i[j]] != -1) ||
+ (work->pol->A_to_Aupp[work->data->A->i[j]] != -1)) Ared_nnz++;
+ }
+
+ // Form Ared
+ // Ared = vstack[Alow, Aupp]
+ work->pol->Ared = csc_spalloc(work->pol->n_low + work->pol->n_upp,
+ work->data->n, Ared_nnz, 1, 0);
+ if (!(work->pol->Ared)) return -1;
+ Ared_nnz = 0; // counter
+
+ for (j = 0; j < work->data->n; j++) { // Cycle over columns of A
+ work->pol->Ared->p[j] = Ared_nnz;
+
+ for (ptr = work->data->A->p[j]; ptr < work->data->A->p[j + 1]; ptr++) {
+ // Cycle over elements in j-th column
+ if (work->pol->A_to_Alow[work->data->A->i[ptr]] != -1) {
+ // Lower-active rows of A
+ work->pol->Ared->i[Ared_nnz] =
+ work->pol->A_to_Alow[work->data->A->i[ptr]];
+ work->pol->Ared->x[Ared_nnz++] = work->data->A->x[ptr];
+ } else if (work->pol->A_to_Aupp[work->data->A->i[ptr]] != -1) {
+ // Upper-active rows of A
+ work->pol->Ared->i[Ared_nnz] = work->pol->A_to_Aupp[work->data->A->i[ptr]] \
+ + work->pol->n_low;
+ work->pol->Ared->x[Ared_nnz++] = work->data->A->x[ptr];
+ }
+ }
+ }
+
+ // Update the last element in Ared->p
+ work->pol->Ared->p[work->data->n] = Ared_nnz;
+
+ // Return number of rows in Ared
+ return work->pol->n_low + work->pol->n_upp;
+}
+
+/**
+ * Form reduced right-hand side rhs_red = vstack[-q, l_low, u_upp]
+ * @param work Workspace
+ * @param rhs right-hand-side
+ * @return reduced rhs
+ */
+static void form_rhs_red(OSQPWorkspace *work, c_float *rhs) {
+ c_int j;
+
+ // Form the rhs of the reduced KKT linear system
+ for (j = 0; j < work->data->n; j++) { // -q
+ rhs[j] = -work->data->q[j];
+ }
+
+ for (j = 0; j < work->pol->n_low; j++) { // l_low
+ rhs[work->data->n + j] = work->data->l[work->pol->Alow_to_A[j]];
+ }
+
+ for (j = 0; j < work->pol->n_upp; j++) { // u_upp
+ rhs[work->data->n + work->pol->n_low + j] =
+ work->data->u[work->pol->Aupp_to_A[j]];
+ }
+}
+
+/**
+ * Perform iterative refinement on the polished solution:
+ * (repeat)
+ * 1. (K + dK) * dz = b - K*z
+ * 2. z <- z + dz
+ * @param work Solver workspace
+ * @param p Private variable for solving linear system
+ * @param z Initial z value
+ * @param b RHS of the linear system
+ * @return Exitflag
+ */
+static c_int iterative_refinement(OSQPWorkspace *work,
+ LinSysSolver *p,
+ c_float *z,
+ c_float *b) {
+ c_int i, j, n;
+ c_float *rhs;
+
+ if (work->settings->polish_refine_iter > 0) {
+
+ // Assign dimension n
+ n = work->data->n + work->pol->Ared->m;
+
+ // Allocate rhs vector
+ rhs = (c_float *)c_malloc(sizeof(c_float) * n);
+
+ if (!rhs) {
+ return osqp_error(OSQP_MEM_ALLOC_ERROR);
+ } else {
+ for (i = 0; i < work->settings->polish_refine_iter; i++) {
+ // Form the RHS for the iterative refinement: b - K*z
+ prea_vec_copy(b, rhs, n);
+
+ // Upper Part: R^{n}
+ // -= Px (upper triang)
+ mat_vec(work->data->P, z, rhs, -1);
+
+ // -= Px (lower triang)
+ mat_tpose_vec(work->data->P, z, rhs, -1, 1);
+
+ // -= Ared'*y_red
+ mat_tpose_vec(work->pol->Ared, z + work->data->n, rhs, -1, 0);
+
+ // Lower Part: R^{m}
+ mat_vec(work->pol->Ared, z, rhs + work->data->n, -1);
+
+ // Solve linear system. Store solution in rhs
+ p->solve(p, rhs);
+
+ // Update solution
+ for (j = 0; j < n; j++) {
+ z[j] += rhs[j];
+ }
+ }
+ }
+ if (rhs) c_free(rhs);
+ }
+ return 0;
+}
+
+/**
+ * Compute dual variable y from yred
+ * @param work Workspace
+ * @param yred Dual variables associated to active constraints
+ */
+static void get_ypol_from_yred(OSQPWorkspace *work, c_float *yred) {
+ c_int j;
+
+ // If there are no active constraints
+ if (work->pol->n_low + work->pol->n_upp == 0) {
+ vec_set_scalar(work->pol->y, 0., work->data->m);
+ return;
+ }
+
+ // NB: yred = vstack[ylow, yupp]
+ for (j = 0; j < work->data->m; j++) {
+ if (work->pol->A_to_Alow[j] != -1) {
+ // lower-active
+ work->pol->y[j] = yred[work->pol->A_to_Alow[j]];
+ } else if (work->pol->A_to_Aupp[j] != -1) {
+ // upper-active
+ work->pol->y[j] = yred[work->pol->A_to_Aupp[j] + work->pol->n_low];
+ } else {
+ // inactive
+ work->pol->y[j] = 0.0;
+ }
+ }
+}
+
+c_int polish(OSQPWorkspace *work) {
+ c_int mred, polish_successful, exitflag;
+ c_float *rhs_red;
+ LinSysSolver *plsh;
+ c_float *pol_sol; // Polished solution
+
+#ifdef PROFILING
+ osqp_tic(work->timer); // Start timer
+#endif /* ifdef PROFILING */
+
+ // Form Ared by assuming the active constraints and store in work->pol->Ared
+ mred = form_Ared(work);
+ if (mred < 0) { // work->pol->red = OSQP_NULL
+ // Polishing failed
+ work->info->status_polish = -1;
+
+ return -1;
+ }
+
+ // Form and factorize reduced KKT
+ exitflag = init_linsys_solver(&plsh, work->data->P, work->pol->Ared,
+ work->settings->delta, OSQP_NULL,
+ work->settings->linsys_solver, 1);
+
+ if (exitflag) {
+ // Polishing failed
+ work->info->status_polish = -1;
+
+ // Memory clean-up
+ if (work->pol->Ared) csc_spfree(work->pol->Ared);
+
+ return 1;
+ }
+
+ // Form reduced right-hand side rhs_red
+ rhs_red = c_malloc(sizeof(c_float) * (work->data->n + mred));
+ if (!rhs_red) {
+ // Polishing failed
+ work->info->status_polish = -1;
+
+ // Memory clean-up
+ csc_spfree(work->pol->Ared);
+
+ return -1;
+ }
+ form_rhs_red(work, rhs_red);
+
+ pol_sol = vec_copy(rhs_red, work->data->n + mred);
+ if (!pol_sol) {
+ // Polishing failed
+ work->info->status_polish = -1;
+
+ // Memory clean-up
+ csc_spfree(work->pol->Ared);
+ c_free(rhs_red);
+
+ return -1;
+ }
+
+ // Solve the reduced KKT system
+ plsh->solve(plsh, pol_sol);
+
+ // Perform iterative refinement to compensate for the regularization error
+ exitflag = iterative_refinement(work, plsh, pol_sol, rhs_red);
+
+ if (exitflag) {
+ // Polishing failed
+ work->info->status_polish = -1;
+
+ // Memory clean-up
+ csc_spfree(work->pol->Ared);
+ c_free(rhs_red);
+ c_free(pol_sol);
+
+ return -1;
+ }
+
+ // Store the polished solution (x,z,y)
+ prea_vec_copy(pol_sol, work->pol->x, work->data->n); // pol->x
+ mat_vec(work->data->A, work->pol->x, work->pol->z, 0); // pol->z
+ get_ypol_from_yred(work, pol_sol + work->data->n); // pol->y
+
+ // Ensure (z,y) satisfies normal cone constraint
+ project_normalcone(work, work->pol->z, work->pol->y);
+
+ // Compute primal and dual residuals at the polished solution
+ update_info(work, 0, 1, 1);
+
+ // Check if polish was successful
+ polish_successful = (work->pol->pri_res < work->info->pri_res &&
+ work->pol->dua_res < work->info->dua_res) || // Residuals
+ // are
+ // reduced
+ (work->pol->pri_res < work->info->pri_res &&
+ work->info->dua_res < 1e-10) || // Dual
+ // residual
+ // already
+ // tiny
+ (work->pol->dua_res < work->info->dua_res &&
+ work->info->pri_res < 1e-10); // Primal
+ // residual
+ // already
+ // tiny
+
+ if (polish_successful) {
+ // Update solver information
+ work->info->obj_val = work->pol->obj_val;
+ work->info->pri_res = work->pol->pri_res;
+ work->info->dua_res = work->pol->dua_res;
+ work->info->status_polish = 1;
+
+ // Update (x, z, y) in ADMM iterations
+ // NB: z needed for warm starting
+ prea_vec_copy(work->pol->x, work->x, work->data->n);
+ prea_vec_copy(work->pol->z, work->z, work->data->m);
+ prea_vec_copy(work->pol->y, work->y, work->data->m);
+
+ // Print summary
+#ifdef PRINTING
+
+ if (work->settings->verbose) print_polish(work);
+#endif /* ifdef PRINTING */
+ } else { // Polishing failed
+ work->info->status_polish = -1;
+
+ // TODO: Try to find a better solution on the line connecting ADMM
+ // and polished solution
+ }
+
+ // Memory clean-up
+ plsh->free(plsh);
+
+ // Checks that they are not NULL are already performed earlier
+ csc_spfree(work->pol->Ared);
+ c_free(rhs_red);
+ c_free(pol_sol);
+
+ return 0;
+}
diff --git a/src/proj.c b/src/proj.c
new file mode 100644
index 0000000..c7fdb45
--- /dev/null
+++ b/src/proj.c
@@ -0,0 +1,29 @@
+#include "proj.h"
+
+
+void project(OSQPWorkspace *work, c_float *z) {
+ c_int i, m;
+
+ m = work->data->m;
+
+ for (i = 0; i < m; i++) {
+ z[i] = c_min(c_max(z[i],
+ work->data->l[i]), // Between lower
+ work->data->u[i]); // and upper bounds
+ }
+}
+
+void project_normalcone(OSQPWorkspace *work, c_float *z, c_float *y) {
+ c_int i, m;
+
+ // NB: Use z_prev as temporary vector
+
+ m = work->data->m;
+
+ for (i = 0; i < m; i++) {
+ work->z_prev[i] = z[i] + y[i];
+ z[i] = c_min(c_max(work->z_prev[i], work->data->l[i]),
+ work->data->u[i]);
+ y[i] = work->z_prev[i] - z[i];
+ }
+}
diff --git a/src/scaling.c b/src/scaling.c
new file mode 100644
index 0000000..74616a7
--- /dev/null
+++ b/src/scaling.c
@@ -0,0 +1,192 @@
+#include "scaling.h"
+
+#if EMBEDDED != 1
+
+
+// Set values lower than threshold SCALING_REG to 1
+void limit_scaling(c_float *D, c_int n) {
+ c_int i;
+
+ for (i = 0; i < n; i++) {
+ D[i] = D[i] < MIN_SCALING ? 1.0 : D[i];
+ D[i] = D[i] > MAX_SCALING ? MAX_SCALING : D[i];
+ }
+}
+
+/**
+ * Compute infinite norm of the columns of the KKT matrix without forming it
+ *
+ * The norm is stored in the vector v = (D, E)
+ *
+ * @param P Cost matrix
+ * @param A Constraints matrix
+ * @param D Norm of columns related to variables
+ * @param D_temp_A Temporary vector for norm of columns of A
+ * @param E Norm of columns related to constraints
+ * @param n Dimension of KKT matrix
+ */
+void compute_inf_norm_cols_KKT(const csc *P, const csc *A,
+ c_float *D, c_float *D_temp_A,
+ c_float *E, c_int n) {
+ // First half
+ // [ P ]
+ // [ A ]
+ mat_inf_norm_cols_sym_triu(P, D);
+ mat_inf_norm_cols(A, D_temp_A);
+ vec_ew_max_vec(D, D_temp_A, D, n);
+
+ // Second half
+ // [ A']
+ // [ 0 ]
+ mat_inf_norm_rows(A, E);
+}
+
+c_int scale_data(OSQPWorkspace *work) {
+ // Scale KKT matrix
+ //
+ // [ P A']
+ // [ A 0 ]
+ //
+ // with diagonal matrix
+ //
+ // S = [ D ]
+ // [ E ]
+ //
+
+ c_int i; // Iterations index
+ c_int n, m; // Number of constraints and variables
+ c_float c_temp; // Cost function scaling
+ c_float inf_norm_q; // Infinity norm of q
+
+ n = work->data->n;
+ m = work->data->m;
+
+ // Initialize scaling to 1
+ work->scaling->c = 1.0;
+ vec_set_scalar(work->scaling->D, 1., work->data->n);
+ vec_set_scalar(work->scaling->Dinv, 1., work->data->n);
+ vec_set_scalar(work->scaling->E, 1., work->data->m);
+ vec_set_scalar(work->scaling->Einv, 1., work->data->m);
+
+
+ for (i = 0; i < work->settings->scaling; i++) {
+ //
+ // First Ruiz step
+ //
+
+ // Compute norm of KKT columns
+ compute_inf_norm_cols_KKT(work->data->P, work->data->A,
+ work->D_temp, work->D_temp_A,
+ work->E_temp, n);
+
+ // Set to 1 values with 0 norms (avoid crazy scaling)
+ limit_scaling(work->D_temp, n);
+ limit_scaling(work->E_temp, m);
+
+ // Take square root of norms
+ vec_ew_sqrt(work->D_temp, n);
+ vec_ew_sqrt(work->E_temp, m);
+
+ // Divide scalings D and E by themselves
+ vec_ew_recipr(work->D_temp, work->D_temp, n);
+ vec_ew_recipr(work->E_temp, work->E_temp, m);
+
+ // Equilibrate matrices P and A and vector q
+ // P <- DPD
+ mat_premult_diag(work->data->P, work->D_temp);
+ mat_postmult_diag(work->data->P, work->D_temp);
+
+ // A <- EAD
+ mat_premult_diag(work->data->A, work->E_temp);
+ mat_postmult_diag(work->data->A, work->D_temp);
+
+ // q <- Dq
+ vec_ew_prod(work->D_temp, work->data->q, work->data->q, n);
+
+ // Update equilibration matrices D and E
+ vec_ew_prod(work->scaling->D, work->D_temp, work->scaling->D, n);
+ vec_ew_prod(work->scaling->E, work->E_temp, work->scaling->E, m);
+
+ //
+ // Cost normalization step
+ //
+
+ // Compute avg norm of cols of P
+ mat_inf_norm_cols_sym_triu(work->data->P, work->D_temp);
+ c_temp = vec_mean(work->D_temp, n);
+
+ // Compute inf norm of q
+ inf_norm_q = vec_norm_inf(work->data->q, n);
+
+ // If norm_q == 0, set it to 1 (ignore it in the scaling)
+ // NB: Using the same function as with vectors here
+ limit_scaling(&inf_norm_q, 1);
+
+ // Compute max between avg norm of cols of P and inf norm of q
+ c_temp = c_max(c_temp, inf_norm_q);
+
+ // Limit scaling (use same function as with vectors)
+ limit_scaling(&c_temp, 1);
+
+ // Invert scaling c = 1 / cost_measure
+ c_temp = 1. / c_temp;
+
+ // Scale P
+ mat_mult_scalar(work->data->P, c_temp);
+
+ // Scale q
+ vec_mult_scalar(work->data->q, c_temp, n);
+
+ // Update cost scaling
+ work->scaling->c *= c_temp;
+ }
+
+
+ // Store cinv, Dinv, Einv
+ work->scaling->cinv = 1. / work->scaling->c;
+ vec_ew_recipr(work->scaling->D, work->scaling->Dinv, work->data->n);
+ vec_ew_recipr(work->scaling->E, work->scaling->Einv, work->data->m);
+
+
+ // Scale problem vectors l, u
+ vec_ew_prod(work->scaling->E, work->data->l, work->data->l, work->data->m);
+ vec_ew_prod(work->scaling->E, work->data->u, work->data->u, work->data->m);
+
+ return 0;
+}
+
+#endif // EMBEDDED
+
+c_int unscale_data(OSQPWorkspace *work) {
+ // Unscale cost
+ mat_mult_scalar(work->data->P, work->scaling->cinv);
+ mat_premult_diag(work->data->P, work->scaling->Dinv);
+ mat_postmult_diag(work->data->P, work->scaling->Dinv);
+ vec_mult_scalar(work->data->q, work->scaling->cinv, work->data->n);
+ vec_ew_prod(work->scaling->Dinv, work->data->q, work->data->q, work->data->n);
+
+ // Unscale constraints
+ mat_premult_diag(work->data->A, work->scaling->Einv);
+ mat_postmult_diag(work->data->A, work->scaling->Dinv);
+ vec_ew_prod(work->scaling->Einv, work->data->l, work->data->l, work->data->m);
+ vec_ew_prod(work->scaling->Einv, work->data->u, work->data->u, work->data->m);
+
+ return 0;
+}
+
+c_int unscale_solution(OSQPWorkspace *work) {
+ // primal
+ vec_ew_prod(work->scaling->D,
+ work->solution->x,
+ work->solution->x,
+ work->data->n);
+
+ // dual
+ vec_ew_prod(work->scaling->E,
+ work->solution->y,
+ work->solution->y,
+ work->data->m);
+ vec_mult_scalar(work->solution->y, work->scaling->cinv, work->data->m);
+
+ return 0;
+}
diff --git a/src/util.c b/src/util.c
new file mode 100644
index 0000000..8e8d04a
--- /dev/null
+++ b/src/util.c
@@ -0,0 +1,487 @@
+#include "util.h"
+
+/***************
+* Versioning *
+***************/
+const char* osqp_version(void) {
+ return OSQP_VERSION;
+}
+
+/************************************
+* Printing Constants to set Layout *
+************************************/
+#ifdef PRINTING
+# define HEADER_LINE_LEN 65
+#endif /* ifdef PRINTING */
+
+/**********************
+* Utility Functions *
+**********************/
+void c_strcpy(char dest[], const char source[]) {
+ int i = 0;
+
+ while (1) {
+ dest[i] = source[i];
+
+ if (dest[i] == '\0') break;
+ i++;
+ }
+}
+
+#ifdef PRINTING
+
+static void print_line(void) {
+ char the_line[HEADER_LINE_LEN + 1];
+ c_int i;
+
+ for (i = 0; i < HEADER_LINE_LEN; ++i) the_line[i] = '-';
+ the_line[HEADER_LINE_LEN] = '\0';
+ c_print("%s\n", the_line);
+}
+
+void print_header(void) {
+ // Different indentation required for windows
+#if defined(IS_WINDOWS) && !defined(PYTHON)
+ c_print("iter ");
+#else
+ c_print("iter ");
+#endif
+
+ // Main information
+ c_print("objective pri res dua res rho");
+# ifdef PROFILING
+ c_print(" time");
+# endif /* ifdef PROFILING */
+ c_print("\n");
+}
+
+void print_setup_header(const OSQPWorkspace *work) {
+ OSQPData *data;
+ OSQPSettings *settings;
+ c_int nnz; // Number of nonzeros in the problem
+
+ data = work->data;
+ settings = work->settings;
+
+ // Number of nonzeros
+ nnz = data->P->p[data->P->n] + data->A->p[data->A->n];
+
+ print_line();
+ c_print(" OSQP v%s - Operator Splitting QP Solver\n"
+ " (c) Bartolomeo Stellato, Goran Banjac\n"
+ " University of Oxford - Stanford University 2021\n",
+ OSQP_VERSION);
+ print_line();
+
+ // Print variables and constraints
+ c_print("problem: ");
+ c_print("variables n = %i, constraints m = %i\n ",
+ (int)data->n,
+ (int)data->m);
+ c_print("nnz(P) + nnz(A) = %i\n", (int)nnz);
+
+ // Print Settings
+ c_print("settings: ");
+ c_print("linear system solver = %s",
+ LINSYS_SOLVER_NAME[settings->linsys_solver]);
+
+ if (work->linsys_solver->nthreads != 1) {
+ c_print(" (%d threads)", (int)work->linsys_solver->nthreads);
+ }
+ c_print(",\n ");
+
+ c_print("eps_abs = %.1e, eps_rel = %.1e,\n ",
+ settings->eps_abs, settings->eps_rel);
+ c_print("eps_prim_inf = %.1e, eps_dual_inf = %.1e,\n ",
+ settings->eps_prim_inf, settings->eps_dual_inf);
+ c_print("rho = %.2e ", settings->rho);
+
+ if (settings->adaptive_rho) {
+ c_print("(adaptive)");
+ }
+ c_print(",\n ");
+ c_print("sigma = %.2e, alpha = %.2f, ",
+ settings->sigma, settings->alpha);
+ c_print("max_iter = %i\n", (int)settings->max_iter);
+
+ if (settings->check_termination) {
+ c_print(" check_termination: on (interval %i),\n",
+ (int)settings->check_termination);
+ } else {c_print(" check_termination: off,\n");}
+# ifdef PROFILING
+ if (settings->time_limit) {
+ c_print(" time_limit: %.2e sec,\n", settings->time_limit);
+ }
+# endif /* ifdef PROFILING */
+
+ if (settings->scaling) {
+ c_print(" scaling: on, ");
+ } else {
+ c_print(" scaling: off, ");
+ }
+
+ if (settings->scaled_termination) {
+ c_print("scaled_termination: on\n");
+ } else {
+ c_print("scaled_termination: off\n");
+ }
+
+ if (settings->warm_start) {
+ c_print(" warm start: on, ");
+ } else {
+ c_print(" warm start: off, ");
+ }
+
+ if (settings->polish) {
+ c_print("polish: on, ");
+ } else {
+ c_print("polish: off, ");
+ }
+
+# ifdef PROFILING
+ if (settings->time_limit) {
+ c_print("time_limit: %.2e sec\n", settings->time_limit);
+ } else {
+ c_print("time_limit: off\n");
+ }
+# endif
+
+ c_print("\n");
+}
+
+void print_summary(OSQPWorkspace *work) {
+ OSQPInfo *info;
+
+ info = work->info;
+
+ c_print("%4i", (int)info->iter);
+ c_print(" %12.4e", info->obj_val);
+ c_print(" %9.2e", info->pri_res);
+ c_print(" %9.2e", info->dua_res);
+ c_print(" %9.2e", work->settings->rho);
+# ifdef PROFILING
+
+ if (work->first_run) {
+ // total time: setup + solve
+ c_print(" %9.2es", info->setup_time + info->solve_time);
+ } else {
+ // total time: update + solve
+ c_print(" %9.2es", info->update_time + info->solve_time);
+ }
+# endif /* ifdef PROFILING */
+ c_print("\n");
+
+ work->summary_printed = 1; // Summary has been printed
+}
+
+void print_polish(OSQPWorkspace *work) {
+ OSQPInfo *info;
+
+ info = work->info;
+
+ c_print("%4s", "plsh");
+ c_print(" %12.4e", info->obj_val);
+ c_print(" %9.2e", info->pri_res);
+ c_print(" %9.2e", info->dua_res);
+
+ // Different characters for windows/unix
+#if defined(IS_WINDOWS) && !defined(PYTHON)
+ c_print(" ---------");
+#else
+ c_print(" --------");
+#endif
+
+# ifdef PROFILING
+ if (work->first_run) {
+ // total time: setup + solve
+ c_print(" %9.2es", info->setup_time + info->solve_time +
+ info->polish_time);
+ } else {
+ // total time: update + solve
+ c_print(" %9.2es", info->update_time + info->solve_time +
+ info->polish_time);
+ }
+# endif /* ifdef PROFILING */
+ c_print("\n");
+}
+
+void print_footer(OSQPInfo *info, c_int polish) {
+ c_print("\n"); // Add space after iterations
+
+ c_print("status: %s\n", info->status);
+
+ if (polish && (info->status_val == OSQP_SOLVED)) {
+ if (info->status_polish == 1) {
+ c_print("solution polish: successful\n");
+ } else if (info->status_polish < 0) {
+ c_print("solution polish: unsuccessful\n");
+ }
+ }
+
+ c_print("number of iterations: %i\n", (int)info->iter);
+
+ if ((info->status_val == OSQP_SOLVED) ||
+ (info->status_val == OSQP_SOLVED_INACCURATE)) {
+ c_print("optimal objective: %.4f\n", info->obj_val);
+ }
+
+# ifdef PROFILING
+ c_print("run time: %.2es\n", info->run_time);
+# endif /* ifdef PROFILING */
+
+# if EMBEDDED != 1
+ c_print("optimal rho estimate: %.2e\n", info->rho_estimate);
+# endif /* if EMBEDDED != 1 */
+ c_print("\n");
+}
+
+#endif /* End #ifdef PRINTING */
+
+
+#ifndef EMBEDDED
+
+OSQPSettings* copy_settings(const OSQPSettings *settings) {
+ OSQPSettings *new = c_malloc(sizeof(OSQPSettings));
+
+ if (!new) return OSQP_NULL;
+
+ // Copy settings
+ // NB. Copying them explicitly because memcpy is not
+ // defined when PRINTING is disabled (appears in string.h)
+ new->rho = settings->rho;
+ new->sigma = settings->sigma;
+ new->scaling = settings->scaling;
+
+# if EMBEDDED != 1
+ new->adaptive_rho = settings->adaptive_rho;
+ new->adaptive_rho_interval = settings->adaptive_rho_interval;
+ new->adaptive_rho_tolerance = settings->adaptive_rho_tolerance;
+# ifdef PROFILING
+ new->adaptive_rho_fraction = settings->adaptive_rho_fraction;
+# endif
+# endif // EMBEDDED != 1
+ new->max_iter = settings->max_iter;
+ new->eps_abs = settings->eps_abs;
+ new->eps_rel = settings->eps_rel;
+ new->eps_prim_inf = settings->eps_prim_inf;
+ new->eps_dual_inf = settings->eps_dual_inf;
+ new->alpha = settings->alpha;
+ new->linsys_solver = settings->linsys_solver;
+ new->delta = settings->delta;
+ new->polish = settings->polish;
+ new->polish_refine_iter = settings->polish_refine_iter;
+ new->verbose = settings->verbose;
+ new->scaled_termination = settings->scaled_termination;
+ new->check_termination = settings->check_termination;
+ new->warm_start = settings->warm_start;
+# ifdef PROFILING
+ new->time_limit = settings->time_limit;
+# endif
+
+ return new;
+}
+
+#endif // #ifndef EMBEDDED
+
+
+/*******************
+* Timer Functions *
+*******************/
+
+#ifdef PROFILING
+
+// Windows
+# ifdef IS_WINDOWS
+
+void osqp_tic(OSQPTimer *t)
+{
+ QueryPerformanceFrequency(&t->freq);
+ QueryPerformanceCounter(&t->tic);
+}
+
+c_float osqp_toc(OSQPTimer *t)
+{
+ QueryPerformanceCounter(&t->toc);
+ return (t->toc.QuadPart - t->tic.QuadPart) / (c_float)t->freq.QuadPart;
+}
+
+// Mac
+# elif defined IS_MAC
+
+void osqp_tic(OSQPTimer *t)
+{
+ /* read current clock cycles */
+ t->tic = mach_absolute_time();
+}
+
+c_float osqp_toc(OSQPTimer *t)
+{
+ uint64_t duration; /* elapsed time in clock cycles*/
+
+ t->toc = mach_absolute_time();
+ duration = t->toc - t->tic;
+
+ /*conversion from clock cycles to nanoseconds*/
+ mach_timebase_info(&(t->tinfo));
+ duration *= t->tinfo.numer;
+ duration /= t->tinfo.denom;
+
+ return (c_float)duration / 1e9;
+}
+
+// Linux
+# else /* ifdef IS_WINDOWS */
+
+/* read current time */
+void osqp_tic(OSQPTimer *t)
+{
+ clock_gettime(CLOCK_MONOTONIC, &t->tic);
+}
+
+/* return time passed since last call to tic on this timer */
+c_float osqp_toc(OSQPTimer *t)
+{
+ struct timespec temp;
+
+ clock_gettime(CLOCK_MONOTONIC, &t->toc);
+
+ if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0) {
+ temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
+ temp.tv_nsec = 1e9 + t->toc.tv_nsec - t->tic.tv_nsec;
+ } else {
+ temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
+ temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
+ }
+ return (c_float)temp.tv_sec + (c_float)temp.tv_nsec / 1e9;
+}
+
+# endif /* ifdef IS_WINDOWS */
+
+#endif // If Profiling end
+
+
+/* ==================== DEBUG FUNCTIONS ======================= */
+
+
+
+// If debug mode enabled
+#ifdef DDEBUG
+
+#ifdef PRINTING
+
+void print_csc_matrix(csc *M, const char *name)
+{
+ c_int j, i, row_start, row_stop;
+ c_int k = 0;
+
+ // Print name
+ c_print("%s :\n", name);
+
+ for (j = 0; j < M->n; j++) {
+ row_start = M->p[j];
+ row_stop = M->p[j + 1];
+
+ if (row_start == row_stop) continue;
+ else {
+ for (i = row_start; i < row_stop; i++) {
+ c_print("\t[%3u,%3u] = %.3g\n", (int)M->i[i], (int)j, M->x[k++]);
+ }
+ }
+ }
+}
+
+void dump_csc_matrix(csc *M, const char *file_name) {
+ c_int j, i, row_strt, row_stop;
+ c_int k = 0;
+ FILE *f = fopen(file_name, "w");
+
+ if (f != NULL) {
+ for (j = 0; j < M->n; j++) {
+ row_strt = M->p[j];
+ row_stop = M->p[j + 1];
+
+ if (row_strt == row_stop) continue;
+ else {
+ for (i = row_strt; i < row_stop; i++) {
+ fprintf(f, "%d\t%d\t%20.18e\n",
+ (int)M->i[i] + 1, (int)j + 1, M->x[k++]);
+ }
+ }
+ }
+ fprintf(f, "%d\t%d\t%20.18e\n", (int)M->m, (int)M->n, 0.0);
+ fclose(f);
+ c_print("File %s successfully written.\n", file_name);
+ } else {
+ c_eprint("Error during writing file %s.\n", file_name);
+ }
+}
+
+void print_trip_matrix(csc *M, const char *name)
+{
+ c_int k = 0;
+
+ // Print name
+ c_print("%s :\n", name);
+
+ for (k = 0; k < M->nz; k++) {
+ c_print("\t[%3u, %3u] = %.3g\n", (int)M->i[k], (int)M->p[k], M->x[k]);
+ }
+}
+
+void print_dns_matrix(c_float *M, c_int m, c_int n, const char *name)
+{
+ c_int i, j;
+
+ c_print("%s : \n\t", name);
+
+ for (i = 0; i < m; i++) { // Cycle over rows
+ for (j = 0; j < n; j++) { // Cycle over columns
+ if (j < n - 1)
+ // c_print("% 14.12e, ", M[j*m+i]);
+ c_print("% .3g, ", M[j * m + i]);
+
+ else
+ // c_print("% 14.12e; ", M[j*m+i]);
+ c_print("% .3g; ", M[j * m + i]);
+ }
+
+ if (i < m - 1) {
+ c_print("\n\t");
+ }
+ }
+ c_print("\n");
+}
+
+void print_vec(c_float *v, c_int n, const char *name) {
+ print_dns_matrix(v, 1, n, name);
+}
+
+void dump_vec(c_float *v, c_int len, const char *file_name) {
+ c_int i;
+ FILE *f = fopen(file_name, "w");
+
+ if (f != NULL) {
+ for (i = 0; i < len; i++) {
+ fprintf(f, "%20.18e\n", v[i]);
+ }
+ fclose(f);
+ c_print("File %s successfully written.\n", file_name);
+ } else {
+ c_print("Error during writing file %s.\n", file_name);
+ }
+}
+
+void print_vec_int(c_int *x, c_int n, const char *name) {
+ c_int i;
+
+ c_print("%s = [", name);
+
+ for (i = 0; i < n; i++) {
+ c_print(" %i ", (int)x[i]);
+ }
+ c_print("]\n");
+}
+
+#endif // PRINTING
+
+#endif // DEBUG MODE