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/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