├── README.md ├── auxil.h ├── constants.h ├── cs.h ├── ctrlc.h ├── error.h ├── glob_opts.h ├── kkt.h ├── lin_alg.h ├── lin_sys.h ├── main.cpp ├── osqp.h ├── osqp ├── auxil.h ├── constants.h ├── cs.h ├── ctrlc.h ├── error.h ├── glob_opts.h ├── kkt.h ├── lin_alg.h ├── lin_sys.h ├── osqp.h ├── osqp_configure.h ├── osqp_problem.cc ├── osqp_problem.h ├── polish.h ├── proj.h ├── scaling.h ├── types.h └── util.h ├── osqp_configure.h ├── osqp_problem.cc ├── osqp_problem.h ├── polish.h ├── proj.h ├── referencesmooth.pro ├── scaling.h ├── smoothosqpproblem.cpp ├── smoothosqpproblem.h ├── types.h └── util.h /README.md: -------------------------------------------------------------------------------- 1 | # Reference-line-smooth 2 | 3 | 1.Path optimization based on SQP-OSQP sequence optimization, including curvature hard constraint (using Taylor expansion at reference point), point boundary constraint, in cartesian coordinate system 4 | 5 | 6 | ![image](https://user-images.githubusercontent.com/32810296/111271204-d64e2400-866b-11eb-8a8c-d258ca0f9330.png) 7 | -------------------------------------------------------------------------------- /auxil.h: -------------------------------------------------------------------------------- 1 | #ifndef AUXIL_H 2 | # define AUXIL_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" 9 | 10 | 11 | /*********************************************************** 12 | * Auxiliary functions needed to compute ADMM iterations * * 13 | ***********************************************************/ 14 | # if EMBEDDED != 1 15 | 16 | /** 17 | * Compute rho estimate from residuals 18 | * @param work Workspace 19 | * @return rho estimate 20 | */ 21 | c_float compute_rho_estimate(OSQPWorkspace *work); 22 | 23 | /** 24 | * Adapt rho value based on current unscaled primal/dual residuals 25 | * @param work Workspace 26 | * @return Exitflag 27 | */ 28 | c_int adapt_rho(OSQPWorkspace *work); 29 | 30 | /** 31 | * Set values of rho vector based on constraint types 32 | * @param work Workspace 33 | */ 34 | void set_rho_vec(OSQPWorkspace *work); 35 | 36 | /** 37 | * Update values of rho vector based on updated constraints. 38 | * If the constraints change, update the linear systems solver. 39 | * 40 | * @param work Workspace 41 | * @return Exitflag 42 | */ 43 | c_int update_rho_vec(OSQPWorkspace *work); 44 | 45 | # endif // EMBEDDED 46 | 47 | /** 48 | * Swap c_float vector pointers 49 | * @param a first vector 50 | * @param b second vector 51 | */ 52 | void swap_vectors(c_float **a, 53 | c_float **b); 54 | 55 | 56 | /** 57 | * Cold start workspace variables xz and y 58 | * @param work Workspace 59 | */ 60 | void cold_start(OSQPWorkspace *work); 61 | 62 | 63 | /** 64 | * Update x_tilde and z_tilde variable (first ADMM step) 65 | * @param work [description] 66 | */ 67 | void update_xz_tilde(OSQPWorkspace *work); 68 | 69 | 70 | /** 71 | * Update x (second ADMM step) 72 | * Update also delta_x (For for dual infeasibility) 73 | * @param work Workspace 74 | */ 75 | void update_x(OSQPWorkspace *work); 76 | 77 | 78 | /** 79 | * Update z (third ADMM step) 80 | * @param work Workspace 81 | */ 82 | void update_z(OSQPWorkspace *work); 83 | 84 | 85 | /** 86 | * Update y variable (fourth ADMM step) 87 | * Update also delta_y to check for primal infeasibility 88 | * @param work Workspace 89 | */ 90 | void update_y(OSQPWorkspace *work); 91 | 92 | 93 | /** 94 | * Compute objective function from data at value x 95 | * @param work OSQPWorkspace structure 96 | * @param x Value x 97 | * @return Objective function value 98 | */ 99 | c_float compute_obj_val(OSQPWorkspace *work, 100 | c_float *x); 101 | 102 | /** 103 | * Check whether QP has solution 104 | * @param info OSQPInfo 105 | */ 106 | c_int has_solution(OSQPInfo *info); 107 | 108 | /** 109 | * Store the QP solution 110 | * @param work Workspace 111 | */ 112 | void store_solution(OSQPWorkspace *work); 113 | 114 | 115 | /** 116 | * Update solver information 117 | * @param work Workspace 118 | * @param iter Iteration number 119 | * @param compute_objective Boolean (if compute the objective or not) 120 | * @param polish Boolean (if called from polish) 121 | */ 122 | void update_info(OSQPWorkspace *work, 123 | c_int iter, 124 | c_int compute_objective, 125 | c_int polish); 126 | 127 | 128 | /** 129 | * Reset solver information (after problem updates) 130 | * @param info Information structure 131 | */ 132 | void reset_info(OSQPInfo *info); 133 | 134 | 135 | /** 136 | * Update solver status (value and string) 137 | * @param info OSQPInfo 138 | * @param status_val new status value 139 | */ 140 | void update_status(OSQPInfo *info, 141 | c_int status_val); 142 | 143 | 144 | /** 145 | * Check if termination conditions are satisfied 146 | * If the boolean flag is ON, it checks for approximate conditions (10 x larger 147 | * tolerances than the ones set) 148 | * 149 | * @param work Workspace 150 | * @param approximate Boolean 151 | * @return Residuals check 152 | */ 153 | c_int check_termination(OSQPWorkspace *work, 154 | c_int approximate); 155 | 156 | 157 | # ifndef EMBEDDED 158 | 159 | /** 160 | * Validate problem data 161 | * @param data OSQPData to be validated 162 | * @return Exitflag to check 163 | */ 164 | c_int validate_data(const OSQPData *data); 165 | 166 | 167 | /** 168 | * Validate problem settings 169 | * @param settings OSQPSettings to be validated 170 | * @return Exitflag to check 171 | */ 172 | c_int validate_settings(const OSQPSettings *settings); 173 | 174 | 175 | # endif // #ifndef EMBEDDED 176 | 177 | # ifdef __cplusplus 178 | } 179 | # endif // ifdef __cplusplus 180 | 181 | #endif // ifndef AUXIL_H 182 | -------------------------------------------------------------------------------- /constants.h: -------------------------------------------------------------------------------- 1 | #ifndef CONSTANTS_H 2 | # define CONSTANTS_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | 9 | /******************* 10 | * OSQP Versioning * 11 | *******************/ 12 | # define OSQP_VERSION ("0.6.0") /* string literals automatically null-terminated 13 | */ 14 | 15 | /****************** 16 | * Solver Status * 17 | ******************/ 18 | # define OSQP_DUAL_INFEASIBLE_INACCURATE (4) 19 | # define OSQP_PRIMAL_INFEASIBLE_INACCURATE (3) 20 | # define OSQP_SOLVED_INACCURATE (2) 21 | # define OSQP_SOLVED (1) 22 | # define OSQP_MAX_ITER_REACHED (-2) 23 | # define OSQP_PRIMAL_INFEASIBLE (-3) /* primal infeasible */ 24 | # define OSQP_DUAL_INFEASIBLE (-4) /* dual infeasible */ 25 | # define OSQP_SIGINT (-5) /* interrupted by user */ 26 | # ifdef PROFILING 27 | # define OSQP_TIME_LIMIT_REACHED (-6) 28 | # endif // ifdef PROFILING 29 | # define OSQP_NON_CVX (-7) /* problem non convex */ 30 | # define OSQP_UNSOLVED (-10) /* Unsolved. Only setup function has been called */ 31 | 32 | 33 | /************************* 34 | * Linear System Solvers * 35 | *************************/ 36 | 37 | extern const char * LINSYS_SOLVER_NAME[]; 38 | 39 | 40 | /****************** 41 | * Solver Errors * 42 | ******************/ 43 | enum osqp_error_type { 44 | OSQP_DATA_VALIDATION_ERROR = 1, /* Start errors from 1 */ 45 | OSQP_SETTINGS_VALIDATION_ERROR, 46 | OSQP_LINSYS_SOLVER_LOAD_ERROR, 47 | OSQP_LINSYS_SOLVER_INIT_ERROR, 48 | OSQP_NONCVX_ERROR, 49 | OSQP_MEM_ALLOC_ERROR, 50 | OSQP_WORKSPACE_NOT_INIT_ERROR, 51 | }; 52 | extern const char * OSQP_ERROR_MESSAGE[]; 53 | 54 | 55 | /********************************** 56 | * Solver Parameters and Settings * 57 | **********************************/ 58 | 59 | # define RHO (0.1) 60 | # define SIGMA (1E-06) 61 | # define MAX_ITER (4000) 62 | # define EPS_ABS (1E-3) 63 | # define EPS_REL (1E-3) 64 | # define EPS_PRIM_INF (1E-4) 65 | # define EPS_DUAL_INF (1E-4) 66 | # define ALPHA (1.6) 67 | # define LINSYS_SOLVER (QDLDL_SOLVER) 68 | 69 | # define RHO_MIN (1e-06) 70 | # define RHO_MAX (1e06) 71 | # define RHO_EQ_OVER_RHO_INEQ (1e03) 72 | # define RHO_TOL (1e-04) ///< tolerance for detecting if an inequality is set to equality 73 | 74 | 75 | # ifndef EMBEDDED 76 | # define DELTA (1E-6) 77 | # define POLISH (0) 78 | # define POLISH_REFINE_ITER (3) 79 | # define VERBOSE (1) 80 | # endif // ifndef EMBEDDED 81 | 82 | # define SCALED_TERMINATION (0) 83 | # define CHECK_TERMINATION (25) 84 | # define WARM_START (1) 85 | # define SCALING (10) 86 | 87 | # define MIN_SCALING (1e-04) ///< minimum scaling value 88 | # define MAX_SCALING (1e+04) ///< maximum scaling value 89 | 90 | 91 | # ifndef OSQP_NULL 92 | # define OSQP_NULL 0 93 | # endif /* ifndef OSQP_NULL */ 94 | 95 | # ifndef OSQP_NAN 96 | # define OSQP_NAN ((c_float)0x7fc00000UL) // not a number 97 | # endif /* ifndef OSQP_NAN */ 98 | 99 | # ifndef OSQP_INFTY 100 | # define OSQP_INFTY ((c_float)1e30) // infinity 101 | # endif /* ifndef OSQP_INFTY */ 102 | 103 | 104 | # if EMBEDDED != 1 105 | # define ADAPTIVE_RHO (1) 106 | # define ADAPTIVE_RHO_INTERVAL (0) 107 | # define ADAPTIVE_RHO_FRACTION (0.4) ///< fraction of setup time after which we update rho 108 | # define ADAPTIVE_RHO_MULTIPLE_TERMINATION (4) ///< multiple of check_termination after which we update rho (if PROFILING disabled) 109 | # define ADAPTIVE_RHO_FIXED (100) ///< number of iterations after which we update rho if termination_check and PROFILING are disabled 110 | # define ADAPTIVE_RHO_TOLERANCE (5) ///< tolerance for adopting new rho; minimum ratio between new rho and the current one 111 | # endif // if EMBEDDED != 1 112 | 113 | # ifdef PROFILING 114 | # define TIME_LIMIT (0) ///< Disable time limit as default 115 | # endif // ifdef PROFILING 116 | 117 | /* Printing */ 118 | # define PRINT_INTERVAL 200 119 | 120 | 121 | # ifdef __cplusplus 122 | } 123 | # endif // ifdef __cplusplus 124 | 125 | #endif // ifndef CONSTANTS_H 126 | -------------------------------------------------------------------------------- /cs.h: -------------------------------------------------------------------------------- 1 | #ifndef CS_H 2 | # define CS_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" // CSC matrix type 9 | # include "lin_alg.h" // Vector copy operations 10 | 11 | /***************************************************************************** 12 | * Create and free CSC Matrices * 13 | *****************************************************************************/ 14 | 15 | /** 16 | * Create Compressed-Column-Sparse matrix from existing arrays 17 | (no MALLOC to create inner arrays x, i, p) 18 | * @param m First dimension 19 | * @param n Second dimension 20 | * @param nzmax Maximum number of nonzero elements 21 | * @param x Vector of data 22 | * @param i Vector of row indices 23 | * @param p Vector of column pointers 24 | * @return New matrix pointer 25 | */ 26 | csc* csc_matrix(c_int m, 27 | c_int n, 28 | c_int nzmax, 29 | c_float *x, 30 | c_int *i, 31 | c_int *p); 32 | 33 | 34 | /** 35 | * Create uninitialized CSC matrix atricture 36 | (uses MALLOC to create inner arrays x, i, p) 37 | * @param m First dimension 38 | * @param n Second dimension 39 | * @param nzmax Maximum number of nonzero elements 40 | * @param values Allocate values (0/1) 41 | * @param triplet Allocate CSC or triplet format matrix (1/0) 42 | * @return Matrix pointer 43 | */ 44 | csc* csc_spalloc(c_int m, 45 | c_int n, 46 | c_int nzmax, 47 | c_int values, 48 | c_int triplet); 49 | 50 | 51 | /** 52 | * Free sparse matrix 53 | (uses FREE to free inner arrays x, i, p) 54 | * @param A Matrix in CSC format 55 | */ 56 | void csc_spfree(csc *A); 57 | 58 | 59 | /** 60 | * free workspace and return a sparse matrix result 61 | * @param C CSC matrix 62 | * @param w Workspace vector 63 | * @param x Workspace vector 64 | * @param ok flag 65 | * @return Return result C if OK, otherwise free it 66 | */ 67 | csc* csc_done(csc *C, 68 | void *w, 69 | void *x, 70 | c_int ok); 71 | 72 | /***************************************************************************** 73 | * Copy Matrices * 74 | *****************************************************************************/ 75 | 76 | /** 77 | * Copy sparse CSC matrix A to output. 78 | * output is allocated by this function (uses MALLOC) 79 | */ 80 | csc* copy_csc_mat(const csc *A); 81 | 82 | 83 | /** 84 | * Copy sparse CSC matrix A to B (B is preallocated, NO MALOC) 85 | */ 86 | void prea_copy_csc_mat(const csc *A, 87 | csc *B); 88 | 89 | 90 | /***************************************************************************** 91 | * Matrices Conversion * 92 | *****************************************************************************/ 93 | 94 | 95 | /** 96 | * C = compressed-column CSC from matrix T in triplet form 97 | * 98 | * TtoC stores the vector of indices from T to C 99 | * -> C[TtoC[i]] = T[i] 100 | * 101 | * @param T matrix in triplet format 102 | * @param TtoC vector of indices from triplet to CSC format 103 | * @return matrix in CSC format 104 | */ 105 | csc* triplet_to_csc(const csc *T, 106 | c_int *TtoC); 107 | 108 | 109 | /** 110 | * C = compressed-row CSR from matrix T in triplet form 111 | * 112 | * TtoC stores the vector of indices from T to C 113 | * -> C[TtoC[i]] = T[i] 114 | * 115 | * @param T matrix in triplet format 116 | * @param TtoC vector of indices from triplet to CSR format 117 | * @return matrix in CSR format 118 | */ 119 | csc* triplet_to_csr(const csc *T, 120 | c_int *TtoC); 121 | 122 | 123 | /** 124 | * Convert sparse to dense 125 | */ 126 | c_float* csc_to_dns(csc *M); 127 | 128 | 129 | /** 130 | * Convert square CSC matrix into upper triangular one 131 | * 132 | * @param M Matrix to be converted 133 | * @return Upper triangular matrix in CSC format 134 | */ 135 | csc* csc_to_triu(csc *M); 136 | 137 | 138 | /***************************************************************************** 139 | * Extra operations * 140 | *****************************************************************************/ 141 | 142 | /** 143 | * p [0..n] = cumulative sum of c [0..n-1], and then copy p [0..n-1] into c 144 | * 145 | * @param p Create cumulative sum into p 146 | * @param c Vector of which we compute cumulative sum 147 | * @param n Number of elements 148 | * @return Exitflag 149 | */ 150 | c_int csc_cumsum(c_int *p, 151 | c_int *c, 152 | c_int n); 153 | 154 | /** 155 | * Compute inverse of permutation matrix stored in the vector p. 156 | * The computed inverse is also stored in a vector. 157 | */ 158 | c_int* csc_pinv(c_int const *p, 159 | c_int n); 160 | 161 | /** 162 | * C = A(p,p)= PAP' where A and C are symmetric the upper part stored; 163 | * NB: pinv not p! 164 | * @param A Original matrix (upper-triangular) 165 | * @param pinv Inverse of permutation vector 166 | * @param AtoC Mapping from indices of A-x to C->x 167 | * @param values Are values of A allocated? 168 | * @return New matrix (allocated) 169 | */ 170 | csc* csc_symperm(const csc *A, 171 | const c_int *pinv, 172 | c_int *AtoC, 173 | c_int values); 174 | 175 | 176 | # ifdef __cplusplus 177 | } 178 | # endif // ifdef __cplusplus 179 | 180 | #endif // ifndef CS_H 181 | -------------------------------------------------------------------------------- /ctrlc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Interface for OSQP signal handling. 3 | */ 4 | 5 | #ifndef CTRLC_H 6 | # define CTRLC_H 7 | 8 | # ifdef __cplusplus 9 | extern "C" { 10 | # endif // ifdef __cplusplus 11 | 12 | # include "glob_opts.h" 13 | 14 | # if defined MATLAB 15 | 16 | /* No header file available here; define the prototypes ourselves */ 17 | bool utIsInterruptPending(void); 18 | bool utSetInterruptEnabled(bool); 19 | 20 | # elif defined IS_WINDOWS 21 | 22 | /* Use Windows SetConsoleCtrlHandler for signal handling */ 23 | # include 24 | 25 | # else // if defined MATLAB 26 | 27 | /* Use sigaction for signal handling on non-Windows machines */ 28 | # include 29 | 30 | # endif // if defined MATLAB 31 | 32 | /* METHODS are the same for both */ 33 | 34 | /** 35 | * Start listener for ctrl-c interrupts 36 | */ 37 | void osqp_start_interrupt_listener(void); 38 | 39 | /** 40 | * End listener for ctrl-c interrupts 41 | */ 42 | void osqp_end_interrupt_listener(void); 43 | 44 | /** 45 | * Check if the solver has been interrupted 46 | * @return Boolean indicating if the solver has been interrupted 47 | */ 48 | int osqp_is_interrupted(void); 49 | 50 | 51 | # ifdef __cplusplus 52 | } 53 | # endif // ifdef __cplusplus 54 | 55 | 56 | #endif /* END IFDEF CTRLC */ 57 | -------------------------------------------------------------------------------- /error.h: -------------------------------------------------------------------------------- 1 | #ifndef ERROR_H 2 | # define ERROR_H 3 | 4 | /* OSQP error handling */ 5 | 6 | # ifdef __cplusplus 7 | extern "C" { 8 | # endif // ifdef __cplusplus 9 | 10 | # include "types.h" 11 | 12 | 13 | /* OSQP error macro */ 14 | # if __STDC_VERSION__ >= 199901L 15 | /* The C99 standard gives the __func__ macro, which is preferred over __FUNCTION__ */ 16 | # define osqp_error(error_code) _osqp_error(error_code, __func__); 17 | #else 18 | # define osqp_error(error_code) _osqp_error(error_code, __FUNCTION__); 19 | #endif 20 | 21 | 22 | 23 | /** 24 | * Internal function to print error description and return error code. 25 | * @param Error code 26 | * @param Function name 27 | * @return Error code 28 | */ 29 | c_int _osqp_error(enum osqp_error_type error_code, 30 | const char * function_name); 31 | 32 | 33 | 34 | # ifdef __cplusplus 35 | } 36 | # endif // ifdef __cplusplus 37 | 38 | #endif // ifndef ERROR_H 39 | -------------------------------------------------------------------------------- /glob_opts.h: -------------------------------------------------------------------------------- 1 | #ifndef GLOB_OPTS_H 2 | # define GLOB_OPTS_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif /* ifdef __cplusplus */ 7 | 8 | /* 9 | Define OSQP compiler flags 10 | */ 11 | 12 | // cmake generated compiler flags 13 | #include "osqp_configure.h" 14 | 15 | /* DATA CUSTOMIZATIONS (depending on memory manager)----------------------- */ 16 | 17 | // We do not need memory allocation functions if EMBEDDED is enabled 18 | # ifndef EMBEDDED 19 | 20 | /* define custom printfs and memory allocation (e.g. matlab/python) */ 21 | # ifdef MATLAB 22 | # include "mex.h" 23 | static void* c_calloc(size_t num, size_t size) { 24 | void *m = mxCalloc(num, size); 25 | mexMakeMemoryPersistent(m); 26 | return m; 27 | } 28 | 29 | static void* c_malloc(size_t size) { 30 | void *m = mxMalloc(size); 31 | mexMakeMemoryPersistent(m); 32 | return m; 33 | } 34 | 35 | static void* c_realloc(void *ptr, size_t size) { 36 | void *m = mxRealloc(ptr, size); 37 | mexMakeMemoryPersistent(m); 38 | return m; 39 | } 40 | 41 | # define c_free mxFree 42 | # elif defined PYTHON 43 | // Define memory allocation for python. Note that in Python 2 memory manager 44 | // Calloc is not implemented 45 | # include 46 | # define c_malloc PyMem_Malloc 47 | # if PY_MAJOR_VERSION >= 3 48 | # define c_calloc PyMem_Calloc 49 | # else /* if PY_MAJOR_VERSION >= 3 */ 50 | static void* c_calloc(size_t num, size_t size) { 51 | void *m = PyMem_Malloc(num * size); 52 | memset(m, 0, num * size); 53 | return m; 54 | } 55 | # endif /* if PY_MAJOR_VERSION >= 3 */ 56 | # define c_free PyMem_Free 57 | # define c_realloc PyMem_Realloc 58 | 59 | # elif !defined OSQP_CUSTOM_MEMORY 60 | /* If no custom memory allocator defined, use 61 | * standard linux functions. Custom memory allocator definitions 62 | * appear in the osqp_configure.h generated file. */ 63 | # include 64 | # define c_malloc malloc 65 | # define c_calloc calloc 66 | # define c_free free 67 | # define c_realloc realloc 68 | # endif /* ifdef MATLAB */ 69 | 70 | # endif // end ifndef EMBEDDED 71 | 72 | 73 | /* Use customized number representation ----------------------------------- */ 74 | # ifdef DLONG // long integers 75 | typedef long long c_int; /* for indices */ 76 | # else // standard integers 77 | typedef int c_int; /* for indices */ 78 | # endif /* ifdef DLONG */ 79 | 80 | 81 | # ifndef DFLOAT // Doubles 82 | typedef double c_float; /* for numerical values */ 83 | # else // Floats 84 | typedef float c_float; /* for numerical values */ 85 | # endif /* ifndef DFLOAT */ 86 | 87 | 88 | /* Use customized operations */ 89 | 90 | # ifndef c_absval 91 | # define c_absval(x) (((x) < 0) ? -(x) : (x)) 92 | # endif /* ifndef c_absval */ 93 | 94 | # ifndef c_max 95 | # define c_max(a, b) (((a) > (b)) ? (a) : (b)) 96 | # endif /* ifndef c_max */ 97 | 98 | # ifndef c_min 99 | # define c_min(a, b) (((a) < (b)) ? (a) : (b)) 100 | # endif /* ifndef c_min */ 101 | 102 | // Round x to the nearest multiple of N 103 | # ifndef c_roundmultiple 104 | # define c_roundmultiple(x, N) ((x) + .5 * (N)-c_fmod((x) + .5 * (N), (N))) 105 | # endif /* ifndef c_roundmultiple */ 106 | 107 | 108 | /* Use customized functions ----------------------------------------------- */ 109 | 110 | # if EMBEDDED != 1 111 | 112 | # include 113 | # ifndef DFLOAT // Doubles 114 | # define c_sqrt sqrt 115 | # define c_fmod fmod 116 | # else // Floats 117 | # define c_sqrt sqrtf 118 | # define c_fmod fmodf 119 | # endif /* ifndef DFLOAT */ 120 | 121 | # endif // end EMBEDDED 122 | 123 | 124 | # ifdef PRINTING 125 | # include 126 | # include 127 | 128 | # ifdef MATLAB 129 | # define c_print mexPrintf 130 | 131 | // The following trick slows down the performance a lot. Since many solvers 132 | // actually 133 | // call mexPrintf and immediately force print buffer flush 134 | // otherwise messages don't appear until solver termination 135 | // ugly because matlab does not provide a vprintf mex interface 136 | // #include 137 | // static int c_print(char *msg, ...) 138 | // { 139 | // va_list argList; 140 | // va_start(argList, msg); 141 | // //message buffer 142 | // int bufferSize = 256; 143 | // char buffer[bufferSize]; 144 | // vsnprintf(buffer,bufferSize-1, msg, argList); 145 | // va_end(argList); 146 | // int out = mexPrintf(buffer); //print to matlab display 147 | // mexEvalString("drawnow;"); // flush matlab print buffer 148 | // return out; 149 | // } 150 | # elif defined PYTHON 151 | # include 152 | # define c_print PySys_WriteStdout 153 | # elif defined R_LANG 154 | # include 155 | # define c_print Rprintf 156 | # else /* ifdef MATLAB */ 157 | # define c_print printf 158 | # endif /* ifdef MATLAB */ 159 | 160 | /* Print error macro */ 161 | # define c_eprint(...) c_print("ERROR in %s: ", __FUNCTION__); c_print(\ 162 | __VA_ARGS__); c_print("\n"); 163 | 164 | # endif /* PRINTING */ 165 | 166 | 167 | # ifdef __cplusplus 168 | } 169 | # endif /* ifdef __cplusplus */ 170 | 171 | #endif /* ifndef GLOB_OPTS_H */ 172 | -------------------------------------------------------------------------------- /kkt.h: -------------------------------------------------------------------------------- 1 | #ifndef KKT_H 2 | # define KKT_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" 9 | 10 | # ifndef EMBEDDED 11 | 12 | # include "cs.h" 13 | 14 | /** 15 | * Form square symmetric KKT matrix of the form 16 | * 17 | * [P + param1 I, A'; 18 | * A -diag(param2)] 19 | * 20 | * NB: Only the upper triangular part is stuffed! 21 | * 22 | * 23 | * If Pdiag_idx is not OSQP_NULL, it saves the index of the diagonal 24 | * elements of P there and the number of diagonal elements in Pdiag_n. 25 | * 26 | * Similarly, if rhotoKKT is not null, 27 | * it saves where the values of param2 go in the final KKT matrix 28 | * 29 | * NB: Pdiag_idx needs to be freed! 30 | * 31 | * @param P cost matrix (already just upper triangular part) 32 | * @param A linear constraint matrix 33 | * @param format CSC (0) or CSR (1) 34 | * @param param1 regularization parameter 35 | * @param param2 regularization parameter (vector) 36 | * @param PtoKKT (modified) index mapping from elements of P to KKT matrix 37 | * @param AtoKKT (modified) index mapping from elements of A to KKT matrix 38 | * @param Pdiag_idx (modified) Address of the index of diagonal elements in P 39 | * @param Pdiag_n (modified) Address to the number of diagonal elements in P 40 | * @param param2toKKT (modified) index mapping from param2 to elements of 41 | *KKT 42 | * @return return status flag 43 | */ 44 | csc* form_KKT(const csc *P, 45 | const csc *A, 46 | c_int format, 47 | c_float param1, 48 | c_float *param2, 49 | c_int *PtoKKT, 50 | c_int *AtoKKT, 51 | c_int **Pdiag_idx, 52 | c_int *Pdiag_n, 53 | c_int *param2toKKT); 54 | # endif // ifndef EMBEDDED 55 | 56 | 57 | # if EMBEDDED != 1 58 | 59 | /** 60 | * Update KKT matrix using the elements of P 61 | * 62 | * @param KKT KKT matrix in CSC form (upper-triangular) 63 | * @param P P matrix in CSC form (upper-triangular) 64 | * @param PtoKKT Vector of pointers from P->x to KKT->x 65 | * @param param1 Parameter added to the diagonal elements of P 66 | * @param Pdiag_idx Index of diagonal elements in P->x 67 | * @param Pdiag_n Number of diagonal elements of P 68 | */ 69 | void update_KKT_P(csc *KKT, 70 | const csc *P, 71 | const c_int *PtoKKT, 72 | const c_float param1, 73 | const c_int *Pdiag_idx, 74 | const c_int Pdiag_n); 75 | 76 | 77 | /** 78 | * Update KKT matrix using the elements of A 79 | * 80 | * @param KKT KKT matrix in CSC form (upper-triangular) 81 | * @param A A matrix in CSC form (upper-triangular) 82 | * @param AtoKKT Vector of pointers from A->x to KKT->x 83 | */ 84 | void update_KKT_A(csc *KKT, 85 | const csc *A, 86 | const c_int *AtoKKT); 87 | 88 | 89 | /** 90 | * Update KKT matrix with new param2 91 | * 92 | * @param KKT KKT matrix 93 | * @param param2 Parameter of the KKT matrix (vector) 94 | * @param param2toKKT index where param2 enters in the KKT matrix 95 | * @param m number of constraints 96 | */ 97 | void update_KKT_param2(csc *KKT, 98 | const c_float *param2, 99 | const c_int *param2toKKT, 100 | const c_int m); 101 | 102 | # endif // EMBEDDED != 1 103 | 104 | 105 | # ifdef __cplusplus 106 | } 107 | # endif // ifdef __cplusplus 108 | 109 | #endif // ifndef KKT_H 110 | -------------------------------------------------------------------------------- /lin_alg.h: -------------------------------------------------------------------------------- 1 | #ifndef LIN_ALG_H 2 | # define LIN_ALG_H 3 | 4 | 5 | # ifdef __cplusplus 6 | extern "C" { 7 | # endif // ifdef __cplusplus 8 | 9 | # include "types.h" 10 | 11 | 12 | /* VECTOR FUNCTIONS ----------------------------------------------------------*/ 13 | 14 | # ifndef EMBEDDED 15 | 16 | /* copy vector a into output (Uses MALLOC)*/ 17 | c_float* vec_copy(c_float *a, 18 | c_int n); 19 | # endif // ifndef EMBEDDED 20 | 21 | /* copy vector a into preallocated vector b */ 22 | void prea_vec_copy(const c_float *a, 23 | c_float *b, 24 | c_int n); 25 | 26 | /* copy integer vector a into preallocated vector b */ 27 | void prea_int_vec_copy(const c_int *a, 28 | c_int *b, 29 | c_int n); 30 | 31 | /* set float vector to scalar */ 32 | void vec_set_scalar(c_float *a, 33 | c_float sc, 34 | c_int n); 35 | 36 | /* set integer vector to scalar */ 37 | void int_vec_set_scalar(c_int *a, 38 | c_int sc, 39 | c_int n); 40 | 41 | /* add scalar to vector*/ 42 | void vec_add_scalar(c_float *a, 43 | c_float sc, 44 | c_int n); 45 | 46 | /* multiply scalar to vector */ 47 | void vec_mult_scalar(c_float *a, 48 | c_float sc, 49 | c_int n); 50 | 51 | /* c = a + sc*b */ 52 | void vec_add_scaled(c_float *c, 53 | const c_float *a, 54 | const c_float *b, 55 | c_int n, 56 | c_float sc); 57 | 58 | /* ||v||_inf */ 59 | c_float vec_norm_inf(const c_float *v, 60 | c_int l); 61 | 62 | /* ||Sv||_inf */ 63 | c_float vec_scaled_norm_inf(const c_float *S, 64 | const c_float *v, 65 | c_int l); 66 | 67 | /* ||a - b||_inf */ 68 | c_float vec_norm_inf_diff(const c_float *a, 69 | const c_float *b, 70 | c_int l); 71 | 72 | /* mean of vector elements */ 73 | c_float vec_mean(const c_float *a, 74 | c_int n); 75 | 76 | # if EMBEDDED != 1 77 | 78 | /* Vector elementwise reciprocal b = 1./a (needed for scaling)*/ 79 | void vec_ew_recipr(const c_float *a, 80 | c_float *b, 81 | c_int n); 82 | # endif // if EMBEDDED != 1 83 | 84 | /* Inner product a'b */ 85 | c_float vec_prod(const c_float *a, 86 | const c_float *b, 87 | c_int n); 88 | 89 | /* Elementwise product a.*b stored in c*/ 90 | void vec_ew_prod(const c_float *a, 91 | const c_float *b, 92 | c_float *c, 93 | c_int n); 94 | 95 | # if EMBEDDED != 1 96 | 97 | /* Elementwise sqrt of the vector elements */ 98 | void vec_ew_sqrt(c_float *a, 99 | c_int n); 100 | 101 | /* Elementwise max between each vector component and max_val */ 102 | void vec_ew_max(c_float *a, 103 | c_int n, 104 | c_float max_val); 105 | 106 | /* Elementwise min between each vector component and max_val */ 107 | void vec_ew_min(c_float *a, 108 | c_int n, 109 | c_float min_val); 110 | 111 | /* Elementwise maximum between vectors c = max(a, b) */ 112 | void vec_ew_max_vec(const c_float *a, 113 | const c_float *b, 114 | c_float *c, 115 | c_int n); 116 | 117 | /* Elementwise minimum between vectors c = min(a, b) */ 118 | void vec_ew_min_vec(const c_float *a, 119 | const c_float *b, 120 | c_float *c, 121 | c_int n); 122 | 123 | # endif // if EMBEDDED != 1 124 | 125 | 126 | /* MATRIX FUNCTIONS ----------------------------------------------------------*/ 127 | 128 | /* multiply scalar to matrix */ 129 | void mat_mult_scalar(csc *A, 130 | c_float sc); 131 | 132 | /* Premultiply matrix A by diagonal matrix with diagonal d, 133 | i.e. scale the rows of A by d 134 | */ 135 | void mat_premult_diag(csc *A, 136 | const c_float *d); 137 | 138 | /* Premultiply matrix A by diagonal matrix with diagonal d, 139 | i.e. scale the columns of A by d 140 | */ 141 | void mat_postmult_diag(csc *A, 142 | const c_float *d); 143 | 144 | 145 | /* Matrix-vector multiplication 146 | * y = A*x (if plus_eq == 0) 147 | * y += A*x (if plus_eq == 1) 148 | * y -= A*x (if plus_eq == -1) 149 | */ 150 | void mat_vec(const csc *A, 151 | const c_float *x, 152 | c_float *y, 153 | c_int plus_eq); 154 | 155 | 156 | /* Matrix-transpose-vector multiplication 157 | * y = A'*x (if plus_eq == 0) 158 | * y += A'*x (if plus_eq == 1) 159 | * y -= A'*x (if plus_eq == -1) 160 | * If skip_diag == 1, then diagonal elements of A are assumed to be zero. 161 | */ 162 | void mat_tpose_vec(const csc *A, 163 | const c_float *x, 164 | c_float *y, 165 | c_int plus_eq, 166 | c_int skip_diag); 167 | 168 | 169 | # if EMBEDDED != 1 170 | 171 | /** 172 | * Infinity norm of each matrix column 173 | * @param M Input matrix 174 | * @param E Vector of infinity norms 175 | * 176 | */ 177 | void mat_inf_norm_cols(const csc *M, 178 | c_float *E); 179 | 180 | /** 181 | * Infinity norm of each matrix row 182 | * @param M Input matrix 183 | * @param E Vector of infinity norms 184 | * 185 | */ 186 | void mat_inf_norm_rows(const csc *M, 187 | c_float *E); 188 | 189 | /** 190 | * Infinity norm of each matrix column 191 | * Matrix M is symmetric upper-triangular 192 | * 193 | * @param M Input matrix (symmetric, upper-triangular) 194 | * @param E Vector of infinity norms 195 | * 196 | */ 197 | void mat_inf_norm_cols_sym_triu(const csc *M, 198 | c_float *E); 199 | 200 | # endif // EMBEDDED != 1 201 | 202 | /** 203 | * Compute quadratic form f(x) = 1/2 x' P x 204 | * @param P quadratic matrix in CSC form (only upper triangular) 205 | * @param x argument float vector 206 | * @return quadratic form value 207 | */ 208 | c_float quad_form(const csc *P, 209 | const c_float *x); 210 | 211 | 212 | # ifdef __cplusplus 213 | } 214 | # endif // ifdef __cplusplus 215 | 216 | #endif // ifndef LIN_ALG_H 217 | -------------------------------------------------------------------------------- /lin_sys.h: -------------------------------------------------------------------------------- 1 | #ifndef LIN_SYS_H 2 | # define LIN_SYS_H 3 | 4 | /* KKT linear system definition and solution */ 5 | 6 | # ifdef __cplusplus 7 | extern "C" { 8 | # endif // ifdef __cplusplus 9 | 10 | # include "types.h" 11 | 12 | /** 13 | * Load linear system solver shared library 14 | * @param linsys_solver Linear system solver 15 | * @return Zero on success, nonzero on failure. 16 | */ 17 | c_int load_linsys_solver(enum linsys_solver_type linsys_solver); 18 | 19 | 20 | /** 21 | * Unload linear system solver shared library 22 | * @param linsys_solver Linear system solver 23 | * @return Zero on success, nonzero on failure. 24 | */ 25 | c_int unload_linsys_solver(enum linsys_solver_type linsys_solver); 26 | 27 | 28 | // NB: Only the upper triangular part of P is stuffed! 29 | 30 | /** 31 | * Initialize linear system solver structure 32 | * @param s Pointer to linear system solver structure 33 | * @param P Cost function matrix 34 | * @param A Constraint matrix 35 | * @param sigma Algorithm parameter 36 | * @param rho_vec Algorithm parameter 37 | * @param linsys_solver Linear system solver 38 | * @param polish 0/1 depending whether we are allocating for 39 | *polishing or not 40 | * @return Exitflag for error (0 if no errors) 41 | */ 42 | c_int init_linsys_solver(LinSysSolver **s, 43 | const csc *P, 44 | const csc *A, 45 | c_float sigma, 46 | const c_float *rho_vec, 47 | enum linsys_solver_type linsys_solver, 48 | c_int polish); 49 | 50 | # ifdef __cplusplus 51 | } 52 | # endif // ifdef __cplusplus 53 | 54 | #endif // ifndef LIN_SYS_H 55 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "osqp_problem.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "smoothosqpproblem.h" 19 | 20 | 21 | using namespace std; 22 | 23 | int main(int argc, char *argv[]) 24 | { 25 | QCoreApplication a(argc, argv); 26 | 27 | std::cout<<"OSQP RUNNING..."<tm_year,1+p->tm_mon,p->tm_mday,8+p->tm_hour,p->tm_min); 37 | mycout.open(timeinfo); 38 | 39 | 40 | std::vector> raw_point2d; 41 | std::vector bounds; 42 | std::vector opt_x; 43 | std::vector opt_y; 44 | 45 | // for (int i = 0 ;i < 25; ++i) { 46 | 47 | // raw_point2d.push_back(std::make_pair(i*1.0,-25.0+i)); 48 | // } 49 | 50 | // for (int i = 25 ;i < 50; ++i) { 51 | // raw_point2d.push_back(std::make_pair(i*1.0,0.0)); 52 | // } 53 | 54 | for (int i = 0 ;i < 50; ++i) { 55 | raw_point2d.push_back(std::make_pair(i*1.0, 0.0)); 56 | } 57 | 58 | bounds.push_back(0.1); 59 | 60 | for (int i = 0; i < 48; ++i) { 61 | bounds.push_back(50.0); 62 | } 63 | 64 | bounds.push_back(0.1); 65 | 66 | // //Now we are using default variables in osqp solver 67 | // double weight_fem_pos_deviation = 1e10; 68 | // double weight_path_length = 1.0; 69 | // double weight_ref_deviation = 1.0; 70 | // double weight_curvature_constraint_slack_var = 10.0; 71 | // double curvature_constraint = 0.08; 72 | // int sqp_sub_max_iter = 20; 73 | // int sqp_pen_max_iter = 10; 74 | // double sqp_ctol = 0.08; 75 | // int max_iter = 500; 76 | // double time_limit = 0.0; 77 | // bool verbose = false; 78 | // bool scaled_termination = true; 79 | // bool warm_start = true; 80 | 81 | FemPosDeviationSqpOsqpInterface solver; 82 | solver.set_ref_points(raw_point2d); 83 | solver.set_bounds_around_refs(bounds); 84 | 85 | if (!solver.Solve()) { 86 | std::cout<<"failture"<> opt_xy = solver.opt_xy(); 90 | 91 | // TODO(Jinyun): unify output data container 92 | opt_x.resize(opt_xy.size()); 93 | opt_y.resize(opt_xy.size()); 94 | for (size_t i = 0; i < opt_xy.size(); ++i) { 95 | opt_x[i] = opt_xy[i].first; 96 | opt_y[i] = opt_xy[i].second; 97 | } 98 | 99 | 100 | std::cout<<"xxx::"<Optimize(1000)){ 112 | // std::cout<<"Optimize successful!!"<x_.size(); ++i) 115 | // { 116 | // mycout<<"x: "<x_.at(i)<<" dx: "<dx_.at(i)<<" ddx: "<ddx_.at(i)<<" left_edge: "<info structure 66 | * 67 | * The solution is stored in the \a work->solution structure 68 | * 69 | * If the problem is primal infeasible, the certificate is stored 70 | * in \a work->delta_y 71 | * 72 | * If the problem is dual infeasible, the certificate is stored in \a 73 | * work->delta_x 74 | * 75 | * @param work Workspace allocated 76 | * @return Exitflag for errors 77 | */ 78 | c_int osqp_solve(OSQPWorkspace *work); 79 | 80 | 81 | # ifndef EMBEDDED 82 | 83 | /** 84 | * Cleanup workspace by deallocating memory 85 | * 86 | * This function is not used in code generation 87 | * @param work Workspace 88 | * @return Exitflag for errors 89 | */ 90 | c_int osqp_cleanup(OSQPWorkspace *work); 91 | 92 | # endif // ifndef EMBEDDED 93 | 94 | /** @} */ 95 | 96 | 97 | /******************************************** 98 | * Sublevel API * 99 | * * 100 | * Edit data without performing setup again * 101 | ********************************************/ 102 | 103 | /** 104 | * @name Sublevel API 105 | * @{ 106 | */ 107 | 108 | /** 109 | * Update linear cost in the problem 110 | * @param work Workspace 111 | * @param q_new New linear cost 112 | * @return Exitflag for errors and warnings 113 | */ 114 | c_int osqp_update_lin_cost(OSQPWorkspace *work, 115 | const c_float *q_new); 116 | 117 | 118 | /** 119 | * Update lower and upper bounds in the problem constraints 120 | * @param work Workspace 121 | * @param l_new New lower bound 122 | * @param u_new New upper bound 123 | * @return Exitflag: 1 if new lower bound is not <= than new upper bound 124 | */ 125 | c_int osqp_update_bounds(OSQPWorkspace *work, 126 | const c_float *l_new, 127 | const c_float *u_new); 128 | 129 | 130 | /** 131 | * Update lower bound in the problem constraints 132 | * @param work Workspace 133 | * @param l_new New lower bound 134 | * @return Exitflag: 1 if new lower bound is not <= than upper bound 135 | */ 136 | c_int osqp_update_lower_bound(OSQPWorkspace *work, 137 | const c_float *l_new); 138 | 139 | 140 | /** 141 | * Update upper bound in the problem constraints 142 | * @param work Workspace 143 | * @param u_new New upper bound 144 | * @return Exitflag: 1 if new upper bound is not >= than lower bound 145 | */ 146 | c_int osqp_update_upper_bound(OSQPWorkspace *work, 147 | const c_float *u_new); 148 | 149 | 150 | /** 151 | * Warm start primal and dual variables 152 | * @param work Workspace structure 153 | * @param x Primal variable 154 | * @param y Dual variable 155 | * @return Exitflag 156 | */ 157 | c_int osqp_warm_start(OSQPWorkspace *work, 158 | const c_float *x, 159 | const c_float *y); 160 | 161 | 162 | /** 163 | * Warm start primal variable 164 | * @param work Workspace structure 165 | * @param x Primal variable 166 | * @return Exitflag 167 | */ 168 | c_int osqp_warm_start_x(OSQPWorkspace *work, 169 | const c_float *x); 170 | 171 | 172 | /** 173 | * Warm start dual variable 174 | * @param work Workspace structure 175 | * @param y Dual variable 176 | * @return Exitflag 177 | */ 178 | c_int osqp_warm_start_y(OSQPWorkspace *work, 179 | const c_float *y); 180 | 181 | 182 | # if EMBEDDED != 1 183 | 184 | /** 185 | * Update elements of matrix P (upper triangular) 186 | * without changing sparsity structure. 187 | * 188 | * 189 | * If Px_new_idx is OSQP_NULL, Px_new is assumed to be as long as P->x 190 | * and the whole P->x is replaced. 191 | * 192 | * @param work Workspace structure 193 | * @param Px_new Vector of new elements in P->x (upper triangular) 194 | * @param Px_new_idx Index mapping new elements to positions in P->x 195 | * @param P_new_n Number of new elements to be changed 196 | * @return output flag: 0: OK 197 | * 1: P_new_n > nnzP 198 | * <0: error in the update 199 | */ 200 | c_int osqp_update_P(OSQPWorkspace *work, 201 | const c_float *Px_new, 202 | const c_int *Px_new_idx, 203 | c_int P_new_n); 204 | 205 | 206 | /** 207 | * Update elements of matrix A without changing sparsity structure. 208 | * 209 | * 210 | * If Ax_new_idx is OSQP_NULL, Ax_new is assumed to be as long as A->x 211 | * and the whole A->x is replaced. 212 | * 213 | * @param work Workspace structure 214 | * @param Ax_new Vector of new elements in A->x 215 | * @param Ax_new_idx Index mapping new elements to positions in A->x 216 | * @param A_new_n Number of new elements to be changed 217 | * @return output flag: 0: OK 218 | * 1: A_new_n > nnzA 219 | * <0: error in the update 220 | */ 221 | c_int osqp_update_A(OSQPWorkspace *work, 222 | const c_float *Ax_new, 223 | const c_int *Ax_new_idx, 224 | c_int A_new_n); 225 | 226 | 227 | /** 228 | * Update elements of matrix P (upper triangular) and elements of matrix A 229 | * without changing sparsity structure. 230 | * 231 | * 232 | * If Px_new_idx is OSQP_NULL, Px_new is assumed to be as long as P->x 233 | * and the whole P->x is replaced. 234 | * 235 | * If Ax_new_idx is OSQP_NULL, Ax_new is assumed to be as long as A->x 236 | * and the whole A->x is replaced. 237 | * 238 | * @param work Workspace structure 239 | * @param Px_new Vector of new elements in P->x (upper triangular) 240 | * @param Px_new_idx Index mapping new elements to positions in P->x 241 | * @param P_new_n Number of new elements to be changed 242 | * @param Ax_new Vector of new elements in A->x 243 | * @param Ax_new_idx Index mapping new elements to positions in A->x 244 | * @param A_new_n Number of new elements to be changed 245 | * @return output flag: 0: OK 246 | * 1: P_new_n > nnzP 247 | * 2: A_new_n > nnzA 248 | * <0: error in the update 249 | */ 250 | c_int osqp_update_P_A(OSQPWorkspace *work, 251 | const c_float *Px_new, 252 | const c_int *Px_new_idx, 253 | c_int P_new_n, 254 | const c_float *Ax_new, 255 | const c_int *Ax_new_idx, 256 | c_int A_new_n); 257 | 258 | /** 259 | * Update rho. Limit it between RHO_MIN and RHO_MAX. 260 | * @param work Workspace 261 | * @param rho_new New rho setting 262 | * @return Exitflag 263 | */ 264 | c_int osqp_update_rho(OSQPWorkspace *work, 265 | c_float rho_new); 266 | 267 | # endif // if EMBEDDED != 1 268 | 269 | /** @} */ 270 | 271 | 272 | /** 273 | * @name Update settings 274 | * @{ 275 | */ 276 | 277 | 278 | /** 279 | * Update max_iter setting 280 | * @param work Workspace 281 | * @param max_iter_new New max_iter setting 282 | * @return Exitflag 283 | */ 284 | c_int osqp_update_max_iter(OSQPWorkspace *work, 285 | c_int max_iter_new); 286 | 287 | 288 | /** 289 | * Update absolute tolernace value 290 | * @param work Workspace 291 | * @param eps_abs_new New absolute tolerance value 292 | * @return Exitflag 293 | */ 294 | c_int osqp_update_eps_abs(OSQPWorkspace *work, 295 | c_float eps_abs_new); 296 | 297 | 298 | /** 299 | * Update relative tolernace value 300 | * @param work Workspace 301 | * @param eps_rel_new New relative tolerance value 302 | * @return Exitflag 303 | */ 304 | c_int osqp_update_eps_rel(OSQPWorkspace *work, 305 | c_float eps_rel_new); 306 | 307 | 308 | /** 309 | * Update primal infeasibility tolerance 310 | * @param work Workspace 311 | * @param eps_prim_inf_new New primal infeasibility tolerance 312 | * @return Exitflag 313 | */ 314 | c_int osqp_update_eps_prim_inf(OSQPWorkspace *work, 315 | c_float eps_prim_inf_new); 316 | 317 | 318 | /** 319 | * Update dual infeasibility tolerance 320 | * @param work Workspace 321 | * @param eps_dual_inf_new New dual infeasibility tolerance 322 | * @return Exitflag 323 | */ 324 | c_int osqp_update_eps_dual_inf(OSQPWorkspace *work, 325 | c_float eps_dual_inf_new); 326 | 327 | 328 | /** 329 | * Update relaxation parameter alpha 330 | * @param work Workspace 331 | * @param alpha_new New relaxation parameter value 332 | * @return Exitflag 333 | */ 334 | c_int osqp_update_alpha(OSQPWorkspace *work, 335 | c_float alpha_new); 336 | 337 | 338 | /** 339 | * Update warm_start setting 340 | * @param work Workspace 341 | * @param warm_start_new New warm_start setting 342 | * @return Exitflag 343 | */ 344 | c_int osqp_update_warm_start(OSQPWorkspace *work, 345 | c_int warm_start_new); 346 | 347 | 348 | /** 349 | * Update scaled_termination setting 350 | * @param work Workspace 351 | * @param scaled_termination_new New scaled_termination setting 352 | * @return Exitflag 353 | */ 354 | c_int osqp_update_scaled_termination(OSQPWorkspace *work, 355 | c_int scaled_termination_new); 356 | 357 | /** 358 | * Update check_termination setting 359 | * @param work Workspace 360 | * @param check_termination_new New check_termination setting 361 | * @return Exitflag 362 | */ 363 | c_int osqp_update_check_termination(OSQPWorkspace *work, 364 | c_int check_termination_new); 365 | 366 | 367 | # ifndef EMBEDDED 368 | 369 | /** 370 | * Update regularization parameter in polish 371 | * @param work Workspace 372 | * @param delta_new New regularization parameter 373 | * @return Exitflag 374 | */ 375 | c_int osqp_update_delta(OSQPWorkspace *work, 376 | c_float delta_new); 377 | 378 | 379 | /** 380 | * Update polish setting 381 | * @param work Workspace 382 | * @param polish_new New polish setting 383 | * @return Exitflag 384 | */ 385 | c_int osqp_update_polish(OSQPWorkspace *work, 386 | c_int polish_new); 387 | 388 | 389 | /** 390 | * Update number of iterative refinement steps in polish 391 | * @param work Workspace 392 | * @param polish_refine_iter_new New iterative reginement steps 393 | * @return Exitflag 394 | */ 395 | c_int osqp_update_polish_refine_iter(OSQPWorkspace *work, 396 | c_int polish_refine_iter_new); 397 | 398 | 399 | /** 400 | * Update verbose setting 401 | * @param work Workspace 402 | * @param verbose_new New verbose setting 403 | * @return Exitflag 404 | */ 405 | c_int osqp_update_verbose(OSQPWorkspace *work, 406 | c_int verbose_new); 407 | 408 | 409 | # endif // #ifndef EMBEDDED 410 | 411 | # ifdef PROFILING 412 | 413 | /** 414 | * Update time_limit setting 415 | * @param work Workspace 416 | * @param time_limit_new New time_limit setting 417 | * @return Exitflag 418 | */ 419 | c_int osqp_update_time_limit(OSQPWorkspace *work, 420 | c_float time_limit_new); 421 | # endif // ifdef PROFILING 422 | 423 | /** @} */ 424 | 425 | 426 | # ifdef __cplusplus 427 | } 428 | # endif // ifdef __cplusplus 429 | 430 | #endif // ifndef OSQP_H 431 | -------------------------------------------------------------------------------- /osqp/auxil.h: -------------------------------------------------------------------------------- 1 | #ifndef AUXIL_H 2 | # define AUXIL_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" 9 | 10 | 11 | /*********************************************************** 12 | * Auxiliary functions needed to compute ADMM iterations * * 13 | ***********************************************************/ 14 | # if EMBEDDED != 1 15 | 16 | /** 17 | * Compute rho estimate from residuals 18 | * @param work Workspace 19 | * @return rho estimate 20 | */ 21 | c_float compute_rho_estimate(OSQPWorkspace *work); 22 | 23 | /** 24 | * Adapt rho value based on current unscaled primal/dual residuals 25 | * @param work Workspace 26 | * @return Exitflag 27 | */ 28 | c_int adapt_rho(OSQPWorkspace *work); 29 | 30 | /** 31 | * Set values of rho vector based on constraint types 32 | * @param work Workspace 33 | */ 34 | void set_rho_vec(OSQPWorkspace *work); 35 | 36 | /** 37 | * Update values of rho vector based on updated constraints. 38 | * If the constraints change, update the linear systems solver. 39 | * 40 | * @param work Workspace 41 | * @return Exitflag 42 | */ 43 | c_int update_rho_vec(OSQPWorkspace *work); 44 | 45 | # endif // EMBEDDED 46 | 47 | /** 48 | * Swap c_float vector pointers 49 | * @param a first vector 50 | * @param b second vector 51 | */ 52 | void swap_vectors(c_float **a, 53 | c_float **b); 54 | 55 | 56 | /** 57 | * Cold start workspace variables xz and y 58 | * @param work Workspace 59 | */ 60 | void cold_start(OSQPWorkspace *work); 61 | 62 | 63 | /** 64 | * Update x_tilde and z_tilde variable (first ADMM step) 65 | * @param work [description] 66 | */ 67 | void update_xz_tilde(OSQPWorkspace *work); 68 | 69 | 70 | /** 71 | * Update x (second ADMM step) 72 | * Update also delta_x (For for dual infeasibility) 73 | * @param work Workspace 74 | */ 75 | void update_x(OSQPWorkspace *work); 76 | 77 | 78 | /** 79 | * Update z (third ADMM step) 80 | * @param work Workspace 81 | */ 82 | void update_z(OSQPWorkspace *work); 83 | 84 | 85 | /** 86 | * Update y variable (fourth ADMM step) 87 | * Update also delta_y to check for primal infeasibility 88 | * @param work Workspace 89 | */ 90 | void update_y(OSQPWorkspace *work); 91 | 92 | 93 | /** 94 | * Compute objective function from data at value x 95 | * @param work OSQPWorkspace structure 96 | * @param x Value x 97 | * @return Objective function value 98 | */ 99 | c_float compute_obj_val(OSQPWorkspace *work, 100 | c_float *x); 101 | 102 | /** 103 | * Check whether QP has solution 104 | * @param info OSQPInfo 105 | */ 106 | c_int has_solution(OSQPInfo *info); 107 | 108 | /** 109 | * Store the QP solution 110 | * @param work Workspace 111 | */ 112 | void store_solution(OSQPWorkspace *work); 113 | 114 | 115 | /** 116 | * Update solver information 117 | * @param work Workspace 118 | * @param iter Iteration number 119 | * @param compute_objective Boolean (if compute the objective or not) 120 | * @param polish Boolean (if called from polish) 121 | */ 122 | void update_info(OSQPWorkspace *work, 123 | c_int iter, 124 | c_int compute_objective, 125 | c_int polish); 126 | 127 | 128 | /** 129 | * Reset solver information (after problem updates) 130 | * @param info Information structure 131 | */ 132 | void reset_info(OSQPInfo *info); 133 | 134 | 135 | /** 136 | * Update solver status (value and string) 137 | * @param info OSQPInfo 138 | * @param status_val new status value 139 | */ 140 | void update_status(OSQPInfo *info, 141 | c_int status_val); 142 | 143 | 144 | /** 145 | * Check if termination conditions are satisfied 146 | * If the boolean flag is ON, it checks for approximate conditions (10 x larger 147 | * tolerances than the ones set) 148 | * 149 | * @param work Workspace 150 | * @param approximate Boolean 151 | * @return Residuals check 152 | */ 153 | c_int check_termination(OSQPWorkspace *work, 154 | c_int approximate); 155 | 156 | 157 | # ifndef EMBEDDED 158 | 159 | /** 160 | * Validate problem data 161 | * @param data OSQPData to be validated 162 | * @return Exitflag to check 163 | */ 164 | c_int validate_data(const OSQPData *data); 165 | 166 | 167 | /** 168 | * Validate problem settings 169 | * @param settings OSQPSettings to be validated 170 | * @return Exitflag to check 171 | */ 172 | c_int validate_settings(const OSQPSettings *settings); 173 | 174 | 175 | # endif // #ifndef EMBEDDED 176 | 177 | # ifdef __cplusplus 178 | } 179 | # endif // ifdef __cplusplus 180 | 181 | #endif // ifndef AUXIL_H 182 | -------------------------------------------------------------------------------- /osqp/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef CONSTANTS_H 2 | # define CONSTANTS_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | 9 | /******************* 10 | * OSQP Versioning * 11 | *******************/ 12 | # define OSQP_VERSION ("0.6.0") /* string literals automatically null-terminated 13 | */ 14 | 15 | /****************** 16 | * Solver Status * 17 | ******************/ 18 | # define OSQP_DUAL_INFEASIBLE_INACCURATE (4) 19 | # define OSQP_PRIMAL_INFEASIBLE_INACCURATE (3) 20 | # define OSQP_SOLVED_INACCURATE (2) 21 | # define OSQP_SOLVED (1) 22 | # define OSQP_MAX_ITER_REACHED (-2) 23 | # define OSQP_PRIMAL_INFEASIBLE (-3) /* primal infeasible */ 24 | # define OSQP_DUAL_INFEASIBLE (-4) /* dual infeasible */ 25 | # define OSQP_SIGINT (-5) /* interrupted by user */ 26 | # ifdef PROFILING 27 | # define OSQP_TIME_LIMIT_REACHED (-6) 28 | # endif // ifdef PROFILING 29 | # define OSQP_NON_CVX (-7) /* problem non convex */ 30 | # define OSQP_UNSOLVED (-10) /* Unsolved. Only setup function has been called */ 31 | 32 | 33 | /************************* 34 | * Linear System Solvers * 35 | *************************/ 36 | enum linsys_solver_type { QDLDL_SOLVER, MKL_PARDISO_SOLVER }; 37 | extern const char * LINSYS_SOLVER_NAME[]; 38 | 39 | 40 | /****************** 41 | * Solver Errors * 42 | ******************/ 43 | enum osqp_error_type { 44 | OSQP_DATA_VALIDATION_ERROR = 1, /* Start errors from 1 */ 45 | OSQP_SETTINGS_VALIDATION_ERROR, 46 | OSQP_LINSYS_SOLVER_LOAD_ERROR, 47 | OSQP_LINSYS_SOLVER_INIT_ERROR, 48 | OSQP_NONCVX_ERROR, 49 | OSQP_MEM_ALLOC_ERROR, 50 | OSQP_WORKSPACE_NOT_INIT_ERROR, 51 | }; 52 | extern const char * OSQP_ERROR_MESSAGE[]; 53 | 54 | 55 | /********************************** 56 | * Solver Parameters and Settings * 57 | **********************************/ 58 | 59 | # define RHO (0.1) 60 | # define SIGMA (1E-06) 61 | # define MAX_ITER (4000) 62 | # define EPS_ABS (1E-3) 63 | # define EPS_REL (1E-3) 64 | # define EPS_PRIM_INF (1E-4) 65 | # define EPS_DUAL_INF (1E-4) 66 | # define ALPHA (1.6) 67 | # define LINSYS_SOLVER (QDLDL_SOLVER) 68 | 69 | # define RHO_MIN (1e-06) 70 | # define RHO_MAX (1e06) 71 | # define RHO_EQ_OVER_RHO_INEQ (1e03) 72 | # define RHO_TOL (1e-04) ///< tolerance for detecting if an inequality is set to equality 73 | 74 | 75 | # ifndef EMBEDDED 76 | # define DELTA (1E-6) 77 | # define POLISH (0) 78 | # define POLISH_REFINE_ITER (3) 79 | # define VERBOSE (1) 80 | # endif // ifndef EMBEDDED 81 | 82 | # define SCALED_TERMINATION (0) 83 | # define CHECK_TERMINATION (25) 84 | # define WARM_START (1) 85 | # define SCALING (10) 86 | 87 | # define MIN_SCALING (1e-04) ///< minimum scaling value 88 | # define MAX_SCALING (1e+04) ///< maximum scaling value 89 | 90 | 91 | # ifndef OSQP_NULL 92 | # define OSQP_NULL 0 93 | # endif /* ifndef OSQP_NULL */ 94 | 95 | # ifndef OSQP_NAN 96 | # define OSQP_NAN ((c_float)0x7fc00000UL) // not a number 97 | # endif /* ifndef OSQP_NAN */ 98 | 99 | # ifndef OSQP_INFTY 100 | # define OSQP_INFTY ((c_float)1e30) // infinity 101 | # endif /* ifndef OSQP_INFTY */ 102 | 103 | 104 | # if EMBEDDED != 1 105 | # define ADAPTIVE_RHO (1) 106 | # define ADAPTIVE_RHO_INTERVAL (0) 107 | # define ADAPTIVE_RHO_FRACTION (0.4) ///< fraction of setup time after which we update rho 108 | # define ADAPTIVE_RHO_MULTIPLE_TERMINATION (4) ///< multiple of check_termination after which we update rho (if PROFILING disabled) 109 | # define ADAPTIVE_RHO_FIXED (100) ///< number of iterations after which we update rho if termination_check and PROFILING are disabled 110 | # define ADAPTIVE_RHO_TOLERANCE (5) ///< tolerance for adopting new rho; minimum ratio between new rho and the current one 111 | # endif // if EMBEDDED != 1 112 | 113 | # ifdef PROFILING 114 | # define TIME_LIMIT (0) ///< Disable time limit as default 115 | # endif // ifdef PROFILING 116 | 117 | /* Printing */ 118 | # define PRINT_INTERVAL 200 119 | 120 | 121 | # ifdef __cplusplus 122 | } 123 | # endif // ifdef __cplusplus 124 | 125 | #endif // ifndef CONSTANTS_H 126 | -------------------------------------------------------------------------------- /osqp/cs.h: -------------------------------------------------------------------------------- 1 | #ifndef CS_H 2 | # define CS_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" // CSC matrix type 9 | # include "lin_alg.h" // Vector copy operations 10 | 11 | /***************************************************************************** 12 | * Create and free CSC Matrices * 13 | *****************************************************************************/ 14 | 15 | /** 16 | * Create Compressed-Column-Sparse matrix from existing arrays 17 | (no MALLOC to create inner arrays x, i, p) 18 | * @param m First dimension 19 | * @param n Second dimension 20 | * @param nzmax Maximum number of nonzero elements 21 | * @param x Vector of data 22 | * @param i Vector of row indices 23 | * @param p Vector of column pointers 24 | * @return New matrix pointer 25 | */ 26 | csc* csc_matrix(c_int m, 27 | c_int n, 28 | c_int nzmax, 29 | c_float *x, 30 | c_int *i, 31 | c_int *p); 32 | 33 | 34 | /** 35 | * Create uninitialized CSC matrix atricture 36 | (uses MALLOC to create inner arrays x, i, p) 37 | * @param m First dimension 38 | * @param n Second dimension 39 | * @param nzmax Maximum number of nonzero elements 40 | * @param values Allocate values (0/1) 41 | * @param triplet Allocate CSC or triplet format matrix (1/0) 42 | * @return Matrix pointer 43 | */ 44 | csc* csc_spalloc(c_int m, 45 | c_int n, 46 | c_int nzmax, 47 | c_int values, 48 | c_int triplet); 49 | 50 | 51 | /** 52 | * Free sparse matrix 53 | (uses FREE to free inner arrays x, i, p) 54 | * @param A Matrix in CSC format 55 | */ 56 | void csc_spfree(csc *A); 57 | 58 | 59 | /** 60 | * free workspace and return a sparse matrix result 61 | * @param C CSC matrix 62 | * @param w Workspace vector 63 | * @param x Workspace vector 64 | * @param ok flag 65 | * @return Return result C if OK, otherwise free it 66 | */ 67 | csc* csc_done(csc *C, 68 | void *w, 69 | void *x, 70 | c_int ok); 71 | 72 | /***************************************************************************** 73 | * Copy Matrices * 74 | *****************************************************************************/ 75 | 76 | /** 77 | * Copy sparse CSC matrix A to output. 78 | * output is allocated by this function (uses MALLOC) 79 | */ 80 | csc* copy_csc_mat(const csc *A); 81 | 82 | 83 | /** 84 | * Copy sparse CSC matrix A to B (B is preallocated, NO MALOC) 85 | */ 86 | void prea_copy_csc_mat(const csc *A, 87 | csc *B); 88 | 89 | 90 | /***************************************************************************** 91 | * Matrices Conversion * 92 | *****************************************************************************/ 93 | 94 | 95 | /** 96 | * C = compressed-column CSC from matrix T in triplet form 97 | * 98 | * TtoC stores the vector of indices from T to C 99 | * -> C[TtoC[i]] = T[i] 100 | * 101 | * @param T matrix in triplet format 102 | * @param TtoC vector of indices from triplet to CSC format 103 | * @return matrix in CSC format 104 | */ 105 | csc* triplet_to_csc(const csc *T, 106 | c_int *TtoC); 107 | 108 | 109 | /** 110 | * C = compressed-row CSR from matrix T in triplet form 111 | * 112 | * TtoC stores the vector of indices from T to C 113 | * -> C[TtoC[i]] = T[i] 114 | * 115 | * @param T matrix in triplet format 116 | * @param TtoC vector of indices from triplet to CSR format 117 | * @return matrix in CSR format 118 | */ 119 | csc* triplet_to_csr(const csc *T, 120 | c_int *TtoC); 121 | 122 | 123 | /** 124 | * Convert sparse to dense 125 | */ 126 | c_float* csc_to_dns(csc *M); 127 | 128 | 129 | /** 130 | * Convert square CSC matrix into upper triangular one 131 | * 132 | * @param M Matrix to be converted 133 | * @return Upper triangular matrix in CSC format 134 | */ 135 | csc* csc_to_triu(csc *M); 136 | 137 | 138 | /***************************************************************************** 139 | * Extra operations * 140 | *****************************************************************************/ 141 | 142 | /** 143 | * p [0..n] = cumulative sum of c [0..n-1], and then copy p [0..n-1] into c 144 | * 145 | * @param p Create cumulative sum into p 146 | * @param c Vector of which we compute cumulative sum 147 | * @param n Number of elements 148 | * @return Exitflag 149 | */ 150 | c_int csc_cumsum(c_int *p, 151 | c_int *c, 152 | c_int n); 153 | 154 | /** 155 | * Compute inverse of permutation matrix stored in the vector p. 156 | * The computed inverse is also stored in a vector. 157 | */ 158 | c_int* csc_pinv(c_int const *p, 159 | c_int n); 160 | 161 | /** 162 | * C = A(p,p)= PAP' where A and C are symmetric the upper part stored; 163 | * NB: pinv not p! 164 | * @param A Original matrix (upper-triangular) 165 | * @param pinv Inverse of permutation vector 166 | * @param AtoC Mapping from indices of A-x to C->x 167 | * @param values Are values of A allocated? 168 | * @return New matrix (allocated) 169 | */ 170 | csc* csc_symperm(const csc *A, 171 | const c_int *pinv, 172 | c_int *AtoC, 173 | c_int values); 174 | 175 | 176 | # ifdef __cplusplus 177 | } 178 | # endif // ifdef __cplusplus 179 | 180 | #endif // ifndef CS_H 181 | -------------------------------------------------------------------------------- /osqp/ctrlc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Interface for OSQP signal handling. 3 | */ 4 | 5 | #ifndef CTRLC_H 6 | # define CTRLC_H 7 | 8 | # ifdef __cplusplus 9 | extern "C" { 10 | # endif // ifdef __cplusplus 11 | 12 | # include "glob_opts.h" 13 | 14 | # if defined MATLAB 15 | 16 | /* No header file available here; define the prototypes ourselves */ 17 | bool utIsInterruptPending(void); 18 | bool utSetInterruptEnabled(bool); 19 | 20 | # elif defined IS_WINDOWS 21 | 22 | /* Use Windows SetConsoleCtrlHandler for signal handling */ 23 | # include 24 | 25 | # else // if defined MATLAB 26 | 27 | /* Use sigaction for signal handling on non-Windows machines */ 28 | # include 29 | 30 | # endif // if defined MATLAB 31 | 32 | /* METHODS are the same for both */ 33 | 34 | /** 35 | * Start listener for ctrl-c interrupts 36 | */ 37 | void osqp_start_interrupt_listener(void); 38 | 39 | /** 40 | * End listener for ctrl-c interrupts 41 | */ 42 | void osqp_end_interrupt_listener(void); 43 | 44 | /** 45 | * Check if the solver has been interrupted 46 | * @return Boolean indicating if the solver has been interrupted 47 | */ 48 | int osqp_is_interrupted(void); 49 | 50 | 51 | # ifdef __cplusplus 52 | } 53 | # endif // ifdef __cplusplus 54 | 55 | 56 | #endif /* END IFDEF CTRLC */ 57 | -------------------------------------------------------------------------------- /osqp/error.h: -------------------------------------------------------------------------------- 1 | #ifndef ERROR_H 2 | # define ERROR_H 3 | 4 | /* OSQP error handling */ 5 | 6 | # ifdef __cplusplus 7 | extern "C" { 8 | # endif // ifdef __cplusplus 9 | 10 | # include "types.h" 11 | 12 | 13 | /* OSQP error macro */ 14 | # if __STDC_VERSION__ >= 199901L 15 | /* The C99 standard gives the __func__ macro, which is preferred over __FUNCTION__ */ 16 | # define osqp_error(error_code) _osqp_error(error_code, __func__); 17 | #else 18 | # define osqp_error(error_code) _osqp_error(error_code, __FUNCTION__); 19 | #endif 20 | 21 | 22 | 23 | /** 24 | * Internal function to print error description and return error code. 25 | * @param Error code 26 | * @param Function name 27 | * @return Error code 28 | */ 29 | c_int _osqp_error(enum osqp_error_type error_code, 30 | const char * function_name); 31 | 32 | 33 | 34 | # ifdef __cplusplus 35 | } 36 | # endif // ifdef __cplusplus 37 | 38 | #endif // ifndef ERROR_H 39 | -------------------------------------------------------------------------------- /osqp/glob_opts.h: -------------------------------------------------------------------------------- 1 | #ifndef GLOB_OPTS_H 2 | # define GLOB_OPTS_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif /* ifdef __cplusplus */ 7 | 8 | /* 9 | Define OSQP compiler flags 10 | */ 11 | 12 | // cmake generated compiler flags 13 | #include "osqp_configure.h" 14 | 15 | /* DATA CUSTOMIZATIONS (depending on memory manager)----------------------- */ 16 | 17 | // We do not need memory allocation functions if EMBEDDED is enabled 18 | # ifndef EMBEDDED 19 | 20 | /* define custom printfs and memory allocation (e.g. matlab/python) */ 21 | # ifdef MATLAB 22 | # include "mex.h" 23 | static void* c_calloc(size_t num, size_t size) { 24 | void *m = mxCalloc(num, size); 25 | mexMakeMemoryPersistent(m); 26 | return m; 27 | } 28 | 29 | static void* c_malloc(size_t size) { 30 | void *m = mxMalloc(size); 31 | mexMakeMemoryPersistent(m); 32 | return m; 33 | } 34 | 35 | static void* c_realloc(void *ptr, size_t size) { 36 | void *m = mxRealloc(ptr, size); 37 | mexMakeMemoryPersistent(m); 38 | return m; 39 | } 40 | 41 | # define c_free mxFree 42 | # elif defined PYTHON 43 | // Define memory allocation for python. Note that in Python 2 memory manager 44 | // Calloc is not implemented 45 | # include 46 | # define c_malloc PyMem_Malloc 47 | # if PY_MAJOR_VERSION >= 3 48 | # define c_calloc PyMem_Calloc 49 | # else /* if PY_MAJOR_VERSION >= 3 */ 50 | static void* c_calloc(size_t num, size_t size) { 51 | void *m = PyMem_Malloc(num * size); 52 | memset(m, 0, num * size); 53 | return m; 54 | } 55 | # endif /* if PY_MAJOR_VERSION >= 3 */ 56 | # define c_free PyMem_Free 57 | # define c_realloc PyMem_Realloc 58 | 59 | # elif !defined OSQP_CUSTOM_MEMORY 60 | /* If no custom memory allocator defined, use 61 | * standard linux functions. Custom memory allocator definitions 62 | * appear in the osqp_configure.h generated file. */ 63 | # include 64 | # define c_malloc malloc 65 | # define c_calloc calloc 66 | # define c_free free 67 | # define c_realloc realloc 68 | # endif /* ifdef MATLAB */ 69 | 70 | # endif // end ifndef EMBEDDED 71 | 72 | 73 | /* Use customized number representation ----------------------------------- */ 74 | # ifdef DLONG // long integers 75 | typedef long long c_int; /* for indices */ 76 | # else // standard integers 77 | typedef int c_int; /* for indices */ 78 | # endif /* ifdef DLONG */ 79 | 80 | 81 | # ifndef DFLOAT // Doubles 82 | typedef double c_float; /* for numerical values */ 83 | # else // Floats 84 | typedef float c_float; /* for numerical values */ 85 | # endif /* ifndef DFLOAT */ 86 | 87 | 88 | /* Use customized operations */ 89 | 90 | # ifndef c_absval 91 | # define c_absval(x) (((x) < 0) ? -(x) : (x)) 92 | # endif /* ifndef c_absval */ 93 | 94 | # ifndef c_max 95 | # define c_max(a, b) (((a) > (b)) ? (a) : (b)) 96 | # endif /* ifndef c_max */ 97 | 98 | # ifndef c_min 99 | # define c_min(a, b) (((a) < (b)) ? (a) : (b)) 100 | # endif /* ifndef c_min */ 101 | 102 | // Round x to the nearest multiple of N 103 | # ifndef c_roundmultiple 104 | # define c_roundmultiple(x, N) ((x) + .5 * (N)-c_fmod((x) + .5 * (N), (N))) 105 | # endif /* ifndef c_roundmultiple */ 106 | 107 | 108 | /* Use customized functions ----------------------------------------------- */ 109 | 110 | # if EMBEDDED != 1 111 | 112 | # include 113 | # ifndef DFLOAT // Doubles 114 | # define c_sqrt sqrt 115 | # define c_fmod fmod 116 | # else // Floats 117 | # define c_sqrt sqrtf 118 | # define c_fmod fmodf 119 | # endif /* ifndef DFLOAT */ 120 | 121 | # endif // end EMBEDDED 122 | 123 | 124 | # ifdef PRINTING 125 | # include 126 | # include 127 | 128 | # ifdef MATLAB 129 | # define c_print mexPrintf 130 | 131 | // The following trick slows down the performance a lot. Since many solvers 132 | // actually 133 | // call mexPrintf and immediately force print buffer flush 134 | // otherwise messages don't appear until solver termination 135 | // ugly because matlab does not provide a vprintf mex interface 136 | // #include 137 | // static int c_print(char *msg, ...) 138 | // { 139 | // va_list argList; 140 | // va_start(argList, msg); 141 | // //message buffer 142 | // int bufferSize = 256; 143 | // char buffer[bufferSize]; 144 | // vsnprintf(buffer,bufferSize-1, msg, argList); 145 | // va_end(argList); 146 | // int out = mexPrintf(buffer); //print to matlab display 147 | // mexEvalString("drawnow;"); // flush matlab print buffer 148 | // return out; 149 | // } 150 | # elif defined PYTHON 151 | # include 152 | # define c_print PySys_WriteStdout 153 | # elif defined R_LANG 154 | # include 155 | # define c_print Rprintf 156 | # else /* ifdef MATLAB */ 157 | # define c_print printf 158 | # endif /* ifdef MATLAB */ 159 | 160 | /* Print error macro */ 161 | # define c_eprint(...) c_print("ERROR in %s: ", __FUNCTION__); c_print(\ 162 | __VA_ARGS__); c_print("\n"); 163 | 164 | # endif /* PRINTING */ 165 | 166 | 167 | # ifdef __cplusplus 168 | } 169 | # endif /* ifdef __cplusplus */ 170 | 171 | #endif /* ifndef GLOB_OPTS_H */ 172 | -------------------------------------------------------------------------------- /osqp/kkt.h: -------------------------------------------------------------------------------- 1 | #ifndef KKT_H 2 | # define KKT_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" 9 | 10 | # ifndef EMBEDDED 11 | 12 | # include "cs.h" 13 | 14 | /** 15 | * Form square symmetric KKT matrix of the form 16 | * 17 | * [P + param1 I, A'; 18 | * A -diag(param2)] 19 | * 20 | * NB: Only the upper triangular part is stuffed! 21 | * 22 | * 23 | * If Pdiag_idx is not OSQP_NULL, it saves the index of the diagonal 24 | * elements of P there and the number of diagonal elements in Pdiag_n. 25 | * 26 | * Similarly, if rhotoKKT is not null, 27 | * it saves where the values of param2 go in the final KKT matrix 28 | * 29 | * NB: Pdiag_idx needs to be freed! 30 | * 31 | * @param P cost matrix (already just upper triangular part) 32 | * @param A linear constraint matrix 33 | * @param format CSC (0) or CSR (1) 34 | * @param param1 regularization parameter 35 | * @param param2 regularization parameter (vector) 36 | * @param PtoKKT (modified) index mapping from elements of P to KKT matrix 37 | * @param AtoKKT (modified) index mapping from elements of A to KKT matrix 38 | * @param Pdiag_idx (modified) Address of the index of diagonal elements in P 39 | * @param Pdiag_n (modified) Address to the number of diagonal elements in P 40 | * @param param2toKKT (modified) index mapping from param2 to elements of 41 | *KKT 42 | * @return return status flag 43 | */ 44 | csc* form_KKT(const csc *P, 45 | const csc *A, 46 | c_int format, 47 | c_float param1, 48 | c_float *param2, 49 | c_int *PtoKKT, 50 | c_int *AtoKKT, 51 | c_int **Pdiag_idx, 52 | c_int *Pdiag_n, 53 | c_int *param2toKKT); 54 | # endif // ifndef EMBEDDED 55 | 56 | 57 | # if EMBEDDED != 1 58 | 59 | /** 60 | * Update KKT matrix using the elements of P 61 | * 62 | * @param KKT KKT matrix in CSC form (upper-triangular) 63 | * @param P P matrix in CSC form (upper-triangular) 64 | * @param PtoKKT Vector of pointers from P->x to KKT->x 65 | * @param param1 Parameter added to the diagonal elements of P 66 | * @param Pdiag_idx Index of diagonal elements in P->x 67 | * @param Pdiag_n Number of diagonal elements of P 68 | */ 69 | void update_KKT_P(csc *KKT, 70 | const csc *P, 71 | const c_int *PtoKKT, 72 | const c_float param1, 73 | const c_int *Pdiag_idx, 74 | const c_int Pdiag_n); 75 | 76 | 77 | /** 78 | * Update KKT matrix using the elements of A 79 | * 80 | * @param KKT KKT matrix in CSC form (upper-triangular) 81 | * @param A A matrix in CSC form (upper-triangular) 82 | * @param AtoKKT Vector of pointers from A->x to KKT->x 83 | */ 84 | void update_KKT_A(csc *KKT, 85 | const csc *A, 86 | const c_int *AtoKKT); 87 | 88 | 89 | /** 90 | * Update KKT matrix with new param2 91 | * 92 | * @param KKT KKT matrix 93 | * @param param2 Parameter of the KKT matrix (vector) 94 | * @param param2toKKT index where param2 enters in the KKT matrix 95 | * @param m number of constraints 96 | */ 97 | void update_KKT_param2(csc *KKT, 98 | const c_float *param2, 99 | const c_int *param2toKKT, 100 | const c_int m); 101 | 102 | # endif // EMBEDDED != 1 103 | 104 | 105 | # ifdef __cplusplus 106 | } 107 | # endif // ifdef __cplusplus 108 | 109 | #endif // ifndef KKT_H 110 | -------------------------------------------------------------------------------- /osqp/lin_alg.h: -------------------------------------------------------------------------------- 1 | #ifndef LIN_ALG_H 2 | # define LIN_ALG_H 3 | 4 | 5 | # ifdef __cplusplus 6 | extern "C" { 7 | # endif // ifdef __cplusplus 8 | 9 | # include "types.h" 10 | 11 | 12 | /* VECTOR FUNCTIONS ----------------------------------------------------------*/ 13 | 14 | # ifndef EMBEDDED 15 | 16 | /* copy vector a into output (Uses MALLOC)*/ 17 | c_float* vec_copy(c_float *a, 18 | c_int n); 19 | # endif // ifndef EMBEDDED 20 | 21 | /* copy vector a into preallocated vector b */ 22 | void prea_vec_copy(const c_float *a, 23 | c_float *b, 24 | c_int n); 25 | 26 | /* copy integer vector a into preallocated vector b */ 27 | void prea_int_vec_copy(const c_int *a, 28 | c_int *b, 29 | c_int n); 30 | 31 | /* set float vector to scalar */ 32 | void vec_set_scalar(c_float *a, 33 | c_float sc, 34 | c_int n); 35 | 36 | /* set integer vector to scalar */ 37 | void int_vec_set_scalar(c_int *a, 38 | c_int sc, 39 | c_int n); 40 | 41 | /* add scalar to vector*/ 42 | void vec_add_scalar(c_float *a, 43 | c_float sc, 44 | c_int n); 45 | 46 | /* multiply scalar to vector */ 47 | void vec_mult_scalar(c_float *a, 48 | c_float sc, 49 | c_int n); 50 | 51 | /* c = a + sc*b */ 52 | void vec_add_scaled(c_float *c, 53 | const c_float *a, 54 | const c_float *b, 55 | c_int n, 56 | c_float sc); 57 | 58 | /* ||v||_inf */ 59 | c_float vec_norm_inf(const c_float *v, 60 | c_int l); 61 | 62 | /* ||Sv||_inf */ 63 | c_float vec_scaled_norm_inf(const c_float *S, 64 | const c_float *v, 65 | c_int l); 66 | 67 | /* ||a - b||_inf */ 68 | c_float vec_norm_inf_diff(const c_float *a, 69 | const c_float *b, 70 | c_int l); 71 | 72 | /* mean of vector elements */ 73 | c_float vec_mean(const c_float *a, 74 | c_int n); 75 | 76 | # if EMBEDDED != 1 77 | 78 | /* Vector elementwise reciprocal b = 1./a (needed for scaling)*/ 79 | void vec_ew_recipr(const c_float *a, 80 | c_float *b, 81 | c_int n); 82 | # endif // if EMBEDDED != 1 83 | 84 | /* Inner product a'b */ 85 | c_float vec_prod(const c_float *a, 86 | const c_float *b, 87 | c_int n); 88 | 89 | /* Elementwise product a.*b stored in c*/ 90 | void vec_ew_prod(const c_float *a, 91 | const c_float *b, 92 | c_float *c, 93 | c_int n); 94 | 95 | # if EMBEDDED != 1 96 | 97 | /* Elementwise sqrt of the vector elements */ 98 | void vec_ew_sqrt(c_float *a, 99 | c_int n); 100 | 101 | /* Elementwise max between each vector component and max_val */ 102 | void vec_ew_max(c_float *a, 103 | c_int n, 104 | c_float max_val); 105 | 106 | /* Elementwise min between each vector component and max_val */ 107 | void vec_ew_min(c_float *a, 108 | c_int n, 109 | c_float min_val); 110 | 111 | /* Elementwise maximum between vectors c = max(a, b) */ 112 | void vec_ew_max_vec(const c_float *a, 113 | const c_float *b, 114 | c_float *c, 115 | c_int n); 116 | 117 | /* Elementwise minimum between vectors c = min(a, b) */ 118 | void vec_ew_min_vec(const c_float *a, 119 | const c_float *b, 120 | c_float *c, 121 | c_int n); 122 | 123 | # endif // if EMBEDDED != 1 124 | 125 | 126 | /* MATRIX FUNCTIONS ----------------------------------------------------------*/ 127 | 128 | /* multiply scalar to matrix */ 129 | void mat_mult_scalar(csc *A, 130 | c_float sc); 131 | 132 | /* Premultiply matrix A by diagonal matrix with diagonal d, 133 | i.e. scale the rows of A by d 134 | */ 135 | void mat_premult_diag(csc *A, 136 | const c_float *d); 137 | 138 | /* Premultiply matrix A by diagonal matrix with diagonal d, 139 | i.e. scale the columns of A by d 140 | */ 141 | void mat_postmult_diag(csc *A, 142 | const c_float *d); 143 | 144 | 145 | /* Matrix-vector multiplication 146 | * y = A*x (if plus_eq == 0) 147 | * y += A*x (if plus_eq == 1) 148 | * y -= A*x (if plus_eq == -1) 149 | */ 150 | void mat_vec(const csc *A, 151 | const c_float *x, 152 | c_float *y, 153 | c_int plus_eq); 154 | 155 | 156 | /* Matrix-transpose-vector multiplication 157 | * y = A'*x (if plus_eq == 0) 158 | * y += A'*x (if plus_eq == 1) 159 | * y -= A'*x (if plus_eq == -1) 160 | * If skip_diag == 1, then diagonal elements of A are assumed to be zero. 161 | */ 162 | void mat_tpose_vec(const csc *A, 163 | const c_float *x, 164 | c_float *y, 165 | c_int plus_eq, 166 | c_int skip_diag); 167 | 168 | 169 | # if EMBEDDED != 1 170 | 171 | /** 172 | * Infinity norm of each matrix column 173 | * @param M Input matrix 174 | * @param E Vector of infinity norms 175 | * 176 | */ 177 | void mat_inf_norm_cols(const csc *M, 178 | c_float *E); 179 | 180 | /** 181 | * Infinity norm of each matrix row 182 | * @param M Input matrix 183 | * @param E Vector of infinity norms 184 | * 185 | */ 186 | void mat_inf_norm_rows(const csc *M, 187 | c_float *E); 188 | 189 | /** 190 | * Infinity norm of each matrix column 191 | * Matrix M is symmetric upper-triangular 192 | * 193 | * @param M Input matrix (symmetric, upper-triangular) 194 | * @param E Vector of infinity norms 195 | * 196 | */ 197 | void mat_inf_norm_cols_sym_triu(const csc *M, 198 | c_float *E); 199 | 200 | # endif // EMBEDDED != 1 201 | 202 | /** 203 | * Compute quadratic form f(x) = 1/2 x' P x 204 | * @param P quadratic matrix in CSC form (only upper triangular) 205 | * @param x argument float vector 206 | * @return quadratic form value 207 | */ 208 | c_float quad_form(const csc *P, 209 | const c_float *x); 210 | 211 | 212 | # ifdef __cplusplus 213 | } 214 | # endif // ifdef __cplusplus 215 | 216 | #endif // ifndef LIN_ALG_H 217 | -------------------------------------------------------------------------------- /osqp/lin_sys.h: -------------------------------------------------------------------------------- 1 | #ifndef LIN_SYS_H 2 | # define LIN_SYS_H 3 | 4 | /* KKT linear system definition and solution */ 5 | 6 | # ifdef __cplusplus 7 | extern "C" { 8 | # endif // ifdef __cplusplus 9 | 10 | # include "types.h" 11 | 12 | /** 13 | * Load linear system solver shared library 14 | * @param linsys_solver Linear system solver 15 | * @return Zero on success, nonzero on failure. 16 | */ 17 | c_int load_linsys_solver(enum linsys_solver_type linsys_solver); 18 | 19 | 20 | /** 21 | * Unload linear system solver shared library 22 | * @param linsys_solver Linear system solver 23 | * @return Zero on success, nonzero on failure. 24 | */ 25 | c_int unload_linsys_solver(enum linsys_solver_type linsys_solver); 26 | 27 | 28 | // NB: Only the upper triangular part of P is stuffed! 29 | 30 | /** 31 | * Initialize linear system solver structure 32 | * @param s Pointer to linear system solver structure 33 | * @param P Cost function matrix 34 | * @param A Constraint matrix 35 | * @param sigma Algorithm parameter 36 | * @param rho_vec Algorithm parameter 37 | * @param linsys_solver Linear system solver 38 | * @param polish 0/1 depending whether we are allocating for 39 | *polishing or not 40 | * @return Exitflag for error (0 if no errors) 41 | */ 42 | c_int init_linsys_solver(LinSysSolver **s, 43 | const csc *P, 44 | const csc *A, 45 | c_float sigma, 46 | const c_float *rho_vec, 47 | enum linsys_solver_type linsys_solver, 48 | c_int polish); 49 | 50 | # ifdef __cplusplus 51 | } 52 | # endif // ifdef __cplusplus 53 | 54 | #endif // ifndef LIN_SYS_H 55 | -------------------------------------------------------------------------------- /osqp/osqp.h: -------------------------------------------------------------------------------- 1 | #ifndef OSQP_H 2 | # define OSQP_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | /* Includes */ 9 | # include "types.h" 10 | # include "util.h" // Needed for osqp_set_default_settings functions 11 | 12 | 13 | // Library to deal with sparse matrices enabled only if embedded not defined 14 | # ifndef EMBEDDED 15 | # include "cs.h" 16 | # endif // ifndef EMBEDDED 17 | 18 | /******************** 19 | * Main Solver API * 20 | ********************/ 21 | 22 | /** 23 | * @name Main solver API 24 | * @{ 25 | */ 26 | 27 | /** 28 | * Set default settings from constants.h file 29 | * assumes settings already allocated in memory 30 | * @param settings settings structure 31 | */ 32 | void osqp_set_default_settings(OSQPSettings *settings); 33 | 34 | 35 | # ifndef EMBEDDED 36 | 37 | /** 38 | * Initialize OSQP solver allocating memory. 39 | * 40 | * All the inputs must be already allocated in memory before calling. 41 | * 42 | * It performs: 43 | * - data and settings validation 44 | * - problem data scaling 45 | * - automatic parameters tuning (if enabled) 46 | * - setup linear system solver: 47 | * - direct solver: KKT matrix factorization is performed here 48 | * - indirect solver: KKT matrix preconditioning is performed here 49 | * 50 | * NB: This is the only function that allocates dynamic memory and is not used 51 | *during code generation 52 | * 53 | * @param workp Solver workspace pointer 54 | * @param data Problem data 55 | * @param settings Solver settings 56 | * @return Exitflag for errors (0 if no errors) 57 | */ 58 | c_int osqp_setup(OSQPWorkspace** workp, const OSQPData* data, const OSQPSettings* settings); 59 | 60 | # endif // #ifndef EMBEDDED 61 | 62 | /** 63 | * Solve quadratic program 64 | * 65 | * The final solver information is stored in the \a work->info structure 66 | * 67 | * The solution is stored in the \a work->solution structure 68 | * 69 | * If the problem is primal infeasible, the certificate is stored 70 | * in \a work->delta_y 71 | * 72 | * If the problem is dual infeasible, the certificate is stored in \a 73 | * work->delta_x 74 | * 75 | * @param work Workspace allocated 76 | * @return Exitflag for errors 77 | */ 78 | c_int osqp_solve(OSQPWorkspace *work); 79 | 80 | 81 | # ifndef EMBEDDED 82 | 83 | /** 84 | * Cleanup workspace by deallocating memory 85 | * 86 | * This function is not used in code generation 87 | * @param work Workspace 88 | * @return Exitflag for errors 89 | */ 90 | c_int osqp_cleanup(OSQPWorkspace *work); 91 | 92 | # endif // ifndef EMBEDDED 93 | 94 | /** @} */ 95 | 96 | 97 | /******************************************** 98 | * Sublevel API * 99 | * * 100 | * Edit data without performing setup again * 101 | ********************************************/ 102 | 103 | /** 104 | * @name Sublevel API 105 | * @{ 106 | */ 107 | 108 | /** 109 | * Update linear cost in the problem 110 | * @param work Workspace 111 | * @param q_new New linear cost 112 | * @return Exitflag for errors and warnings 113 | */ 114 | c_int osqp_update_lin_cost(OSQPWorkspace *work, 115 | const c_float *q_new); 116 | 117 | 118 | /** 119 | * Update lower and upper bounds in the problem constraints 120 | * @param work Workspace 121 | * @param l_new New lower bound 122 | * @param u_new New upper bound 123 | * @return Exitflag: 1 if new lower bound is not <= than new upper bound 124 | */ 125 | c_int osqp_update_bounds(OSQPWorkspace *work, 126 | const c_float *l_new, 127 | const c_float *u_new); 128 | 129 | 130 | /** 131 | * Update lower bound in the problem constraints 132 | * @param work Workspace 133 | * @param l_new New lower bound 134 | * @return Exitflag: 1 if new lower bound is not <= than upper bound 135 | */ 136 | c_int osqp_update_lower_bound(OSQPWorkspace *work, 137 | const c_float *l_new); 138 | 139 | 140 | /** 141 | * Update upper bound in the problem constraints 142 | * @param work Workspace 143 | * @param u_new New upper bound 144 | * @return Exitflag: 1 if new upper bound is not >= than lower bound 145 | */ 146 | c_int osqp_update_upper_bound(OSQPWorkspace *work, 147 | const c_float *u_new); 148 | 149 | 150 | /** 151 | * Warm start primal and dual variables 152 | * @param work Workspace structure 153 | * @param x Primal variable 154 | * @param y Dual variable 155 | * @return Exitflag 156 | */ 157 | c_int osqp_warm_start(OSQPWorkspace *work, 158 | const c_float *x, 159 | const c_float *y); 160 | 161 | 162 | /** 163 | * Warm start primal variable 164 | * @param work Workspace structure 165 | * @param x Primal variable 166 | * @return Exitflag 167 | */ 168 | c_int osqp_warm_start_x(OSQPWorkspace *work, 169 | const c_float *x); 170 | 171 | 172 | /** 173 | * Warm start dual variable 174 | * @param work Workspace structure 175 | * @param y Dual variable 176 | * @return Exitflag 177 | */ 178 | c_int osqp_warm_start_y(OSQPWorkspace *work, 179 | const c_float *y); 180 | 181 | 182 | # if EMBEDDED != 1 183 | 184 | /** 185 | * Update elements of matrix P (upper triangular) 186 | * without changing sparsity structure. 187 | * 188 | * 189 | * If Px_new_idx is OSQP_NULL, Px_new is assumed to be as long as P->x 190 | * and the whole P->x is replaced. 191 | * 192 | * @param work Workspace structure 193 | * @param Px_new Vector of new elements in P->x (upper triangular) 194 | * @param Px_new_idx Index mapping new elements to positions in P->x 195 | * @param P_new_n Number of new elements to be changed 196 | * @return output flag: 0: OK 197 | * 1: P_new_n > nnzP 198 | * <0: error in the update 199 | */ 200 | c_int osqp_update_P(OSQPWorkspace *work, 201 | const c_float *Px_new, 202 | const c_int *Px_new_idx, 203 | c_int P_new_n); 204 | 205 | 206 | /** 207 | * Update elements of matrix A without changing sparsity structure. 208 | * 209 | * 210 | * If Ax_new_idx is OSQP_NULL, Ax_new is assumed to be as long as A->x 211 | * and the whole A->x is replaced. 212 | * 213 | * @param work Workspace structure 214 | * @param Ax_new Vector of new elements in A->x 215 | * @param Ax_new_idx Index mapping new elements to positions in A->x 216 | * @param A_new_n Number of new elements to be changed 217 | * @return output flag: 0: OK 218 | * 1: A_new_n > nnzA 219 | * <0: error in the update 220 | */ 221 | c_int osqp_update_A(OSQPWorkspace *work, 222 | const c_float *Ax_new, 223 | const c_int *Ax_new_idx, 224 | c_int A_new_n); 225 | 226 | 227 | /** 228 | * Update elements of matrix P (upper triangular) and elements of matrix A 229 | * without changing sparsity structure. 230 | * 231 | * 232 | * If Px_new_idx is OSQP_NULL, Px_new is assumed to be as long as P->x 233 | * and the whole P->x is replaced. 234 | * 235 | * If Ax_new_idx is OSQP_NULL, Ax_new is assumed to be as long as A->x 236 | * and the whole A->x is replaced. 237 | * 238 | * @param work Workspace structure 239 | * @param Px_new Vector of new elements in P->x (upper triangular) 240 | * @param Px_new_idx Index mapping new elements to positions in P->x 241 | * @param P_new_n Number of new elements to be changed 242 | * @param Ax_new Vector of new elements in A->x 243 | * @param Ax_new_idx Index mapping new elements to positions in A->x 244 | * @param A_new_n Number of new elements to be changed 245 | * @return output flag: 0: OK 246 | * 1: P_new_n > nnzP 247 | * 2: A_new_n > nnzA 248 | * <0: error in the update 249 | */ 250 | c_int osqp_update_P_A(OSQPWorkspace *work, 251 | const c_float *Px_new, 252 | const c_int *Px_new_idx, 253 | c_int P_new_n, 254 | const c_float *Ax_new, 255 | const c_int *Ax_new_idx, 256 | c_int A_new_n); 257 | 258 | /** 259 | * Update rho. Limit it between RHO_MIN and RHO_MAX. 260 | * @param work Workspace 261 | * @param rho_new New rho setting 262 | * @return Exitflag 263 | */ 264 | c_int osqp_update_rho(OSQPWorkspace *work, 265 | c_float rho_new); 266 | 267 | # endif // if EMBEDDED != 1 268 | 269 | /** @} */ 270 | 271 | 272 | /** 273 | * @name Update settings 274 | * @{ 275 | */ 276 | 277 | 278 | /** 279 | * Update max_iter setting 280 | * @param work Workspace 281 | * @param max_iter_new New max_iter setting 282 | * @return Exitflag 283 | */ 284 | c_int osqp_update_max_iter(OSQPWorkspace *work, 285 | c_int max_iter_new); 286 | 287 | 288 | /** 289 | * Update absolute tolernace value 290 | * @param work Workspace 291 | * @param eps_abs_new New absolute tolerance value 292 | * @return Exitflag 293 | */ 294 | c_int osqp_update_eps_abs(OSQPWorkspace *work, 295 | c_float eps_abs_new); 296 | 297 | 298 | /** 299 | * Update relative tolernace value 300 | * @param work Workspace 301 | * @param eps_rel_new New relative tolerance value 302 | * @return Exitflag 303 | */ 304 | c_int osqp_update_eps_rel(OSQPWorkspace *work, 305 | c_float eps_rel_new); 306 | 307 | 308 | /** 309 | * Update primal infeasibility tolerance 310 | * @param work Workspace 311 | * @param eps_prim_inf_new New primal infeasibility tolerance 312 | * @return Exitflag 313 | */ 314 | c_int osqp_update_eps_prim_inf(OSQPWorkspace *work, 315 | c_float eps_prim_inf_new); 316 | 317 | 318 | /** 319 | * Update dual infeasibility tolerance 320 | * @param work Workspace 321 | * @param eps_dual_inf_new New dual infeasibility tolerance 322 | * @return Exitflag 323 | */ 324 | c_int osqp_update_eps_dual_inf(OSQPWorkspace *work, 325 | c_float eps_dual_inf_new); 326 | 327 | 328 | /** 329 | * Update relaxation parameter alpha 330 | * @param work Workspace 331 | * @param alpha_new New relaxation parameter value 332 | * @return Exitflag 333 | */ 334 | c_int osqp_update_alpha(OSQPWorkspace *work, 335 | c_float alpha_new); 336 | 337 | 338 | /** 339 | * Update warm_start setting 340 | * @param work Workspace 341 | * @param warm_start_new New warm_start setting 342 | * @return Exitflag 343 | */ 344 | c_int osqp_update_warm_start(OSQPWorkspace *work, 345 | c_int warm_start_new); 346 | 347 | 348 | /** 349 | * Update scaled_termination setting 350 | * @param work Workspace 351 | * @param scaled_termination_new New scaled_termination setting 352 | * @return Exitflag 353 | */ 354 | c_int osqp_update_scaled_termination(OSQPWorkspace *work, 355 | c_int scaled_termination_new); 356 | 357 | /** 358 | * Update check_termination setting 359 | * @param work Workspace 360 | * @param check_termination_new New check_termination setting 361 | * @return Exitflag 362 | */ 363 | c_int osqp_update_check_termination(OSQPWorkspace *work, 364 | c_int check_termination_new); 365 | 366 | 367 | # ifndef EMBEDDED 368 | 369 | /** 370 | * Update regularization parameter in polish 371 | * @param work Workspace 372 | * @param delta_new New regularization parameter 373 | * @return Exitflag 374 | */ 375 | c_int osqp_update_delta(OSQPWorkspace *work, 376 | c_float delta_new); 377 | 378 | 379 | /** 380 | * Update polish setting 381 | * @param work Workspace 382 | * @param polish_new New polish setting 383 | * @return Exitflag 384 | */ 385 | c_int osqp_update_polish(OSQPWorkspace *work, 386 | c_int polish_new); 387 | 388 | 389 | /** 390 | * Update number of iterative refinement steps in polish 391 | * @param work Workspace 392 | * @param polish_refine_iter_new New iterative reginement steps 393 | * @return Exitflag 394 | */ 395 | c_int osqp_update_polish_refine_iter(OSQPWorkspace *work, 396 | c_int polish_refine_iter_new); 397 | 398 | 399 | /** 400 | * Update verbose setting 401 | * @param work Workspace 402 | * @param verbose_new New verbose setting 403 | * @return Exitflag 404 | */ 405 | c_int osqp_update_verbose(OSQPWorkspace *work, 406 | c_int verbose_new); 407 | 408 | 409 | # endif // #ifndef EMBEDDED 410 | 411 | # ifdef PROFILING 412 | 413 | /** 414 | * Update time_limit setting 415 | * @param work Workspace 416 | * @param time_limit_new New time_limit setting 417 | * @return Exitflag 418 | */ 419 | c_int osqp_update_time_limit(OSQPWorkspace *work, 420 | c_float time_limit_new); 421 | # endif // ifdef PROFILING 422 | 423 | /** @} */ 424 | 425 | 426 | # ifdef __cplusplus 427 | } 428 | # endif // ifdef __cplusplus 429 | 430 | #endif // ifndef OSQP_H 431 | -------------------------------------------------------------------------------- /osqp/osqp_configure.h: -------------------------------------------------------------------------------- 1 | #ifndef OSQP_CONFIGURE_H 2 | # define OSQP_CONFIGURE_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif /* ifdef __cplusplus */ 7 | 8 | /* DEBUG */ 9 | /* #undef DEBUG */ 10 | 11 | /* Operating system */ 12 | #define IS_LINUX 13 | /* #undef IS_MAC */ 14 | /* #undef IS_WINDOWS */ 15 | 16 | /* EMBEDDED */ 17 | /* #undef EMBEDDED */ 18 | 19 | /* PRINTING */ 20 | #define PRINTING 21 | 22 | /* PROFILING */ 23 | #define PROFILING 24 | 25 | /* CTRLC */ 26 | #define CTRLC 27 | 28 | /* DFLOAT */ 29 | /* #undef DFLOAT */ 30 | 31 | /* DLONG */ 32 | #define DLONG 33 | 34 | /* ENABLE_MKL_PARDISO */ 35 | #define ENABLE_MKL_PARDISO 36 | 37 | /* MEMORY MANAGEMENT */ 38 | /* #undef OSQP_CUSTOM_MEMORY */ 39 | #ifdef OSQP_CUSTOM_MEMORY 40 | #include "" 41 | #endif 42 | 43 | 44 | 45 | # ifdef __cplusplus 46 | } 47 | # endif /* ifdef __cplusplus */ 48 | 49 | #endif /* ifndef OSQP_CONFIGURE_H */ 50 | -------------------------------------------------------------------------------- /osqp/osqp_problem.cc: -------------------------------------------------------------------------------- 1 | #include "osqp_problem.h" 2 | 3 | namespace { 4 | constexpr double kMaxVarRange = 1.0e10; 5 | } 6 | 7 | OSQPProblem::OSQPProblem(const size_t num_of_knots, const double delta_s, 8 | const std::array& x_init) { 9 | if (num_of_knots <= 2) { 10 | std::cout << "Error when init problem." << std::endl; 11 | return; 12 | } 13 | num_of_knots_ = num_of_knots; 14 | x_init_ = x_init; 15 | delta_s_ = delta_s; 16 | x_bounds_.resize(num_of_knots_, std::make_pair(-kMaxVarRange, kMaxVarRange)); 17 | dx_bounds_.resize(num_of_knots_, std::make_pair(-kMaxVarRange, kMaxVarRange)); 18 | ddx_bounds_.resize(num_of_knots_, 19 | std::make_pair(-kMaxVarRange, kMaxVarRange)); 20 | } 21 | 22 | void OSQPProblem::CalculateKernel(std::vector* P_data, 23 | std::vector* P_indices, 24 | std::vector* P_indptr) { 25 | 26 | const int n = static_cast(num_of_knots_); 27 | const int num_of_variables = 3 * n; 28 | const int num_of_nonzeros = num_of_variables + (n - 1); 29 | std::vector>> columns(num_of_variables); 30 | int value_index = 0; 31 | 32 | // x(i)^2 * (w_x + w_x_ref) 33 | for (int i = 0; i < n - 1; ++i) { 34 | columns[i].emplace_back(i, (weight_x_ + weight_x_ref_) / (scale_factor_[0] * scale_factor_[0])); 35 | ++value_index; 36 | } 37 | // x(n-1)^2 * (w_x + w_x_ref + w_end_x) 38 | columns[n - 1].emplace_back(n - 1, (weight_x_ + weight_x_ref_ + weight_end_state_[0]) / 39 | (scale_factor_[0] * scale_factor_[0])); 40 | ++value_index; 41 | 42 | // x(i)'^2 * w_dx 43 | for (int i = 0; i < n - 1; ++i) { 44 | columns[n + i].emplace_back(n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1])); 45 | ++value_index; 46 | } 47 | 48 | // x(n-1)'^2 * (w_dx + w_end_dx) 49 | columns[2 * n - 1].emplace_back(2 * n - 1, (weight_dx_ + weight_end_state_[1]) / 50 | (scale_factor_[1] * scale_factor_[1])); 51 | ++value_index; 52 | 53 | auto delta_s_square = delta_s_ * delta_s_; 54 | // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2) 55 | columns[2 * n].emplace_back(2 * n, 56 | (weight_ddx_ + weight_dddx_ / delta_s_square) / 57 | (scale_factor_[2] * scale_factor_[2])); 58 | ++value_index; 59 | for (int i = 1; i < n - 1; ++i) { 60 | columns[2 * n + i].emplace_back( 61 | 2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) / 62 | (scale_factor_[2] * scale_factor_[2])); 63 | ++value_index; 64 | } 65 | 66 | columns[3 * n - 1].emplace_back( 67 | 3 * n - 1, 68 | (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) / 69 | (scale_factor_[2] * scale_factor_[2])); 70 | ++value_index; 71 | 72 | // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)'' 73 | // for (int i = 1; i < n; ++i) { 74 | // columns[2 * n + i].emplace_back(2 * n + i - 1, 75 | // (-2.0 * weight_dddx_ / delta_s_square) / 76 | // (scale_factor_[2] * scale_factor_[2])); 77 | // ++value_index; 78 | // } 79 | 80 | if (value_index != num_of_nonzeros) { 81 | std::cout << "Error in calculate kernel!" << std::endl; 82 | } 83 | 84 | int ind_p = 0; 85 | for (int i = 0; i < num_of_variables; ++i) { 86 | P_indptr->push_back(ind_p); 87 | for (const auto& row_data_pair : columns[i]) { 88 | P_data->push_back(row_data_pair.second * 2.0); 89 | P_indices->push_back(row_data_pair.first); 90 | ++ind_p; 91 | } 92 | } 93 | 94 | P_indptr->push_back(ind_p); 95 | } 96 | 97 | void OSQPProblem::CalculateAffineConstraint( 98 | std::vector* A_data, std::vector* A_indices, 99 | std::vector* A_indptr, std::vector* lower_bounds, 100 | std::vector* upper_bounds) 101 | { 102 | 103 | // 3N params bounds on x, x', x'' 104 | // 3(N-1) constraints on x, x', x'' 105 | // 3 constraints on x_init_ 106 | 107 | const int n = static_cast(num_of_knots_); 108 | const int num_of_variables = 3 * n; 109 | const int num_of_constraints = num_of_variables + 3 * (n - 1) + 3; 110 | 111 | lower_bounds->resize(num_of_constraints); 112 | upper_bounds->resize(num_of_constraints); 113 | 114 | std::vector>> variables( 115 | num_of_variables); 116 | 117 | int constraint_index = 0; 118 | // set x, x', x'' bounds 119 | for (int i = 0; i < num_of_variables; ++i) { 120 | if (i < n) { 121 | variables[i].emplace_back(constraint_index, 1.0); 122 | lower_bounds->at(constraint_index) = 123 | x_bounds_[i].first * scale_factor_[0]; 124 | upper_bounds->at(constraint_index) = 125 | x_bounds_[i].second * scale_factor_[0]; 126 | } else if (i < 2 * n) { 127 | variables[i].emplace_back(constraint_index, 1.0); 128 | lower_bounds->at(constraint_index) = 129 | dx_bounds_[i - n].first * scale_factor_[1]; 130 | upper_bounds->at(constraint_index) = 131 | dx_bounds_[i - n].second * scale_factor_[1]; 132 | } else { 133 | variables[i].emplace_back(constraint_index, 1.0); 134 | lower_bounds->at(constraint_index) = 135 | ddx_bounds_[i - 2 * n].first * scale_factor_[2]; 136 | upper_bounds->at(constraint_index) = 137 | ddx_bounds_[i - 2 * n].second * scale_factor_[2]; 138 | } 139 | ++constraint_index; 140 | } 141 | 142 | if (constraint_index != num_of_variables) { 143 | std::cout << "Error in Calculate Affine Constraint 1" << std::endl; 144 | } 145 | 146 | // x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s 147 | for (int i = 0; i + 1 < n; ++i) { 148 | variables[2 * n + i].emplace_back(constraint_index, -1.0); 149 | variables[2 * n + i + 1].emplace_back(constraint_index, 1.0); 150 | lower_bounds->at(constraint_index) = 151 | dddx_bound_.first * delta_s_ * scale_factor_[2]; 152 | upper_bounds->at(constraint_index) = 153 | dddx_bound_.second * delta_s_ * scale_factor_[2]; 154 | ++constraint_index; 155 | if(lower_bounds->at(constraint_index) > 0){ 156 | std::cout<<"error"<at(constraint_index) = 0.0; 169 | upper_bounds->at(constraint_index) = 0.0; 170 | ++constraint_index; 171 | } 172 | 173 | // x(i+1) - x(i) - delta_s * x(i)' 174 | // - 1/3 * delta_s^2 * x(i)'' - 1/6 * delta_s^2 * x(i+1)'' 175 | 176 | auto delta_s_sq_ = delta_s_ * delta_s_; 177 | for (int i = 0; i + 1 < n; ++i) 178 | { 179 | variables[i].emplace_back(constraint_index, 180 | -1.0 * scale_factor_[1] * scale_factor_[2]); 181 | variables[i + 1].emplace_back(constraint_index, 182 | 1.0 * scale_factor_[1] * scale_factor_[2]); 183 | variables[n + i].emplace_back( 184 | constraint_index, -delta_s_ * scale_factor_[0] * scale_factor_[2]); 185 | variables[2 * n + i].emplace_back( 186 | constraint_index, 187 | -delta_s_sq_ / 3.0 * scale_factor_[0] * scale_factor_[1]); 188 | variables[2 * n + i + 1].emplace_back( 189 | constraint_index, 190 | -delta_s_sq_ / 6.0 * scale_factor_[0] * scale_factor_[1]); 191 | 192 | lower_bounds->at(constraint_index) = 0.0; 193 | upper_bounds->at(constraint_index) = 0.0; 194 | ++constraint_index; 195 | } 196 | 197 | // constrain on x_init 198 | variables[0].emplace_back(constraint_index, 1.0); 199 | lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0]; 200 | upper_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0]; 201 | ++constraint_index; 202 | 203 | variables[n].emplace_back(constraint_index, 1.0); 204 | lower_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1]; 205 | upper_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1]; 206 | ++constraint_index; 207 | 208 | variables[2 * n].emplace_back(constraint_index, 1.0); 209 | lower_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2]; 210 | upper_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2]; 211 | ++constraint_index; 212 | 213 | if (constraint_index != num_of_constraints) { 214 | std::cout << "Error in Calculate Affine Constraint 2" << std::endl; 215 | } 216 | 217 | int ind_p = 0; 218 | for (int i = 0; i < num_of_variables; ++i) { 219 | A_indptr->push_back(ind_p); 220 | for (const auto& variable_nz : variables[i]) { 221 | // coefficient 222 | A_data->push_back(variable_nz.second); 223 | 224 | // constraint index 225 | A_indices->push_back(variable_nz.first); 226 | ++ind_p; 227 | } 228 | } 229 | 230 | A_indptr->push_back(ind_p); 231 | } 232 | 233 | void OSQPProblem::CalculateOffset(std::vector* q) { 234 | if (q == nullptr) { 235 | std::cout << "q should not be nullptr!!" << std::endl; 236 | return; 237 | } 238 | const int n = static_cast(num_of_knots_); 239 | const int kNumParam = 3 * n; 240 | q->resize(kNumParam, 0.0); 241 | 242 | if (has_x_ref_) { 243 | for (int i = 0; i < n; ++i) { 244 | q->at(i) += -2.0 * weight_x_ref_ * x_ref_[i] / scale_factor_[0]; 245 | } 246 | } 247 | 248 | if (has_end_state_ref_) { 249 | q->at(n - 1) += 250 | -2.0 * weight_end_state_[0] * end_state_ref_[0] / scale_factor_[0]; 251 | q->at(2 * n - 1) += 252 | -2.0 * weight_end_state_[1] * end_state_ref_[1] / scale_factor_[1]; 253 | q->at(3 * n - 1) += 254 | -2.0 * weight_end_state_[2] * end_state_ref_[2] / scale_factor_[2]; 255 | } 256 | } 257 | 258 | OSQPData* OSQPProblem::FormulateProblem() { 259 | // calculate kernel 260 | std::vector P_data; 261 | std::vector P_indices; 262 | std::vector P_indptr; 263 | CalculateKernel(&P_data, &P_indices, &P_indptr); 264 | 265 | // calculate affine constraints 266 | std::vector A_data; 267 | std::vector A_indices; 268 | std::vector A_indptr; 269 | std::vector lower_bounds; 270 | std::vector upper_bounds; 271 | CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds, 272 | &upper_bounds); 273 | // calculate offset 274 | std::vector q; 275 | CalculateOffset(&q); 276 | 277 | OSQPData* data = reinterpret_cast(c_malloc(sizeof(OSQPData))); 278 | if (lower_bounds.size() != upper_bounds.size()) { 279 | std::cout << "Formulate data failed!" << std::endl; 280 | } 281 | 282 | size_t kernel_dim = 3 * num_of_knots_; 283 | size_t num_affine_constraint = lower_bounds.size(); 284 | 285 | data->n = kernel_dim; 286 | data->m = num_affine_constraint; 287 | data->P = csc_matrix(kernel_dim, kernel_dim, P_data.size(), CopyData(P_data), 288 | CopyData(P_indices), CopyData(P_indptr)); 289 | data->q = CopyData(q); 290 | data->A = 291 | csc_matrix(num_affine_constraint, kernel_dim, A_data.size(), 292 | CopyData(A_data), CopyData(A_indices), CopyData(A_indptr)); 293 | data->l = CopyData(lower_bounds); 294 | data->u = CopyData(upper_bounds); 295 | return data; 296 | } 297 | 298 | OSQPSettings* OSQPProblem::SolverDefaultSettings() { 299 | // Define Solver default settings 300 | OSQPSettings* settings = 301 | reinterpret_cast(c_malloc(sizeof(OSQPSettings))); 302 | osqp_set_default_settings(settings); 303 | settings->eps_abs = 5e-3; 304 | settings->eps_rel = 5e-3; 305 | settings->eps_prim_inf = 5e-4; 306 | settings->eps_dual_inf = 5e-4; 307 | settings->polish = true; 308 | settings->verbose = false; 309 | settings->scaled_termination = true; 310 | return settings; 311 | } 312 | 313 | bool OSQPProblem::Optimize(const int max_iter) { 314 | OSQPData* data = FormulateProblem(); 315 | 316 | OSQPSettings* settings = SolverDefaultSettings(); 317 | settings->max_iter = max_iter; 318 | 319 | OSQPWorkspace* osqp_work; 320 | 321 | c_int status = 0; 322 | 323 | status = osqp_setup(&osqp_work, data, settings); 324 | 325 | osqp_solve(osqp_work); 326 | 327 | if (status != 0) { 328 | std::cout << "failed optimization status:\t" << osqp_work->info->status 329 | << std::endl; 330 | osqp_cleanup(osqp_work); 331 | FreeData(data); 332 | c_free(settings); 333 | return false; 334 | } else if (osqp_work->solution == nullptr) { 335 | std::cout << "The solution from OSQP is nullptr" << std::endl; 336 | osqp_cleanup(osqp_work); 337 | FreeData(data); 338 | c_free(settings); 339 | return false; 340 | } 341 | 342 | // extract primal results 343 | x_.resize(num_of_knots_); 344 | dx_.resize(num_of_knots_); 345 | ddx_.resize(num_of_knots_); 346 | for (size_t i = 0; i < num_of_knots_; ++i) { 347 | x_.at(i) = osqp_work->solution->x[i] / scale_factor_[0]; 348 | dx_.at(i) = osqp_work->solution->x[i + num_of_knots_] / scale_factor_[1]; 349 | ddx_.at(i) = osqp_work->solution->x[i + 2 * num_of_knots_] / scale_factor_[2]; 350 | } 351 | 352 | // Cleanup 353 | osqp_cleanup(osqp_work); 354 | FreeData(data); 355 | c_free(settings); 356 | return true; 357 | } 358 | 359 | void OSQPProblem::FreeData(OSQPData* data) { 360 | delete[] data->q; 361 | delete[] data->l; 362 | delete[] data->u; 363 | 364 | delete[] data->P->i; 365 | delete[] data->P->p; 366 | delete[] data->P->x; 367 | 368 | delete[] data->A->i; 369 | delete[] data->A->p; 370 | delete[] data->A->x; 371 | } 372 | 373 | void OSQPProblem::set_end_state_ref( 374 | const std::array& weight_end_state, 375 | const std::array& end_state_ref) { 376 | weight_end_state_ = weight_end_state; 377 | end_state_ref_ = end_state_ref; 378 | has_end_state_ref_ = true; 379 | } 380 | 381 | void OSQPProblem::set_x_ref(const double weight_x_ref, 382 | std::vector x_ref) { 383 | if (x_ref.size() != num_of_knots_) { 384 | std::cout << "x_ref size is wrong!" << std::endl; 385 | return; 386 | } 387 | weight_x_ref_ = weight_x_ref; 388 | x_ref_ = x_ref; 389 | has_x_ref_ = true; 390 | } 391 | 392 | void OSQPProblem::set_dx_bounds(double low_bound, double up_bound) { 393 | for (auto& x : dx_bounds_) { 394 | x.first = low_bound; 395 | x.second = up_bound; 396 | } 397 | } 398 | 399 | void OSQPProblem::set_ddx_bounds(double low_bound, double up_bound) { 400 | for (auto& x : ddx_bounds_) { 401 | x.first = low_bound; 402 | x.second = up_bound; 403 | } 404 | } 405 | 406 | void OSQPProblem::set_dddx_bounds(double low, double up) { 407 | dddx_bound_.first = low; 408 | dddx_bound_.second = up; 409 | } 410 | -------------------------------------------------------------------------------- /osqp/osqp_problem.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | //#include "osqp.h" 6 | #include 7 | 8 | using namespace std; 9 | 10 | template 11 | T* CopyData(const std::vector& vec) { 12 | T* data = new T[vec.size()]; 13 | memcpy(data, vec.data(), sizeof(T) * vec.size()); 14 | return data; 15 | } 16 | 17 | class OSQPProblem { 18 | public: 19 | OSQPProblem(const size_t num_of_knots, const double delta_s, 20 | const std::array& x_init); 21 | void CalculateKernel(std::vector* P_data, 22 | std::vector* P_indices, 23 | std::vector* P_indptr); 24 | void CalculateAffineConstraint(std::vector* A_data, 25 | std::vector* A_indices, 26 | std::vector* A_indptr, 27 | std::vector* lower_bounds, 28 | std::vector* upper_bounds); 29 | void CalculateOffset(std::vector* q); 30 | OSQPSettings* SolverDefaultSettings(); 31 | OSQPData* FormulateProblem(); 32 | bool Optimize(const int max_iter); 33 | void FreeData(OSQPData* data); 34 | 35 | inline void set_weight_x(double w) { weight_x_ = w; } 36 | inline void set_weight_dx(double w) { weight_dx_ = w; } 37 | inline void set_weight_ddx(double w) { weight_ddx_ = w; } 38 | inline void set_weight_dddx(double w) { weight_dddx_ = w; } 39 | 40 | void set_end_state_ref(const std::array& weight_end_state, 41 | const std::array& end_state_ref); 42 | 43 | void set_x_ref(const double weight_x_ref, std::vector x_ref); 44 | 45 | inline void set_scale_factor(const std::array& factor) { scale_factor_ = factor; } 46 | 47 | inline void set_x_bounds(const std::vector>& x_bounds) { x_bounds_ = x_bounds; } 48 | void set_dx_bounds(double low_bound, double up_bound); 49 | void set_ddx_bounds(double low_bound, double up_bound); 50 | void set_dddx_bounds(double low, double up); 51 | 52 | // output 53 | std::vector x_; 54 | std::vector dx_; 55 | std::vector ddx_; 56 | 57 | private: 58 | size_t num_of_knots_; 59 | 60 | 61 | 62 | std::array x_init_; 63 | std::array scale_factor_ = {{1.0, 1.0, 1.0}}; 64 | 65 | std::vector> x_bounds_; 66 | std::vector> dx_bounds_; 67 | std::vector> ddx_bounds_; 68 | std::pair dddx_bound_; 69 | 70 | double weight_x_ = 1.0; 71 | double weight_dx_ = 1.0; 72 | double weight_ddx_ = 1.0; 73 | double weight_dddx_ = 1.0; 74 | 75 | double delta_s_ = 0.5; 76 | 77 | bool has_x_ref_ = false; 78 | double weight_x_ref_ = 1.0; 79 | std::vector x_ref_; 80 | 81 | bool has_end_state_ref_ = false; 82 | std::array weight_end_state_ = {{0.0, 0.0, 0.0}}; 83 | std::array end_state_ref_; 84 | }; 85 | -------------------------------------------------------------------------------- /osqp/polish.h: -------------------------------------------------------------------------------- 1 | /* Solution polish based on assuming the active set */ 2 | #ifndef POLISH_H 3 | # define POLISH_H 4 | 5 | # ifdef __cplusplus 6 | extern "C" { 7 | # endif // ifdef __cplusplus 8 | 9 | 10 | # include "types.h" 11 | 12 | /** 13 | * Solution polish: Solve equality constrained QP with assumed active 14 | *constraints 15 | * @param work Workspace 16 | * @return Exitflag 17 | */ 18 | c_int polish(OSQPWorkspace *work); 19 | 20 | 21 | # ifdef __cplusplus 22 | } 23 | # endif // ifdef __cplusplus 24 | 25 | #endif // ifndef POLISH_H 26 | -------------------------------------------------------------------------------- /osqp/proj.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJ_H 2 | # define PROJ_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" 9 | 10 | 11 | /* Define Projections onto set C involved in the ADMM algorithm */ 12 | 13 | /** 14 | * Project z onto \f$C = [l, u]\f$ 15 | * @param z Vector to project 16 | * @param work Workspace 17 | */ 18 | void project(OSQPWorkspace *work, 19 | c_float *z); 20 | 21 | 22 | /** 23 | * Ensure z satisfies box constraints and y is is normal cone of z 24 | * @param work Workspace 25 | * @param z Primal variable z 26 | * @param y Dual variable y 27 | */ 28 | void project_normalcone(OSQPWorkspace *work, 29 | c_float *z, 30 | c_float *y); 31 | 32 | 33 | # ifdef __cplusplus 34 | } 35 | # endif // ifdef __cplusplus 36 | 37 | #endif // ifndef PROJ_H 38 | -------------------------------------------------------------------------------- /osqp/scaling.h: -------------------------------------------------------------------------------- 1 | #ifndef SCALING_H 2 | # define SCALING_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | // Functions to scale problem data 9 | # include "types.h" 10 | # include "lin_alg.h" 11 | # include "constants.h" 12 | 13 | // Enable data scaling if EMBEDDED is disabled or if EMBEDDED == 2 14 | # if EMBEDDED != 1 15 | 16 | /** 17 | * Scale problem matrices 18 | * @param work Workspace 19 | * @return exitflag 20 | */ 21 | c_int scale_data(OSQPWorkspace *work); 22 | # endif // if EMBEDDED != 1 23 | 24 | 25 | /** 26 | * Unscale problem matrices 27 | * @param work Workspace 28 | * @return exitflag 29 | */ 30 | c_int unscale_data(OSQPWorkspace *work); 31 | 32 | 33 | /** 34 | * Unscale solution 35 | * @param work Workspace 36 | * @return exitflag 37 | */ 38 | c_int unscale_solution(OSQPWorkspace *work); 39 | 40 | # ifdef __cplusplus 41 | } 42 | # endif // ifdef __cplusplus 43 | 44 | #endif // ifndef SCALING_H 45 | -------------------------------------------------------------------------------- /osqp/types.h: -------------------------------------------------------------------------------- 1 | #ifndef OSQP_TYPES_H 2 | # define OSQP_TYPES_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "glob_opts.h" 9 | # include "constants.h" 10 | 11 | 12 | /****************** 13 | * Internal types * 14 | ******************/ 15 | 16 | /** 17 | * Matrix in compressed-column form. 18 | * The structure is used internally to store matrices in the triplet form as well, 19 | * but the API requires that the matrices are in the CSC format. 20 | */ 21 | typedef struct { 22 | c_int nzmax; ///< maximum number of entries 23 | c_int m; ///< number of rows 24 | c_int n; ///< number of columns 25 | c_int *p; ///< column pointers (size n+1); col indices (size nzmax) start from 0 when using triplet format (direct KKT matrix formation) 26 | c_int *i; ///< row indices, size nzmax starting from 0 27 | c_float *x; ///< numerical values, size nzmax 28 | c_int nz; ///< number of entries in triplet matrix, -1 for csc 29 | } csc; 30 | 31 | /** 32 | * Linear system solver structure (sublevel objects initialize it differently) 33 | */ 34 | 35 | typedef struct linsys_solver LinSysSolver; 36 | 37 | /** 38 | * OSQP Timer for statistics 39 | */ 40 | typedef struct OSQP_TIMER OSQPTimer; 41 | 42 | /** 43 | * Problem scaling matrices stored as vectors 44 | */ 45 | typedef struct { 46 | c_float c; ///< cost function scaling 47 | c_float *D; ///< primal variable scaling 48 | c_float *E; ///< dual variable scaling 49 | c_float cinv; ///< cost function rescaling 50 | c_float *Dinv; ///< primal variable rescaling 51 | c_float *Einv; ///< dual variable rescaling 52 | } OSQPScaling; 53 | 54 | /** 55 | * Solution structure 56 | */ 57 | typedef struct { 58 | c_float *x; ///< primal solution 59 | c_float *y; ///< Lagrange multiplier associated to \f$l <= Ax <= u\f$ 60 | } OSQPSolution; 61 | 62 | 63 | /** 64 | * Solver return information 65 | */ 66 | typedef struct { 67 | c_int iter; ///< number of iterations taken 68 | char status[32]; ///< status string, e.g. 'solved' 69 | c_int status_val; ///< status as c_int, defined in constants.h 70 | 71 | # ifndef EMBEDDED 72 | c_int status_polish; ///< polish status: successful (1), unperformed (0), (-1) unsuccessful 73 | # endif // ifndef EMBEDDED 74 | 75 | c_float obj_val; ///< primal objective 76 | c_float pri_res; ///< norm of primal residual 77 | c_float dua_res; ///< norm of dual residual 78 | 79 | # ifdef PROFILING 80 | c_float setup_time; ///< time taken for setup phase (seconds) 81 | c_float solve_time; ///< time taken for solve phase (seconds) 82 | c_float update_time; ///< time taken for update phase (seconds) 83 | c_float polish_time; ///< time taken for polish phase (seconds) 84 | c_float run_time; ///< total time (seconds) 85 | # endif // ifdef PROFILING 86 | 87 | # if EMBEDDED != 1 88 | c_int rho_updates; ///< number of rho updates 89 | c_float rho_estimate; ///< best rho estimate so far from residuals 90 | # endif // if EMBEDDED != 1 91 | } OSQPInfo; 92 | 93 | 94 | # ifndef EMBEDDED 95 | 96 | /** 97 | * Polish structure 98 | */ 99 | typedef struct { 100 | csc *Ared; ///< active rows of A 101 | ///< Ared = vstack[Alow, Aupp] 102 | c_int n_low; ///< number of lower-active rows 103 | c_int n_upp; ///< number of upper-active rows 104 | c_int *A_to_Alow; ///< Maps indices in A to indices in Alow 105 | c_int *A_to_Aupp; ///< Maps indices in A to indices in Aupp 106 | c_int *Alow_to_A; ///< Maps indices in Alow to indices in A 107 | c_int *Aupp_to_A; ///< Maps indices in Aupp to indices in A 108 | c_float *x; ///< optimal x-solution obtained by polish 109 | c_float *z; ///< optimal z-solution obtained by polish 110 | c_float *y; ///< optimal y-solution obtained by polish 111 | c_float obj_val; ///< objective value at polished solution 112 | c_float pri_res; ///< primal residual at polished solution 113 | c_float dua_res; ///< dual residual at polished solution 114 | } OSQPPolish; 115 | # endif // ifndef EMBEDDED 116 | 117 | 118 | /********************************** 119 | * Main structures and Data Types * 120 | **********************************/ 121 | 122 | /** 123 | * Data structure 124 | */ 125 | typedef struct { 126 | c_int n; ///< number of variables n 127 | c_int m; ///< number of constraints m 128 | csc *P; ///< the upper triangular part of the quadratic cost matrix P in csc format (size n x n). 129 | csc *A; ///< linear constraints matrix A in csc format (size m x n) 130 | c_float *q; ///< dense array for linear part of cost function (size n) 131 | c_float *l; ///< dense array for lower bound (size m) 132 | c_float *u; ///< dense array for upper bound (size m) 133 | } OSQPData; 134 | 135 | 136 | /** 137 | * Settings struct 138 | */ 139 | typedef struct { 140 | c_float rho; ///< ADMM step rho 141 | c_float sigma; ///< ADMM step sigma 142 | c_int scaling; ///< heuristic data scaling iterations; if 0, then disabled. 143 | 144 | # if EMBEDDED != 1 145 | c_int adaptive_rho; ///< boolean, is rho step size adaptive? 146 | c_int adaptive_rho_interval; ///< number of iterations between rho adaptations; if 0, then it is automatic 147 | c_float adaptive_rho_tolerance; ///< tolerance X for adapting rho. The new rho has to be X times larger or 1/X times smaller than the current one to trigger a new factorization. 148 | # ifdef PROFILING 149 | c_float adaptive_rho_fraction; ///< interval for adapting rho (fraction of the setup time) 150 | # endif // Profiling 151 | # endif // EMBEDDED != 1 152 | 153 | c_int max_iter; ///< maximum number of iterations 154 | c_float eps_abs; ///< absolute convergence tolerance 155 | c_float eps_rel; ///< relative convergence tolerance 156 | c_float eps_prim_inf; ///< primal infeasibility tolerance 157 | c_float eps_dual_inf; ///< dual infeasibility tolerance 158 | c_float alpha; ///< relaxation parameter 159 | enum linsys_solver_type linsys_solver; ///< linear system solver to use 160 | 161 | # ifndef EMBEDDED 162 | c_float delta; ///< regularization parameter for polishing 163 | c_int polish; ///< boolean, polish ADMM solution 164 | c_int polish_refine_iter; ///< number of iterative refinement steps in polishing 165 | 166 | c_int verbose; ///< boolean, write out progress 167 | # endif // ifndef EMBEDDED 168 | 169 | c_int scaled_termination; ///< boolean, use scaled termination criteria 170 | c_int check_termination; ///< integer, check termination interval; if 0, then termination checking is disabled 171 | c_int warm_start; ///< boolean, warm start 172 | 173 | # ifdef PROFILING 174 | c_float time_limit; ///< maximum number of seconds allowed to solve the problem; if 0, then disabled 175 | # endif // ifdef PROFILING 176 | } OSQPSettings; 177 | 178 | 179 | /** 180 | * OSQP Workspace 181 | */ 182 | typedef struct { 183 | /// Problem data to work on (possibly scaled) 184 | OSQPData *data; 185 | 186 | /// Linear System solver structure 187 | LinSysSolver *linsys_solver; 188 | 189 | # ifndef EMBEDDED 190 | /// Polish structure 191 | OSQPPolish *pol; 192 | # endif // ifndef EMBEDDED 193 | 194 | /** 195 | * @name Vector used to store a vectorized rho parameter 196 | * @{ 197 | */ 198 | c_float *rho_vec; ///< vector of rho values 199 | c_float *rho_inv_vec; ///< vector of inv rho values 200 | 201 | /** @} */ 202 | 203 | # if EMBEDDED != 1 204 | c_int *constr_type; ///< Type of constraints: loose (-1), equality (1), inequality (0) 205 | # endif // if EMBEDDED != 1 206 | 207 | /** 208 | * @name Iterates 209 | * @{ 210 | */ 211 | c_float *x; ///< Iterate x 212 | c_float *y; ///< Iterate y 213 | c_float *z; ///< Iterate z 214 | c_float *xz_tilde; ///< Iterate xz_tilde 215 | 216 | c_float *x_prev; ///< Previous x 217 | 218 | /**< NB: Used also as workspace vector for dual residual */ 219 | c_float *z_prev; ///< Previous z 220 | 221 | /**< NB: Used also as workspace vector for primal residual */ 222 | 223 | /** 224 | * @name Primal and dual residuals workspace variables 225 | * 226 | * Needed for residuals computation, tolerances computation, 227 | * approximate tolerances computation and adapting rho 228 | * @{ 229 | */ 230 | c_float *Ax; ///< scaled A * x 231 | c_float *Px; ///< scaled P * x 232 | c_float *Aty; ///< scaled A' * y 233 | 234 | /** @} */ 235 | 236 | /** 237 | * @name Primal infeasibility variables 238 | * @{ 239 | */ 240 | c_float *delta_y; ///< difference between consecutive dual iterates 241 | c_float *Atdelta_y; ///< A' * delta_y 242 | 243 | /** @} */ 244 | 245 | /** 246 | * @name Dual infeasibility variables 247 | * @{ 248 | */ 249 | c_float *delta_x; ///< difference between consecutive primal iterates 250 | c_float *Pdelta_x; ///< P * delta_x 251 | c_float *Adelta_x; ///< A * delta_x 252 | 253 | /** @} */ 254 | 255 | /** 256 | * @name Temporary vectors used in scaling 257 | * @{ 258 | */ 259 | 260 | c_float *D_temp; ///< temporary primal variable scaling vectors 261 | c_float *D_temp_A; ///< temporary primal variable scaling vectors storing norms of A columns 262 | c_float *E_temp; ///< temporary constraints scaling vectors storing norms of A' columns 263 | 264 | 265 | /** @} */ 266 | 267 | OSQPSettings *settings; ///< problem settings 268 | OSQPScaling *scaling; ///< scaling vectors 269 | OSQPSolution *solution; ///< problem solution 270 | OSQPInfo *info; ///< solver information 271 | 272 | # ifdef PROFILING 273 | OSQPTimer *timer; ///< timer object 274 | 275 | /// flag indicating whether the solve function has been run before 276 | c_int first_run; 277 | 278 | /// flag indicating whether the update_time should be cleared 279 | c_int clear_update_time; 280 | 281 | /// flag indicating that osqp_update_rho is called from osqp_solve function 282 | c_int rho_update_from_solve; 283 | # endif // ifdef PROFILING 284 | 285 | # ifdef PRINTING 286 | c_int summary_printed; ///< Has last summary been printed? (true/false) 287 | # endif // ifdef PRINTING 288 | 289 | } OSQPWorkspace; 290 | 291 | 292 | /** 293 | * Define linsys_solver prototype structure 294 | * 295 | * NB: The details are defined when the linear solver is initialized depending 296 | * on the choice 297 | */ 298 | struct linsys_solver { 299 | enum linsys_solver_type type; ///< linear system solver type functions 300 | c_int (*solve)(LinSysSolver *self, 301 | c_float *b); ///< solve linear system 302 | 303 | # ifndef EMBEDDED 304 | void (*free)(LinSysSolver *self); ///< free linear system solver (only in desktop version) 305 | # endif // ifndef EMBEDDED 306 | 307 | # if EMBEDDED != 1 308 | c_int (*update_matrices)(LinSysSolver *s, 309 | const csc *P, ///< update matrices P 310 | const csc *A); // and A in the solver 311 | 312 | c_int (*update_rho_vec)(LinSysSolver *s, 313 | const c_float *rho_vec); ///< Update rho_vec 314 | # endif // if EMBEDDED != 1 315 | 316 | # ifndef EMBEDDED 317 | c_int nthreads; ///< number of threads active 318 | # endif // ifndef EMBEDDED 319 | }; 320 | 321 | 322 | # ifdef __cplusplus 323 | } 324 | # endif // ifdef __cplusplus 325 | 326 | #endif // ifndef OSQP_TYPES_H 327 | -------------------------------------------------------------------------------- /osqp/util.h: -------------------------------------------------------------------------------- 1 | #ifndef UTIL_H 2 | # define UTIL_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" 9 | # include "constants.h" 10 | 11 | /****************** 12 | * Versioning * 13 | ******************/ 14 | 15 | /** 16 | * Return OSQP version 17 | * @return OSQP version 18 | */ 19 | const char* osqp_version(void); 20 | 21 | 22 | /********************** 23 | * Utility Functions * 24 | **********************/ 25 | 26 | # ifndef EMBEDDED 27 | 28 | /** 29 | * Copy settings creating a new settings structure (uses MALLOC) 30 | * @param settings Settings to be copied 31 | * @return New settings structure 32 | */ 33 | OSQPSettings* copy_settings(const OSQPSettings *settings); 34 | 35 | # endif // #ifndef EMBEDDED 36 | 37 | /** 38 | * Custom string copy to avoid string.h library 39 | * @param dest destination string 40 | * @param source source string 41 | */ 42 | void c_strcpy(char dest[], 43 | const char source[]); 44 | 45 | 46 | # ifdef PRINTING 47 | 48 | /** 49 | * Print Header before running the algorithm 50 | * @param work osqp workspace 51 | */ 52 | void print_setup_header(const OSQPWorkspace *work); 53 | 54 | /** 55 | * Print header with data to be displayed per iteration 56 | */ 57 | void print_header(void); 58 | 59 | /** 60 | * Print iteration summary 61 | * @param work current workspace 62 | */ 63 | void print_summary(OSQPWorkspace *work); 64 | 65 | /** 66 | * Print information after polish 67 | * @param work current workspace 68 | */ 69 | void print_polish(OSQPWorkspace *work); 70 | 71 | /** 72 | * Print footer when algorithm terminates 73 | * @param info info structure 74 | * @param polish is polish enabled? 75 | */ 76 | void print_footer(OSQPInfo *info, 77 | c_int polish); 78 | 79 | 80 | # endif // ifdef PRINTING 81 | 82 | 83 | /********************************* 84 | * Timer Structs and Functions * * 85 | *********************************/ 86 | 87 | /*! \cond PRIVATE */ 88 | 89 | # ifdef PROFILING 90 | 91 | // Windows 92 | # ifdef IS_WINDOWS 93 | 94 | // Some R packages clash with elements 95 | // of the windows.h header, so use a 96 | // slimmer version for conflict avoidance 97 | # ifdef R_LANG 98 | #define NOGDI 99 | # endif 100 | 101 | # include 102 | 103 | struct OSQP_TIMER { 104 | LARGE_INTEGER tic; 105 | LARGE_INTEGER toc; 106 | LARGE_INTEGER freq; 107 | }; 108 | 109 | // Mac 110 | # elif defined IS_MAC 111 | 112 | # include 113 | 114 | /* Use MAC OSX mach_time for timing */ 115 | struct OSQP_TIMER { 116 | uint64_t tic; 117 | uint64_t toc; 118 | mach_timebase_info_data_t tinfo; 119 | }; 120 | 121 | // Linux 122 | # else // ifdef IS_WINDOWS 123 | 124 | /* Use POSIX clock_gettime() for timing on non-Windows machines */ 125 | # include 126 | # include 127 | 128 | 129 | struct OSQP_TIMER { 130 | struct timespec tic; 131 | struct timespec toc; 132 | }; 133 | 134 | # endif // ifdef IS_WINDOWS 135 | 136 | /*! \endcond */ 137 | 138 | /** 139 | * Timer Methods 140 | */ 141 | 142 | /** 143 | * Start timer 144 | * @param t Timer object 145 | */ 146 | void osqp_tic(OSQPTimer *t); 147 | 148 | /** 149 | * Report time 150 | * @param t Timer object 151 | * @return Reported time 152 | */ 153 | c_float osqp_toc(OSQPTimer *t); 154 | 155 | # endif /* END #ifdef PROFILING */ 156 | 157 | 158 | /* ================================= DEBUG FUNCTIONS ======================= */ 159 | 160 | /*! \cond PRIVATE */ 161 | 162 | 163 | # ifndef EMBEDDED 164 | 165 | /* Compare CSC matrices */ 166 | c_int is_eq_csc(csc *A, 167 | csc *B, 168 | c_float tol); 169 | 170 | /* Convert sparse CSC to dense */ 171 | c_float* csc_to_dns(csc *M); 172 | 173 | # endif // #ifndef EMBEDDED 174 | 175 | 176 | # ifdef PRINTING 177 | # include 178 | 179 | 180 | /* Print a csc sparse matrix */ 181 | void print_csc_matrix(csc *M, 182 | const char *name); 183 | 184 | /* Dump csc sparse matrix to file */ 185 | void dump_csc_matrix(csc *M, 186 | const char *file_name); 187 | 188 | /* Print a triplet format sparse matrix */ 189 | void print_trip_matrix(csc *M, 190 | const char *name); 191 | 192 | /* Print a dense matrix */ 193 | void print_dns_matrix(c_float *M, 194 | c_int m, 195 | c_int n, 196 | const char *name); 197 | 198 | /* Print vector */ 199 | void print_vec(c_float *v, 200 | c_int n, 201 | const char *name); 202 | 203 | /* Dump vector to file */ 204 | void dump_vec(c_float *v, 205 | c_int len, 206 | const char *file_name); 207 | 208 | // Print int array 209 | void print_vec_int(c_int *x, 210 | c_int n, 211 | const char *name); 212 | 213 | # endif // ifdef PRINTING 214 | 215 | /*! \endcond */ 216 | 217 | 218 | # ifdef __cplusplus 219 | } 220 | # endif // ifdef __cplusplus 221 | 222 | #endif // ifndef UTIL_H 223 | -------------------------------------------------------------------------------- /osqp_configure.h: -------------------------------------------------------------------------------- 1 | #ifndef OSQP_CONFIGURE_H 2 | # define OSQP_CONFIGURE_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif /* ifdef __cplusplus */ 7 | 8 | /* DEBUG */ 9 | /* #undef DEBUG */ 10 | 11 | /* Operating system */ 12 | #define IS_LINUX 13 | /* #undef IS_MAC */ 14 | /* #undef IS_WINDOWS */ 15 | 16 | /* EMBEDDED */ 17 | /* #undef EMBEDDED */ 18 | 19 | /* PRINTING */ 20 | #define PRINTING 21 | 22 | /* PROFILING */ 23 | #define PROFILING 24 | 25 | /* CTRLC */ 26 | #define CTRLC 27 | 28 | /* DFLOAT */ 29 | /* #undef DFLOAT */ 30 | 31 | /* DLONG */ 32 | #define DLONG 33 | 34 | /* ENABLE_MKL_PARDISO */ 35 | #define ENABLE_MKL_PARDISO 36 | 37 | /* MEMORY MANAGEMENT */ 38 | /* #undef OSQP_CUSTOM_MEMORY */ 39 | #ifdef OSQP_CUSTOM_MEMORY 40 | #include "" 41 | #endif 42 | 43 | 44 | 45 | # ifdef __cplusplus 46 | } 47 | # endif /* ifdef __cplusplus */ 48 | 49 | #endif /* ifndef OSQP_CONFIGURE_H */ 50 | -------------------------------------------------------------------------------- /osqp_problem.cc: -------------------------------------------------------------------------------- 1 | #include "osqp_problem.h" 2 | 3 | namespace { 4 | constexpr double kMaxVarRange = 1.0e10; 5 | } 6 | 7 | OSQPProblem::OSQPProblem(const size_t num_of_knots, const double delta_s, 8 | const std::array& x_init) { 9 | if (num_of_knots <= 2) { 10 | std::cout << "Error when init problem." << std::endl; 11 | return; 12 | } 13 | num_of_knots_ = num_of_knots; 14 | x_init_ = x_init; 15 | delta_s_ = delta_s; 16 | x_bounds_.resize(num_of_knots_, std::make_pair(-kMaxVarRange, kMaxVarRange)); 17 | dx_bounds_.resize(num_of_knots_, std::make_pair(-kMaxVarRange, kMaxVarRange)); 18 | ddx_bounds_.resize(num_of_knots_, 19 | std::make_pair(-kMaxVarRange, kMaxVarRange)); 20 | } 21 | 22 | void OSQPProblem::CalculateKernel(std::vector* P_data, 23 | std::vector* P_indices, 24 | std::vector* P_indptr) { 25 | 26 | const int n = static_cast(num_of_knots_); 27 | const int num_of_variables = 3 * n; 28 | const int num_of_nonzeros = num_of_variables + (n - 1); 29 | std::vector>> columns(num_of_variables); 30 | int value_index = 0; 31 | 32 | // x(i)^2 * (w_x + w_x_ref) 33 | for (int i = 0; i < n - 1; ++i) { 34 | columns[i].emplace_back(i, (weight_x_ + weight_x_ref_) / (scale_factor_[0] * scale_factor_[0])); 35 | ++value_index; 36 | } 37 | // x(n-1)^2 * (w_x + w_x_ref + w_end_x) 38 | columns[n - 1].emplace_back(n - 1, (weight_x_ + weight_x_ref_ + weight_end_state_[0]) / 39 | (scale_factor_[0] * scale_factor_[0])); 40 | ++value_index; 41 | 42 | // x(i)'^2 * w_dx 43 | for (int i = 0; i < n - 1; ++i) { 44 | columns[n + i].emplace_back(n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1])); 45 | ++value_index; 46 | } 47 | 48 | // x(n-1)'^2 * (w_dx + w_end_dx) 49 | columns[2 * n - 1].emplace_back(2 * n - 1, (weight_dx_ + weight_end_state_[1]) / 50 | (scale_factor_[1] * scale_factor_[1])); 51 | ++value_index; 52 | 53 | auto delta_s_square = delta_s_ * delta_s_; 54 | // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2) 55 | columns[2 * n].emplace_back(2 * n, 56 | (weight_ddx_ + weight_dddx_ / delta_s_square) / 57 | (scale_factor_[2] * scale_factor_[2])); 58 | ++value_index; 59 | for (int i = 1; i < n - 1; ++i) { 60 | columns[2 * n + i].emplace_back( 61 | 2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) / 62 | (scale_factor_[2] * scale_factor_[2])); 63 | ++value_index; 64 | } 65 | 66 | columns[3 * n - 1].emplace_back( 67 | 3 * n - 1, 68 | (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) / 69 | (scale_factor_[2] * scale_factor_[2])); 70 | ++value_index; 71 | 72 | // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)'' 73 | for (int i = 1; i < n; ++i) { 74 | columns[2 * n + i].emplace_back(2 * n + i - 1, 75 | (0.0 * weight_dddx_ / delta_s_square) / 76 | (scale_factor_[2] * scale_factor_[2])); 77 | ++value_index; 78 | } 79 | 80 | if (value_index != num_of_nonzeros) { 81 | std::cout << "Error in calculate kernel!" << std::endl; 82 | } 83 | 84 | int ind_p = 0; 85 | for (int i = 0; i < num_of_variables; ++i) { 86 | 87 | P_indptr->push_back(ind_p); 88 | for (const auto& row_data_pair : columns[i]) { 89 | P_data->push_back(row_data_pair.second * 2.0); 90 | P_indices->push_back(row_data_pair.first); 91 | ++ind_p; 92 | } 93 | } 94 | 95 | P_indptr->push_back(ind_p); 96 | } 97 | 98 | void OSQPProblem::CalculateAffineConstraint( 99 | std::vector* A_data, std::vector* A_indices, 100 | std::vector* A_indptr, std::vector* lower_bounds, 101 | std::vector* upper_bounds) 102 | { 103 | 104 | // 3N params bounds on x, x', x'' 105 | // 3(N-1) constraints on x, x', x'' 106 | // 3 constraints on x_init_ 107 | 108 | const int n = static_cast(num_of_knots_); 109 | const int num_of_variables = 3 * n; 110 | const int num_of_constraints = num_of_variables + 3 * (n - 1) + 3; 111 | 112 | lower_bounds->resize(num_of_constraints); 113 | upper_bounds->resize(num_of_constraints); 114 | 115 | std::vector>> variables( 116 | num_of_variables); 117 | 118 | int constraint_index = 0; 119 | // set x, x', x'' bounds 120 | for (int i = 0; i < num_of_variables; ++i) { 121 | if (i < n) { 122 | variables[i].emplace_back(constraint_index, 1.0); 123 | lower_bounds->at(constraint_index) = 124 | x_bounds_[i].first * scale_factor_[0]; 125 | upper_bounds->at(constraint_index) = 126 | x_bounds_[i].second * scale_factor_[0]; 127 | } else if (i < 2 * n) { 128 | variables[i].emplace_back(constraint_index, 1.0); 129 | lower_bounds->at(constraint_index) = 130 | dx_bounds_[i - n].first * scale_factor_[1]; 131 | upper_bounds->at(constraint_index) = 132 | dx_bounds_[i - n].second * scale_factor_[1]; 133 | } else { 134 | variables[i].emplace_back(constraint_index, 1.0); 135 | lower_bounds->at(constraint_index) = 136 | ddx_bounds_[i - 2 * n].first * scale_factor_[2]; 137 | upper_bounds->at(constraint_index) = 138 | ddx_bounds_[i - 2 * n].second * scale_factor_[2]; 139 | } 140 | ++constraint_index; 141 | } 142 | 143 | if (constraint_index != num_of_variables) { 144 | std::cout << "Error in Calculate Affine Constraint 1" << std::endl; 145 | } 146 | 147 | // x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s 148 | for (int i = 0; i + 1 < n; ++i) { 149 | variables[2 * n + i].emplace_back(constraint_index, -1.0); 150 | variables[2 * n + i + 1].emplace_back(constraint_index, 1.0); 151 | lower_bounds->at(constraint_index) = 152 | dddx_bound_.first * delta_s_ * scale_factor_[2]; 153 | upper_bounds->at(constraint_index) = 154 | dddx_bound_.second * delta_s_ * scale_factor_[2]; 155 | ++constraint_index; 156 | if(lower_bounds->at(constraint_index) > 0){ 157 | std::cout<<"error"<at(constraint_index) = 0.0; 170 | upper_bounds->at(constraint_index) = 0.0; 171 | ++constraint_index; 172 | } 173 | 174 | // x(i+1) - x(i) - delta_s * x(i)' 175 | // - 1/3 * delta_s^2 * x(i)'' - 1/6 * delta_s^2 * x(i+1)'' 176 | 177 | auto delta_s_sq_ = delta_s_ * delta_s_; 178 | for (int i = 0; i + 1 < n; ++i) 179 | { 180 | variables[i].emplace_back(constraint_index, 181 | -1.0 * scale_factor_[1] * scale_factor_[2]); 182 | variables[i + 1].emplace_back(constraint_index, 183 | 1.0 * scale_factor_[1] * scale_factor_[2]); 184 | variables[n + i].emplace_back( 185 | constraint_index, -delta_s_ * scale_factor_[0] * scale_factor_[2]); 186 | variables[2 * n + i].emplace_back( 187 | constraint_index, 188 | -delta_s_sq_ / 3.0 * scale_factor_[0] * scale_factor_[1]); 189 | variables[2 * n + i + 1].emplace_back( 190 | constraint_index, 191 | -delta_s_sq_ / 6.0 * scale_factor_[0] * scale_factor_[1]); 192 | 193 | lower_bounds->at(constraint_index) = 0.0; 194 | upper_bounds->at(constraint_index) = 0.0; 195 | ++constraint_index; 196 | } 197 | 198 | // constrain on x_init 199 | variables[0].emplace_back(constraint_index, 1.0); 200 | lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0]; 201 | upper_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0]; 202 | ++constraint_index; 203 | 204 | variables[n].emplace_back(constraint_index, 1.0); 205 | lower_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1]; 206 | upper_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1]; 207 | ++constraint_index; 208 | 209 | variables[2 * n].emplace_back(constraint_index, 1.0); 210 | lower_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2]; 211 | upper_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2]; 212 | ++constraint_index; 213 | 214 | if (constraint_index != num_of_constraints) { 215 | std::cout << "Error in Calculate Affine Constraint 2" << std::endl; 216 | } 217 | 218 | int ind_p = 0; 219 | for (int i = 0; i < num_of_variables; ++i) { 220 | A_indptr->push_back(ind_p); 221 | for (const auto& variable_nz : variables[i]) { 222 | // coefficient 223 | A_data->push_back(variable_nz.second); 224 | 225 | // constraint index 226 | A_indices->push_back(variable_nz.first); 227 | ++ind_p; 228 | } 229 | } 230 | 231 | A_indptr->push_back(ind_p); 232 | } 233 | 234 | void OSQPProblem::CalculateOffset(std::vector* q) { 235 | if (q == nullptr) { 236 | std::cout << "q should not be nullptr!!" << std::endl; 237 | return; 238 | } 239 | const int n = static_cast(num_of_knots_); 240 | const int kNumParam = 3 * n; 241 | q->resize(kNumParam, 0.0); 242 | 243 | if (has_x_ref_) { 244 | for (int i = 0; i < n; ++i) { 245 | q->at(i) += -2.0 * weight_x_ref_ * x_ref_[i] / scale_factor_[0]; 246 | } 247 | } 248 | 249 | if (has_end_state_ref_) { 250 | q->at(n - 1) += 251 | -2.0 * weight_end_state_[0] * end_state_ref_[0] / scale_factor_[0]; 252 | q->at(2 * n - 1) += 253 | -2.0 * weight_end_state_[1] * end_state_ref_[1] / scale_factor_[1]; 254 | q->at(3 * n - 1) += 255 | -2.0 * weight_end_state_[2] * end_state_ref_[2] / scale_factor_[2]; 256 | } 257 | } 258 | 259 | OSQPData* OSQPProblem::FormulateProblem() { 260 | // calculate kernel 261 | std::vector P_data; 262 | std::vector P_indices; 263 | std::vector P_indptr; 264 | CalculateKernel(&P_data, &P_indices, &P_indptr); 265 | 266 | // calculate affine constraints 267 | std::vector A_data; 268 | std::vector A_indices; 269 | std::vector A_indptr; 270 | std::vector lower_bounds; 271 | std::vector upper_bounds; 272 | CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds, 273 | &upper_bounds); 274 | // calculate offset 275 | std::vector q; 276 | CalculateOffset(&q); 277 | 278 | OSQPData* data = reinterpret_cast(c_malloc(sizeof(OSQPData))); 279 | if (lower_bounds.size() != upper_bounds.size()) { 280 | std::cout << "Formulate data failed!" << std::endl; 281 | } 282 | 283 | size_t kernel_dim = 3 * num_of_knots_; 284 | size_t num_affine_constraint = lower_bounds.size(); 285 | 286 | data->n = kernel_dim; 287 | data->m = num_affine_constraint; 288 | data->P = csc_matrix(kernel_dim, kernel_dim, P_data.size(), CopyData(P_data), 289 | CopyData(P_indices), CopyData(P_indptr)); 290 | data->q = CopyData(q); 291 | data->A = 292 | csc_matrix(num_affine_constraint, kernel_dim, A_data.size(), 293 | CopyData(A_data), CopyData(A_indices), CopyData(A_indptr)); 294 | data->l = CopyData(lower_bounds); 295 | data->u = CopyData(upper_bounds); 296 | return data; 297 | } 298 | 299 | OSQPSettings* OSQPProblem::SolverDefaultSettings() { 300 | // Define Solver default settings 301 | OSQPSettings* settings = 302 | reinterpret_cast(c_malloc(sizeof(OSQPSettings))); 303 | osqp_set_default_settings(settings); 304 | settings->eps_abs = 5e-3; 305 | settings->eps_rel = 5e-3; 306 | settings->eps_prim_inf = 5e-4; 307 | settings->eps_dual_inf = 5e-4; 308 | settings->polish = true; 309 | settings->verbose = false; 310 | settings->scaled_termination = true; 311 | return settings; 312 | } 313 | 314 | bool OSQPProblem::Optimize(const int max_iter) { 315 | 316 | OSQPData* data = FormulateProblem(); 317 | 318 | OSQPSettings* settings = SolverDefaultSettings(); 319 | settings->max_iter = max_iter; 320 | 321 | OSQPWorkspace* osqp_work; 322 | 323 | c_int status = 0; 324 | 325 | status = osqp_setup(&osqp_work, data, settings); 326 | 327 | osqp_solve(osqp_work); 328 | 329 | if (data) { 330 | if(data->A) c_free(data->A); 331 | if(data->P) c_free(data->P); 332 | c_free(data); 333 | } 334 | 335 | // extract primal results 336 | x_.resize(num_of_knots_); 337 | dx_.resize(num_of_knots_); 338 | ddx_.resize(num_of_knots_); 339 | 340 | 341 | for (size_t i = 0; i < num_of_knots_; ++i) { 342 | x_.at(i) = osqp_work->solution->x[i] / scale_factor_[0]; 343 | dx_.at(i) = osqp_work->solution->x[i + num_of_knots_] / scale_factor_[1]; 344 | ddx_.at(i) = osqp_work->solution->x[i + 2 * num_of_knots_] / scale_factor_[2]; 345 | } 346 | osqp_cleanup(osqp_work); 347 | 348 | return true; 349 | } 350 | 351 | void OSQPProblem::FreeData(OSQPData* data) { 352 | delete[] data->q; 353 | delete[] data->l; 354 | delete[] data->u; 355 | 356 | delete[] data->P->i; 357 | delete[] data->P->p; 358 | delete[] data->P->x; 359 | 360 | delete[] data->A->i; 361 | delete[] data->A->p; 362 | delete[] data->A->x; 363 | } 364 | 365 | void OSQPProblem::set_end_state_ref( 366 | const std::array& weight_end_state, 367 | const std::array& end_state_ref) { 368 | weight_end_state_ = weight_end_state; 369 | end_state_ref_ = end_state_ref; 370 | has_end_state_ref_ = true; 371 | } 372 | 373 | void OSQPProblem::set_x_ref(const double weight_x_ref, 374 | std::vector x_ref) { 375 | if (x_ref.size() != num_of_knots_) { 376 | std::cout << "x_ref size is wrong!" << std::endl; 377 | return; 378 | } 379 | weight_x_ref_ = weight_x_ref; 380 | x_ref_ = x_ref; 381 | has_x_ref_ = true; 382 | } 383 | 384 | void OSQPProblem::set_dx_bounds(double low_bound, double up_bound) { 385 | for (auto& x : dx_bounds_) { 386 | x.first = low_bound; 387 | x.second = up_bound; 388 | } 389 | } 390 | 391 | void OSQPProblem::set_ddx_bounds(double low_bound, double up_bound) { 392 | for (auto& x : ddx_bounds_) { 393 | x.first = low_bound; 394 | x.second = up_bound; 395 | } 396 | } 397 | 398 | void OSQPProblem::set_dddx_bounds(double low, double up) { 399 | dddx_bound_.first = low; 400 | dddx_bound_.second = up; 401 | } 402 | -------------------------------------------------------------------------------- /osqp_problem.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | //#include "osqp.h" 7 | #include 8 | 9 | using namespace std; 10 | 11 | template 12 | T* CopyData(const std::vector& vec) { 13 | T* data = new T[vec.size()]; 14 | memcpy(data, vec.data(), sizeof(T) * vec.size()); 15 | return data; 16 | } 17 | 18 | class OSQPProblem { 19 | public: 20 | OSQPProblem(const size_t num_of_knots, const double delta_s, 21 | const std::array& x_init); 22 | void CalculateKernel(std::vector* P_data, 23 | std::vector* P_indices, 24 | std::vector* P_indptr); 25 | void CalculateAffineConstraint(std::vector* A_data, 26 | std::vector* A_indices, 27 | std::vector* A_indptr, 28 | std::vector* lower_bounds, 29 | std::vector* upper_bounds); 30 | void CalculateOffset(std::vector* q); 31 | OSQPSettings* SolverDefaultSettings(); 32 | OSQPData* FormulateProblem(); 33 | bool Optimize(const int max_iter); 34 | void FreeData(OSQPData* data); 35 | 36 | inline void set_weight_x(double w) { weight_x_ = w; } 37 | inline void set_weight_dx(double w) { weight_dx_ = w; } 38 | inline void set_weight_ddx(double w) { weight_ddx_ = w; } 39 | inline void set_weight_dddx(double w) { weight_dddx_ = w; } 40 | 41 | void set_end_state_ref(const std::array& weight_end_state, 42 | const std::array& end_state_ref); 43 | 44 | void set_x_ref(const double weight_x_ref, std::vector x_ref); 45 | 46 | inline void set_scale_factor(const std::array& factor) { scale_factor_ = factor; } 47 | 48 | inline void set_x_bounds(const std::vector>& x_bounds) { x_bounds_ = x_bounds; } 49 | void set_dx_bounds(double low_bound, double up_bound); 50 | void set_ddx_bounds(double low_bound, double up_bound); 51 | void set_dddx_bounds(double low, double up); 52 | 53 | // output 54 | std::vector x_; 55 | std::vector dx_; 56 | std::vector ddx_; 57 | 58 | private: 59 | size_t num_of_knots_; 60 | 61 | 62 | 63 | std::array x_init_; 64 | std::array scale_factor_ = {{1.0, 1.0, 1.0}}; 65 | 66 | std::vector> x_bounds_; 67 | std::vector> dx_bounds_; 68 | std::vector> ddx_bounds_; 69 | std::pair dddx_bound_; 70 | 71 | double weight_x_ = 1.0; 72 | double weight_dx_ = 1.0; 73 | double weight_ddx_ = 1.0; 74 | double weight_dddx_ = 1.0; 75 | 76 | double delta_s_ = 0.5; 77 | 78 | bool has_x_ref_ = false; 79 | double weight_x_ref_ = 1.0; 80 | std::vector x_ref_; 81 | 82 | bool has_end_state_ref_ = false; 83 | std::array weight_end_state_ = {{0.0, 0.0, 0.0}}; 84 | std::array end_state_ref_; 85 | }; 86 | -------------------------------------------------------------------------------- /polish.h: -------------------------------------------------------------------------------- 1 | /* Solution polish based on assuming the active set */ 2 | #ifndef POLISH_H 3 | # define POLISH_H 4 | 5 | # ifdef __cplusplus 6 | extern "C" { 7 | # endif // ifdef __cplusplus 8 | 9 | 10 | # include "types.h" 11 | 12 | /** 13 | * Solution polish: Solve equality constrained QP with assumed active 14 | *constraints 15 | * @param work Workspace 16 | * @return Exitflag 17 | */ 18 | c_int polish(OSQPWorkspace *work); 19 | 20 | 21 | # ifdef __cplusplus 22 | } 23 | # endif // ifdef __cplusplus 24 | 25 | #endif // ifndef POLISH_H 26 | -------------------------------------------------------------------------------- /proj.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJ_H 2 | # define PROJ_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" 9 | 10 | 11 | /* Define Projections onto set C involved in the ADMM algorithm */ 12 | 13 | /** 14 | * Project z onto \f$C = [l, u]\f$ 15 | * @param z Vector to project 16 | * @param work Workspace 17 | */ 18 | void project(OSQPWorkspace *work, 19 | c_float *z); 20 | 21 | 22 | /** 23 | * Ensure z satisfies box constraints and y is is normal cone of z 24 | * @param work Workspace 25 | * @param z Primal variable z 26 | * @param y Dual variable y 27 | */ 28 | void project_normalcone(OSQPWorkspace *work, 29 | c_float *z, 30 | c_float *y); 31 | 32 | 33 | # ifdef __cplusplus 34 | } 35 | # endif // ifdef __cplusplus 36 | 37 | #endif // ifndef PROJ_H 38 | -------------------------------------------------------------------------------- /referencesmooth.pro: -------------------------------------------------------------------------------- 1 | QT -= gui 2 | 3 | CONFIG += c++11 4 | CONFIG -= app_bundle 5 | QMAKE_CXXFLAGS += -std=c++11 6 | # The following define makes your compiler emit warnings if you use 7 | # any Qt feature that has been marked deprecated (the exact warnings 8 | # depend on your compiler). Please consult the documentation of the 9 | # deprecated API in order to know how to port your code away from it. 10 | DEFINES += QT_DEPRECATED_WARNINGS 11 | 12 | # You can also make your code fail to compile if it uses deprecated APIs. 13 | # In order to do so, uncomment the following line. 14 | # You can also select to disable deprecated APIs only up to a certain version of Qt. 15 | #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0 16 | 17 | 18 | LIBS += /usr/local/lib/libosqp.so \ 19 | 20 | SOURCES += \ 21 | main.cpp \ 22 | smoothosqpproblem.cpp 23 | 24 | # Default rules for deployment. 25 | qnx: target.path = /tmp/$${TARGET}/bin 26 | else: unix:!android: target.path = /opt/$${TARGET}/bin 27 | !isEmpty(target.path): INSTALLS += target 28 | 29 | HEADERS += \ 30 | auxil.h \ 31 | constants.h \ 32 | cs.h \ 33 | ctrlc.h \ 34 | error.h \ 35 | glob_opts.h \ 36 | kkt.h \ 37 | lin_alg.h \ 38 | lin_sys.h \ 39 | osqp_configure.h \ 40 | polish.h \ 41 | proj.h \ 42 | scaling.h \ 43 | smoothosqpproblem.h \ 44 | types.h \ 45 | util.h 46 | -------------------------------------------------------------------------------- /scaling.h: -------------------------------------------------------------------------------- 1 | #ifndef SCALING_H 2 | # define SCALING_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | // Functions to scale problem data 9 | # include "types.h" 10 | # include "lin_alg.h" 11 | # include "constants.h" 12 | 13 | // Enable data scaling if EMBEDDED is disabled or if EMBEDDED == 2 14 | # if EMBEDDED != 1 15 | 16 | /** 17 | * Scale problem matrices 18 | * @param work Workspace 19 | * @return exitflag 20 | */ 21 | c_int scale_data(OSQPWorkspace *work); 22 | # endif // if EMBEDDED != 1 23 | 24 | 25 | /** 26 | * Unscale problem matrices 27 | * @param work Workspace 28 | * @return exitflag 29 | */ 30 | c_int unscale_data(OSQPWorkspace *work); 31 | 32 | 33 | /** 34 | * Unscale solution 35 | * @param work Workspace 36 | * @return exitflag 37 | */ 38 | c_int unscale_solution(OSQPWorkspace *work); 39 | 40 | # ifdef __cplusplus 41 | } 42 | # endif // ifdef __cplusplus 43 | 44 | #endif // ifndef SCALING_H 45 | -------------------------------------------------------------------------------- /smoothosqpproblem.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "smoothosqpproblem.h" 5 | 6 | 7 | bool FemPosDeviationSqpOsqpInterface::Solve() { 8 | // Sanity Check 9 | if (ref_points_.empty()) { 10 | std::cout << "reference points empty, solver early terminates"; 11 | return false; 12 | } 13 | 14 | if (ref_points_.size() != bounds_around_refs_.size()) { 15 | std::cout << "ref_points and bounds size not equal, solver early terminates"; 16 | return false; 17 | } 18 | 19 | if (ref_points_.size() < 3) { 20 | std::cout << "ref_points size smaller than 3, solver early terminates"; 21 | return false; 22 | } 23 | 24 | if (ref_points_.size() > std::numeric_limits::max()) { 25 | std::cout << "ref_points size too large, solver early terminates"; 26 | return false; 27 | } 28 | 29 | // Calculate optimization states definitions 30 | num_of_points_ = static_cast(ref_points_.size()); 31 | num_of_pos_variables_ = num_of_points_ * 2; 32 | num_of_slack_variables_ = num_of_points_ - 2; 33 | num_of_variables_ = num_of_pos_variables_ + num_of_slack_variables_; 34 | 35 | num_of_variable_constraints_ = num_of_variables_; 36 | num_of_curvature_constraints_ = num_of_points_ - 2; 37 | num_of_constraints_ = 38 | num_of_variable_constraints_ + num_of_curvature_constraints_; 39 | 40 | // Set primal warm start 41 | std::vector primal_warm_start; 42 | SetPrimalWarmStart(ref_points_, &primal_warm_start); 43 | 44 | // Calculate kernel 45 | std::vector P_data; 46 | std::vector P_indices; 47 | std::vector P_indptr; 48 | CalculateKernel(&P_data, &P_indices, &P_indptr); 49 | 50 | // Calculate offset 51 | std::vector q; 52 | CalculateOffset(&q); 53 | 54 | // Calculate affine constraints 55 | std::vector A_data; 56 | std::vector A_indices; 57 | std::vector A_indptr; 58 | std::vector lower_bounds; 59 | std::vector upper_bounds; 60 | CalculateAffineConstraint(ref_points_, &A_data, &A_indices, &A_indptr, 61 | &lower_bounds, &upper_bounds); 62 | 63 | // Load matrices and vectors into OSQPData 64 | OSQPData* data = reinterpret_cast(c_malloc(sizeof(OSQPData))); 65 | data->n = num_of_variables_; 66 | data->m = num_of_constraints_; 67 | data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(), 68 | P_indices.data(), P_indptr.data()); 69 | data->q = q.data(); 70 | data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(), 71 | A_indices.data(), A_indptr.data()); 72 | data->l = lower_bounds.data(); 73 | data->u = upper_bounds.data(); 74 | 75 | // Define osqp solver settings 76 | OSQPSettings* settings = 77 | reinterpret_cast(c_malloc(sizeof(OSQPSettings))); 78 | osqp_set_default_settings(settings); 79 | settings->max_iter = max_iter_; 80 | settings->time_limit = time_limit_; 81 | settings->verbose = verbose_; 82 | settings->scaled_termination = scaled_termination_; 83 | settings->warm_start = warm_start_; 84 | settings->polish = true; 85 | settings->eps_abs = 1e-5; 86 | settings->eps_rel = 1e-5; 87 | settings->eps_prim_inf = 1e-5; 88 | settings->eps_dual_inf = 1e-5; 89 | 90 | // Define osqp workspace 91 | OSQPWorkspace* work ; 92 | c_int status = osqp_setup(&work, data, settings); 93 | 94 | // Initial solution 95 | bool initial_solve_res = OptimizeWithOsqp(primal_warm_start, &work); 96 | 97 | if (!initial_solve_res) { 98 | std::cout << "initial iteration solving fails"; 99 | osqp_cleanup(work); 100 | c_free(data->A); 101 | c_free(data->P); 102 | c_free(data); 103 | c_free(settings); 104 | return false; 105 | } 106 | 107 | // Sequential solution 108 | 109 | int pen_itr = 0; 110 | double ctol = 0.0; 111 | double original_slack_penalty = weight_curvature_constraint_slack_var_; 112 | double last_fvalue = work->info->obj_val; 113 | 114 | while (pen_itr < sqp_pen_max_iter_) { 115 | int sub_itr = 1; 116 | bool fconverged = false; 117 | 118 | while (sub_itr < sqp_sub_max_iter_) { 119 | SetPrimalWarmStart(opt_xy_, &primal_warm_start); 120 | CalculateOffset(&q); 121 | CalculateAffineConstraint(opt_xy_, &A_data, &A_indices, &A_indptr, 122 | &lower_bounds, &upper_bounds); 123 | osqp_update_lin_cost(work, q.data()); 124 | osqp_update_A(work, A_data.data(), OSQP_NULL, A_data.size()); 125 | osqp_update_bounds(work, lower_bounds.data(), upper_bounds.data()); 126 | 127 | bool iterative_solve_res = OptimizeWithOsqp(primal_warm_start, &work); 128 | if (!iterative_solve_res) { 129 | std::cout << "iteration at " << sub_itr 130 | << ", solving fails with max sub iter " << sqp_sub_max_iter_; 131 | weight_curvature_constraint_slack_var_ = original_slack_penalty; 132 | osqp_cleanup(work); 133 | c_free(data->A); 134 | c_free(data->P); 135 | c_free(data); 136 | c_free(settings); 137 | return false; 138 | } 139 | 140 | double cur_fvalue = work->info->obj_val; 141 | double ftol = std::abs((last_fvalue - cur_fvalue) / last_fvalue); 142 | 143 | if (ftol < sqp_ftol_) { 144 | // std::cout << "merit function value converges at sub itr num " << sub_itr; 145 | // std::cout << "merit function value converges to " << cur_fvalue 146 | // << ", with ftol " << ftol << ", under max_ftol " << sqp_ftol_; 147 | fconverged = true; 148 | break; 149 | } 150 | 151 | last_fvalue = cur_fvalue; 152 | ++sub_itr; 153 | } 154 | 155 | if (!fconverged) { 156 | std::cout << "Max number of iteration reached"; 157 | weight_curvature_constraint_slack_var_ = original_slack_penalty; 158 | osqp_cleanup(work); 159 | c_free(data->A); 160 | c_free(data->P); 161 | c_free(data); 162 | c_free(settings); 163 | return false; 164 | } 165 | 166 | ctol = CalculateConstraintViolation(opt_xy_); 167 | 168 | // std::cout << "ctol is " << ctol << ", at pen itr " << pen_itr; 169 | 170 | if (ctol < sqp_ctol_) { 171 | // std::cout << "constraint satisfied at pen itr num " << pen_itr; 172 | // std::cout << "constraint voilation value drops to " << ctol 173 | // << ", under max_ctol " << sqp_ctol_; 174 | weight_curvature_constraint_slack_var_ = original_slack_penalty; 175 | osqp_cleanup(work); 176 | c_free(data->A); 177 | c_free(data->P); 178 | c_free(data); 179 | c_free(settings); 180 | return true; 181 | } 182 | 183 | weight_curvature_constraint_slack_var_ *= 10; 184 | ++pen_itr; 185 | } 186 | 187 | // std::cout << "constraint not satisfied with total itr num " << pen_itr; 188 | // std::cout << "constraint voilation value drops to " << ctol 189 | // << ", higher than max_ctol " << sqp_ctol_; 190 | weight_curvature_constraint_slack_var_ = original_slack_penalty; 191 | osqp_cleanup(work); 192 | c_free(data->A); 193 | c_free(data->P); 194 | c_free(data); 195 | c_free(settings); 196 | return true; 197 | } 198 | 199 | void FemPosDeviationSqpOsqpInterface::CalculateKernel( 200 | std::vector* P_data, std::vector* P_indices, 201 | std::vector* P_indptr) { 202 | 203 | 204 | 205 | if(num_of_points_< 2) 206 | { 207 | std::cout<<"num_of_points_< 2"<>> columns; 231 | columns.resize(num_of_variables_); 232 | int col_num = 0; 233 | 234 | for (int col = 0; col < 2; ++col) { 235 | columns[col].emplace_back(col, weight_fem_pos_deviation_ + 236 | weight_path_length_ + 237 | weight_ref_deviation_); 238 | ++col_num; 239 | } 240 | 241 | for (int col = 2; col < 4; ++col) { 242 | columns[col].emplace_back( 243 | col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_); 244 | columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ + 245 | 2.0 * weight_path_length_ + 246 | weight_ref_deviation_); 247 | ++col_num; 248 | } 249 | 250 | int second_point_from_last_index = num_of_points_ - 2; 251 | for (int point_index = 2; point_index < second_point_from_last_index; 252 | ++point_index) { 253 | int col_index = point_index * 2; 254 | for (int col = 0; col < 2; ++col) { 255 | col_index += col; 256 | columns[col_index].emplace_back(col_index - 4, weight_fem_pos_deviation_); 257 | columns[col_index].emplace_back( 258 | col_index - 2, 259 | -4.0 * weight_fem_pos_deviation_ - weight_path_length_); 260 | columns[col_index].emplace_back( 261 | col_index, 6.0 * weight_fem_pos_deviation_ + 262 | 2.0 * weight_path_length_ + weight_ref_deviation_); 263 | ++col_num; 264 | } 265 | } 266 | 267 | int second_point_col_from_last_col = num_of_pos_variables_ - 4; 268 | int last_point_col_from_last_col = num_of_pos_variables_ - 2; 269 | for (int col = second_point_col_from_last_col; 270 | col < last_point_col_from_last_col; ++col) { 271 | columns[col].emplace_back(col - 4, weight_fem_pos_deviation_); 272 | columns[col].emplace_back( 273 | col - 2, -4.0 * weight_fem_pos_deviation_ - weight_path_length_); 274 | columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ + 275 | 2.0 * weight_path_length_ + 276 | weight_ref_deviation_); 277 | ++col_num; 278 | } 279 | 280 | for (int col = last_point_col_from_last_col; col < num_of_pos_variables_; 281 | ++col) { 282 | columns[col].emplace_back(col - 4, weight_fem_pos_deviation_); 283 | columns[col].emplace_back( 284 | col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_); 285 | columns[col].emplace_back(col, weight_fem_pos_deviation_ + 286 | weight_path_length_ + 287 | weight_ref_deviation_); 288 | ++col_num; 289 | } 290 | 291 | if(col_num!= num_of_pos_variables_) 292 | { 293 | std::cout<<"col_num!= num_of_pos_variables_"<push_back(ind_p); 299 | for (const auto& row_data_pair : columns[i]) { 300 | // Rescale by 2.0 as the quadratic term in osqp default qp problem setup 301 | // is set as (1/2) * x' * P * x 302 | P_data->push_back(row_data_pair.second * 2.0); 303 | P_indices->push_back(row_data_pair.first); 304 | ++ind_p; 305 | } 306 | } 307 | P_indptr->push_back(ind_p); 308 | } 309 | 310 | void FemPosDeviationSqpOsqpInterface::CalculateOffset(std::vector* q) { 311 | q->resize(num_of_variables_); 312 | for (int i = 0; i < num_of_points_; ++i) { 313 | const auto& ref_point_xy = ref_points_[i]; 314 | (*q)[2 * i] = -2.0 * weight_ref_deviation_ * ref_point_xy.first; 315 | (*q)[2 * i + 1] = -2.0 * weight_ref_deviation_ * ref_point_xy.second; 316 | } 317 | for (int i = 0; i < num_of_slack_variables_; ++i) { 318 | (*q)[num_of_pos_variables_ + i] = weight_curvature_constraint_slack_var_; 319 | } 320 | } 321 | 322 | std::vector 323 | FemPosDeviationSqpOsqpInterface::CalculateLinearizedFemPosParams( 324 | const std::vector>& points, const size_t index) { 325 | 326 | if(index < 0) 327 | { 328 | std::cout<<"index < 0"<= points.size() - 1) 332 | { 333 | std::cout<<"index >= points.size() - 1"<>& points, 362 | std::vector* A_data, std::vector* A_indices, 363 | std::vector* A_indptr, std::vector* lower_bounds, 364 | std::vector* upper_bounds) { 365 | const double scale_factor = 1; 366 | 367 | std::vector> lin_cache; 368 | for (int i = 1; i < num_of_points_ - 1; ++i) { 369 | lin_cache.push_back(CalculateLinearizedFemPosParams(points, i)); 370 | } 371 | 372 | std::vector>> columns; 373 | columns.resize(num_of_variables_); 374 | 375 | for (int i = 0; i < num_of_variables_; ++i) { 376 | columns[i].emplace_back(i, 1.0); 377 | } 378 | 379 | for (int i = num_of_pos_variables_; i < num_of_variables_; ++i) { 380 | columns[i].emplace_back(i + num_of_slack_variables_, -1.0 * scale_factor); 381 | } 382 | 383 | for (int i = 2; i < num_of_points_; ++i) { 384 | int index = 2 * i; 385 | columns[index].emplace_back(i - 2 + num_of_variables_, 386 | lin_cache[i - 2][4] * scale_factor); 387 | columns[index + 1].emplace_back(i - 2 + num_of_variables_, 388 | lin_cache[i - 2][5] * scale_factor); 389 | } 390 | 391 | for (int i = 1; i < num_of_points_ - 1; ++i) { 392 | int index = 2 * i; 393 | columns[index].emplace_back(i - 1 + num_of_variables_, 394 | lin_cache[i - 1][2] * scale_factor); 395 | columns[index + 1].emplace_back(i - 1 + num_of_variables_, 396 | lin_cache[i - 1][3] * scale_factor); 397 | } 398 | 399 | for (int i = 0; i < num_of_points_ - 2; ++i) { 400 | int index = 2 * i; 401 | columns[index].emplace_back(i + num_of_variables_, 402 | lin_cache[i][0] * scale_factor); 403 | columns[index + 1].emplace_back(i + num_of_variables_, 404 | lin_cache[i][1] * scale_factor); 405 | } 406 | 407 | int ind_a = 0; 408 | for (int i = 0; i < num_of_variables_; ++i) { 409 | A_indptr->push_back(ind_a); 410 | for (const auto& row_data_pair : columns[i]) { 411 | A_data->push_back(row_data_pair.second); 412 | A_indices->push_back(row_data_pair.first); 413 | ++ind_a; 414 | } 415 | } 416 | A_indptr->push_back(ind_a); 417 | 418 | lower_bounds->resize(num_of_constraints_); 419 | upper_bounds->resize(num_of_constraints_); 420 | 421 | const auto& ref_point_xy = ref_points_.front(); 422 | (*upper_bounds)[0 * 2] = ref_point_xy.first + bounds_around_refs_[0]; 423 | (*upper_bounds)[0 * 2 + 1] = ref_point_xy.second - 25.0; 424 | (*lower_bounds)[0 * 2] = ref_point_xy.first - bounds_around_refs_[0]; 425 | (*lower_bounds)[0 * 2 + 1] = ref_point_xy.second - 26.0; 426 | 427 | for (int i = 1; i < num_of_points_; ++i) { 428 | const auto& ref_point_xy = ref_points_[i]; 429 | (*upper_bounds)[i * 2] = ref_point_xy.first + bounds_around_refs_[i]; 430 | (*upper_bounds)[i * 2 + 1] = ref_point_xy.second + bounds_around_refs_[i]; 431 | (*lower_bounds)[i * 2] = ref_point_xy.first - bounds_around_refs_[i]; 432 | (*lower_bounds)[i * 2 + 1] = ref_point_xy.second - bounds_around_refs_[i]; 433 | } 434 | 435 | for (int i = 0; i < num_of_slack_variables_; ++i) { 436 | (*upper_bounds)[num_of_pos_variables_ + i] = 1e20; 437 | (*lower_bounds)[num_of_pos_variables_ + i] = 0.0; 438 | } 439 | 440 | double interval_sqr = average_interval_length_ * average_interval_length_; 441 | double curvature_constraint_sqr = (interval_sqr * curvature_constraint_) * 442 | (interval_sqr * curvature_constraint_); 443 | for (int i = 0; i < num_of_curvature_constraints_; ++i) { 444 | (*upper_bounds)[num_of_variable_constraints_ + i] = 445 | (curvature_constraint_sqr - lin_cache[i][6]) * scale_factor; 446 | (*lower_bounds)[num_of_variable_constraints_ + i] = -1e20; 447 | } 448 | } 449 | 450 | void FemPosDeviationSqpOsqpInterface::SetPrimalWarmStart( 451 | const std::vector>& points, 452 | std::vector* primal_warm_start) { 453 | 454 | if(points.size()!= num_of_points_) 455 | { 456 | std::cout<<"points.size()!= num_of_points_"<resize(num_of_variables_); 466 | for (int i = 0; i < num_of_points_; ++i) { 467 | (*primal_warm_start)[2 * i] = points[i].first; 468 | (*primal_warm_start)[2 * i + 1] = points[i].second; 469 | } 470 | 471 | slack_.resize(num_of_slack_variables_); 472 | for (int i = 0; i < num_of_slack_variables_; ++i) { 473 | (*primal_warm_start)[num_of_pos_variables_ + i] = slack_[i]; 474 | } 475 | 476 | // Calculate average interval length for curvature constraints 477 | double total_length = 0.0; 478 | auto pre_point = points.front(); 479 | for (int i = 1; i < num_of_points_; ++i) { 480 | const auto& cur_point = points[i]; 481 | total_length += std::sqrt((pre_point.first - cur_point.first) * 482 | (pre_point.first - cur_point.first) + 483 | (pre_point.second - cur_point.second) * 484 | (pre_point.second - cur_point.second)); 485 | pre_point = cur_point; 486 | } 487 | average_interval_length_ = total_length / (num_of_points_ - 1); 488 | } 489 | 490 | bool FemPosDeviationSqpOsqpInterface::OptimizeWithOsqp( 491 | const std::vector& primal_warm_start, OSQPWorkspace** work) { 492 | osqp_warm_start_x(*work, primal_warm_start.data()); 493 | 494 | // Solve Problem 495 | osqp_solve(*work); 496 | 497 | auto status = (*work)->info->status_val; 498 | 499 | if (status < 0) { 500 | std::cout << "failed optimization status:\t" << (*work)->info->status; 501 | return false; 502 | } 503 | 504 | if (status != 1 && status != 2) { 505 | std::cout << "failed optimization status:\t" << (*work)->info->status; 506 | return false; 507 | } 508 | 509 | // Extract primal results 510 | opt_xy_.resize(num_of_points_); 511 | slack_.resize(num_of_slack_variables_); 512 | for (int i = 0; i < num_of_points_; ++i) { 513 | int index = i * 2; 514 | opt_xy_.at(i) = std::make_pair((*work)->solution->x[index], 515 | (*work)->solution->x[index + 1]); 516 | } 517 | 518 | for (int i = 0; i < num_of_slack_variables_; ++i) { 519 | slack_.at(i) = (*work)->solution->x[num_of_pos_variables_ + i]; 520 | } 521 | 522 | return true; 523 | } 524 | 525 | double FemPosDeviationSqpOsqpInterface::CalculateConstraintViolation( 526 | const std::vector>& points) { 527 | 528 | if(points.size() < 2) 529 | { 530 | std::cout<<"points.size() < 2"<::min(); 549 | for (size_t i = 1; i < points.size() - 1; ++i) { 550 | double x_f = points[i - 1].first; 551 | double x_m = points[i].first; 552 | double x_l = points[i + 1].first; 553 | double y_f = points[i - 1].second; 554 | double y_m = points[i].second; 555 | double y_l = points[i + 1].second; 556 | double cviolation = curvature_constraint_sqr - 557 | (-2.0 * x_m + x_f + x_l) * (-2.0 * x_m + x_f + x_l) + 558 | (-2.0 * y_m + y_f + y_l) * (-2.0 * y_m + y_f + y_l); 559 | max_cviolation = max_cviolation < cviolation ? cviolation : max_cviolation; 560 | } 561 | return max_cviolation; 562 | } 563 | -------------------------------------------------------------------------------- /smoothosqpproblem.h: -------------------------------------------------------------------------------- 1 | #ifndef SMOOTHOSQPPROBLEM_H 2 | #define SMOOTHOSQPPROBLEM_H 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | class FemPosDeviationSqpOsqpInterface { 16 | public: 17 | FemPosDeviationSqpOsqpInterface() = default; 18 | 19 | virtual ~FemPosDeviationSqpOsqpInterface() = default; 20 | 21 | void set_ref_points( 22 | const std::vector>& ref_points) { 23 | ref_points_ = ref_points; 24 | } 25 | 26 | void set_bounds_around_refs(const std::vector& bounds_around_refs) { 27 | bounds_around_refs_ = bounds_around_refs; 28 | } 29 | 30 | void set_weight_fem_pos_deviation(const double weight_fem_pos_deviation) { 31 | weight_fem_pos_deviation_ = weight_fem_pos_deviation; 32 | } 33 | 34 | void set_weight_path_length(const double weight_path_length) { 35 | weight_path_length_ = weight_path_length; 36 | } 37 | 38 | void set_weight_ref_deviation(const double weight_ref_deviation) { 39 | weight_ref_deviation_ = weight_ref_deviation; 40 | } 41 | 42 | void set_weight_curvature_constraint_slack_var( 43 | const double weight_curvature_constraint_slack_var) { 44 | weight_curvature_constraint_slack_var_ = 45 | weight_curvature_constraint_slack_var; 46 | } 47 | 48 | void set_curvature_constraint(const double curvature_constraint) { 49 | curvature_constraint_ = curvature_constraint; 50 | } 51 | 52 | void set_max_iter(const int max_iter) { max_iter_ = max_iter; } 53 | 54 | void set_time_limit(const double time_limit) { time_limit_ = time_limit; } 55 | 56 | void set_verbose(const bool verbose) { verbose_ = verbose; } 57 | 58 | void set_scaled_termination(const bool scaled_termination) { 59 | scaled_termination_ = scaled_termination; 60 | } 61 | 62 | void set_warm_start(const bool warm_start) { warm_start_ = warm_start; } 63 | 64 | void set_sqp_pen_max_iter(const int sqp_pen_max_iter) { 65 | sqp_pen_max_iter_ = sqp_pen_max_iter; 66 | } 67 | 68 | void set_sqp_ftol(const double sqp_ftol) { sqp_ftol_ = sqp_ftol; } 69 | 70 | void set_sqp_sub_max_iter(const int sqp_sub_max_iter) { 71 | sqp_sub_max_iter_ = sqp_sub_max_iter; 72 | } 73 | 74 | void set_sqp_ctol(const double sqp_ctol) { sqp_ctol_ = sqp_ctol; } 75 | 76 | bool Solve(); 77 | 78 | const std::vector>& opt_xy() const { 79 | return opt_xy_; 80 | } 81 | 82 | private: 83 | void CalculateKernel(std::vector* P_data, 84 | std::vector* P_indices, 85 | std::vector* P_indptr); 86 | 87 | void CalculateOffset(std::vector* q); 88 | 89 | std::vector CalculateLinearizedFemPosParams( 90 | const std::vector>& points, const size_t index); 91 | 92 | void CalculateAffineConstraint( 93 | const std::vector>& points, 94 | std::vector* A_data, std::vector* A_indices, 95 | std::vector* A_indptr, std::vector* lower_bounds, 96 | std::vector* upper_bounds); 97 | 98 | void SetPrimalWarmStart(const std::vector>& points, 99 | std::vector* primal_warm_start); 100 | 101 | bool OptimizeWithOsqp(const std::vector& primal_warm_start, 102 | OSQPWorkspace** work); 103 | 104 | double CalculateConstraintViolation( 105 | const std::vector>& points); 106 | 107 | private: 108 | // Init states and constraints 109 | std::vector> ref_points_; 110 | std::vector bounds_around_refs_; 111 | double curvature_constraint_ = 0.15; 112 | 113 | // Weights in optimization cost function 114 | double weight_fem_pos_deviation_ = 1.0e5; 115 | double weight_path_length_ = 1.0; 116 | double weight_ref_deviation_ = 1.0; 117 | double weight_curvature_constraint_slack_var_ = 1.0e5; 118 | 119 | // Settings of osqp 120 | int max_iter_ = 4000; 121 | double time_limit_ = 0.0; 122 | bool verbose_ = false; 123 | bool scaled_termination_ = true; 124 | bool warm_start_ = true; 125 | 126 | // Settings of sqp 127 | int sqp_pen_max_iter_ = 100; 128 | double sqp_ftol_ = 1e-2; 129 | int sqp_sub_max_iter_ = 100; 130 | double sqp_ctol_ = 0.1; 131 | 132 | // Optimization problem definitions 133 | int num_of_points_ = 0; 134 | int num_of_pos_variables_ = 0; 135 | int num_of_slack_variables_ = 0; 136 | int num_of_variables_ = 0; 137 | int num_of_variable_constraints_ = 0; 138 | int num_of_curvature_constraints_ = 0; 139 | int num_of_constraints_ = 0; 140 | 141 | // Optimized_result 142 | std::vector> opt_xy_; 143 | std::vector slack_; 144 | double average_interval_length_ = 0.0; 145 | }; 146 | 147 | 148 | 149 | 150 | 151 | #endif // SMOOTHOSQPPROBLEM_H 152 | -------------------------------------------------------------------------------- /types.h: -------------------------------------------------------------------------------- 1 | #ifndef OSQP_TYPES_H 2 | # define OSQP_TYPES_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "glob_opts.h" 9 | # include "constants.h" 10 | 11 | 12 | /****************** 13 | * Internal types * 14 | ******************/ 15 | enum linsys_solver_type { QDLDL_SOLVER, MKL_PARDISO_SOLVER }; 16 | /** 17 | * Matrix in compressed-column form. 18 | * The structure is used internally to store matrices in the triplet form as well, 19 | * but the API requires that the matrices are in the CSC format. 20 | */ 21 | typedef struct { 22 | c_int nzmax; ///< maximum number of entries 23 | c_int m; ///< number of rows 24 | c_int n; ///< number of columns 25 | c_int *p; ///< column pointers (size n+1); col indices (size nzmax) start from 0 when using triplet format (direct KKT matrix formation) 26 | c_int *i; ///< row indices, size nzmax starting from 0 27 | c_float *x; ///< numerical values, size nzmax 28 | c_int nz; ///< number of entries in triplet matrix, -1 for csc 29 | } csc; 30 | 31 | /** 32 | * Linear system solver structure (sublevel objects initialize it differently) 33 | */ 34 | 35 | typedef struct linsys_solver LinSysSolver; 36 | 37 | /** 38 | * OSQP Timer for statistics 39 | */ 40 | typedef struct OSQP_TIMER OSQPTimer; 41 | 42 | /** 43 | * Problem scaling matrices stored as vectors 44 | */ 45 | typedef struct { 46 | c_float c; ///< cost function scaling 47 | c_float *D; ///< primal variable scaling 48 | c_float *E; ///< dual variable scaling 49 | c_float cinv; ///< cost function rescaling 50 | c_float *Dinv; ///< primal variable rescaling 51 | c_float *Einv; ///< dual variable rescaling 52 | } OSQPScaling; 53 | 54 | /** 55 | * Solution structure 56 | */ 57 | typedef struct { 58 | c_float *x; ///< primal solution 59 | c_float *y; ///< Lagrange multiplier associated to \f$l <= Ax <= u\f$ 60 | } OSQPSolution; 61 | 62 | 63 | /** 64 | * Solver return information 65 | */ 66 | typedef struct { 67 | c_int iter; ///< number of iterations taken 68 | char status[32]; ///< status string, e.g. 'solved' 69 | c_int status_val; ///< status as c_int, defined in constants.h 70 | 71 | # ifndef EMBEDDED 72 | c_int status_polish; ///< polish status: successful (1), unperformed (0), (-1) unsuccessful 73 | # endif // ifndef EMBEDDED 74 | 75 | c_float obj_val; ///< primal objective 76 | c_float pri_res; ///< norm of primal residual 77 | c_float dua_res; ///< norm of dual residual 78 | 79 | # ifdef PROFILING 80 | c_float setup_time; ///< time taken for setup phase (seconds) 81 | c_float solve_time; ///< time taken for solve phase (seconds) 82 | c_float update_time; ///< time taken for update phase (seconds) 83 | c_float polish_time; ///< time taken for polish phase (seconds) 84 | c_float run_time; ///< total time (seconds) 85 | # endif // ifdef PROFILING 86 | 87 | # if EMBEDDED != 1 88 | c_int rho_updates; ///< number of rho updates 89 | c_float rho_estimate; ///< best rho estimate so far from residuals 90 | # endif // if EMBEDDED != 1 91 | } OSQPInfo; 92 | 93 | 94 | # ifndef EMBEDDED 95 | 96 | /** 97 | * Polish structure 98 | */ 99 | typedef struct { 100 | csc *Ared; ///< active rows of A 101 | ///< Ared = vstack[Alow, Aupp] 102 | c_int n_low; ///< number of lower-active rows 103 | c_int n_upp; ///< number of upper-active rows 104 | c_int *A_to_Alow; ///< Maps indices in A to indices in Alow 105 | c_int *A_to_Aupp; ///< Maps indices in A to indices in Aupp 106 | c_int *Alow_to_A; ///< Maps indices in Alow to indices in A 107 | c_int *Aupp_to_A; ///< Maps indices in Aupp to indices in A 108 | c_float *x; ///< optimal x-solution obtained by polish 109 | c_float *z; ///< optimal z-solution obtained by polish 110 | c_float *y; ///< optimal y-solution obtained by polish 111 | c_float obj_val; ///< objective value at polished solution 112 | c_float pri_res; ///< primal residual at polished solution 113 | c_float dua_res; ///< dual residual at polished solution 114 | } OSQPPolish; 115 | # endif // ifndef EMBEDDED 116 | 117 | 118 | /********************************** 119 | * Main structures and Data Types * 120 | **********************************/ 121 | 122 | /** 123 | * Data structure 124 | */ 125 | typedef struct { 126 | c_int n; ///< number of variables n 127 | c_int m; ///< number of constraints m 128 | csc *P; ///< the upper triangular part of the quadratic cost matrix P in csc format (size n x n). 129 | csc *A; ///< linear constraints matrix A in csc format (size m x n) 130 | c_float *q; ///< dense array for linear part of cost function (size n) 131 | c_float *l; ///< dense array for lower bound (size m) 132 | c_float *u; ///< dense array for upper bound (size m) 133 | } OSQPData; 134 | 135 | 136 | /** 137 | * Settings struct 138 | */ 139 | typedef struct { 140 | c_float rho; ///< ADMM step rho 141 | c_float sigma; ///< ADMM step sigma 142 | c_int scaling; ///< heuristic data scaling iterations; if 0, then disabled. 143 | 144 | # if EMBEDDED != 1 145 | c_int adaptive_rho; ///< boolean, is rho step size adaptive? 146 | c_int adaptive_rho_interval; ///< number of iterations between rho adaptations; if 0, then it is automatic 147 | c_float adaptive_rho_tolerance; ///< tolerance X for adapting rho. The new rho has to be X times larger or 1/X times smaller than the current one to trigger a new factorization. 148 | # ifdef PROFILING 149 | c_float adaptive_rho_fraction; ///< interval for adapting rho (fraction of the setup time) 150 | # endif // Profiling 151 | # endif // EMBEDDED != 1 152 | 153 | c_int max_iter; ///< maximum number of iterations 154 | c_float eps_abs; ///< absolute convergence tolerance 155 | c_float eps_rel; ///< relative convergence tolerance 156 | c_float eps_prim_inf; ///< primal infeasibility tolerance 157 | c_float eps_dual_inf; ///< dual infeasibility tolerance 158 | c_float alpha; ///< relaxation parameter 159 | enum linsys_solver_type linsys_solver; ///< linear system solver to use 160 | 161 | # ifndef EMBEDDED 162 | c_float delta; ///< regularization parameter for polishing 163 | c_int polish; ///< boolean, polish ADMM solution 164 | c_int polish_refine_iter; ///< number of iterative refinement steps in polishing 165 | 166 | c_int verbose; ///< boolean, write out progress 167 | # endif // ifndef EMBEDDED 168 | 169 | c_int scaled_termination; ///< boolean, use scaled termination criteria 170 | c_int check_termination; ///< integer, check termination interval; if 0, then termination checking is disabled 171 | c_int warm_start; ///< boolean, warm start 172 | 173 | # ifdef PROFILING 174 | c_float time_limit; ///< maximum number of seconds allowed to solve the problem; if 0, then disabled 175 | # endif // ifdef PROFILING 176 | } OSQPSettings; 177 | 178 | 179 | /** 180 | * OSQP Workspace 181 | */ 182 | typedef struct { 183 | /// Problem data to work on (possibly scaled) 184 | OSQPData *data; 185 | 186 | /// Linear System solver structure 187 | LinSysSolver *linsys_solver; 188 | 189 | # ifndef EMBEDDED 190 | /// Polish structure 191 | OSQPPolish *pol; 192 | # endif // ifndef EMBEDDED 193 | 194 | /** 195 | * @name Vector used to store a vectorized rho parameter 196 | * @{ 197 | */ 198 | c_float *rho_vec; ///< vector of rho values 199 | c_float *rho_inv_vec; ///< vector of inv rho values 200 | 201 | /** @} */ 202 | 203 | # if EMBEDDED != 1 204 | c_int *constr_type; ///< Type of constraints: loose (-1), equality (1), inequality (0) 205 | # endif // if EMBEDDED != 1 206 | 207 | /** 208 | * @name Iterates 209 | * @{ 210 | */ 211 | c_float *x; ///< Iterate x 212 | c_float *y; ///< Iterate y 213 | c_float *z; ///< Iterate z 214 | c_float *xz_tilde; ///< Iterate xz_tilde 215 | 216 | c_float *x_prev; ///< Previous x 217 | 218 | /**< NB: Used also as workspace vector for dual residual */ 219 | c_float *z_prev; ///< Previous z 220 | 221 | /**< NB: Used also as workspace vector for primal residual */ 222 | 223 | /** 224 | * @name Primal and dual residuals workspace variables 225 | * 226 | * Needed for residuals computation, tolerances computation, 227 | * approximate tolerances computation and adapting rho 228 | * @{ 229 | */ 230 | c_float *Ax; ///< scaled A * x 231 | c_float *Px; ///< scaled P * x 232 | c_float *Aty; ///< scaled A' * y 233 | 234 | /** @} */ 235 | 236 | /** 237 | * @name Primal infeasibility variables 238 | * @{ 239 | */ 240 | c_float *delta_y; ///< difference between consecutive dual iterates 241 | c_float *Atdelta_y; ///< A' * delta_y 242 | 243 | /** @} */ 244 | 245 | /** 246 | * @name Dual infeasibility variables 247 | * @{ 248 | */ 249 | c_float *delta_x; ///< difference between consecutive primal iterates 250 | c_float *Pdelta_x; ///< P * delta_x 251 | c_float *Adelta_x; ///< A * delta_x 252 | 253 | /** @} */ 254 | 255 | /** 256 | * @name Temporary vectors used in scaling 257 | * @{ 258 | */ 259 | 260 | c_float *D_temp; ///< temporary primal variable scaling vectors 261 | c_float *D_temp_A; ///< temporary primal variable scaling vectors storing norms of A columns 262 | c_float *E_temp; ///< temporary constraints scaling vectors storing norms of A' columns 263 | 264 | 265 | /** @} */ 266 | 267 | OSQPSettings *settings; ///< problem settings 268 | OSQPScaling *scaling; ///< scaling vectors 269 | OSQPSolution *solution; ///< problem solution 270 | OSQPInfo *info; ///< solver information 271 | 272 | # ifdef PROFILING 273 | OSQPTimer *timer; ///< timer object 274 | 275 | /// flag indicating whether the solve function has been run before 276 | c_int first_run; 277 | 278 | /// flag indicating whether the update_time should be cleared 279 | c_int clear_update_time; 280 | 281 | /// flag indicating that osqp_update_rho is called from osqp_solve function 282 | c_int rho_update_from_solve; 283 | # endif // ifdef PROFILING 284 | 285 | # ifdef PRINTING 286 | c_int summary_printed; ///< Has last summary been printed? (true/false) 287 | # endif // ifdef PRINTING 288 | 289 | } OSQPWorkspace; 290 | 291 | 292 | /** 293 | * Define linsys_solver prototype structure 294 | * 295 | * NB: The details are defined when the linear solver is initialized depending 296 | * on the choice 297 | */ 298 | struct linsys_solver { 299 | enum linsys_solver_type type; ///< linear system solver type functions 300 | c_int (*solve)(LinSysSolver *self, 301 | c_float *b); ///< solve linear system 302 | 303 | # ifndef EMBEDDED 304 | void (*free)(LinSysSolver *self); ///< free linear system solver (only in desktop version) 305 | # endif // ifndef EMBEDDED 306 | 307 | # if EMBEDDED != 1 308 | c_int (*update_matrices)(LinSysSolver *s, 309 | const csc *P, ///< update matrices P 310 | const csc *A); // and A in the solver 311 | 312 | c_int (*update_rho_vec)(LinSysSolver *s, 313 | const c_float *rho_vec); ///< Update rho_vec 314 | # endif // if EMBEDDED != 1 315 | 316 | # ifndef EMBEDDED 317 | c_int nthreads; ///< number of threads active 318 | # endif // ifndef EMBEDDED 319 | }; 320 | 321 | 322 | # ifdef __cplusplus 323 | } 324 | # endif // ifdef __cplusplus 325 | 326 | #endif // ifndef OSQP_TYPES_H 327 | -------------------------------------------------------------------------------- /util.h: -------------------------------------------------------------------------------- 1 | #ifndef UTIL_H 2 | # define UTIL_H 3 | 4 | # ifdef __cplusplus 5 | extern "C" { 6 | # endif // ifdef __cplusplus 7 | 8 | # include "types.h" 9 | # include "constants.h" 10 | 11 | /****************** 12 | * Versioning * 13 | ******************/ 14 | 15 | /** 16 | * Return OSQP version 17 | * @return OSQP version 18 | */ 19 | const char* osqp_version(void); 20 | 21 | 22 | /********************** 23 | * Utility Functions * 24 | **********************/ 25 | 26 | # ifndef EMBEDDED 27 | 28 | /** 29 | * Copy settings creating a new settings structure (uses MALLOC) 30 | * @param settings Settings to be copied 31 | * @return New settings structure 32 | */ 33 | OSQPSettings* copy_settings(const OSQPSettings *settings); 34 | 35 | # endif // #ifndef EMBEDDED 36 | 37 | /** 38 | * Custom string copy to avoid string.h library 39 | * @param dest destination string 40 | * @param source source string 41 | */ 42 | void c_strcpy(char dest[], 43 | const char source[]); 44 | 45 | 46 | # ifdef PRINTING 47 | 48 | /** 49 | * Print Header before running the algorithm 50 | * @param work osqp workspace 51 | */ 52 | void print_setup_header(const OSQPWorkspace *work); 53 | 54 | /** 55 | * Print header with data to be displayed per iteration 56 | */ 57 | void print_header(void); 58 | 59 | /** 60 | * Print iteration summary 61 | * @param work current workspace 62 | */ 63 | void print_summary(OSQPWorkspace *work); 64 | 65 | /** 66 | * Print information after polish 67 | * @param work current workspace 68 | */ 69 | void print_polish(OSQPWorkspace *work); 70 | 71 | /** 72 | * Print footer when algorithm terminates 73 | * @param info info structure 74 | * @param polish is polish enabled? 75 | */ 76 | void print_footer(OSQPInfo *info, 77 | c_int polish); 78 | 79 | 80 | # endif // ifdef PRINTING 81 | 82 | 83 | /********************************* 84 | * Timer Structs and Functions * * 85 | *********************************/ 86 | 87 | /*! \cond PRIVATE */ 88 | 89 | # ifdef PROFILING 90 | 91 | // Windows 92 | # ifdef IS_WINDOWS 93 | 94 | // Some R packages clash with elements 95 | // of the windows.h header, so use a 96 | // slimmer version for conflict avoidance 97 | # ifdef R_LANG 98 | #define NOGDI 99 | # endif 100 | 101 | # include 102 | 103 | struct OSQP_TIMER { 104 | LARGE_INTEGER tic; 105 | LARGE_INTEGER toc; 106 | LARGE_INTEGER freq; 107 | }; 108 | 109 | // Mac 110 | # elif defined IS_MAC 111 | 112 | # include 113 | 114 | /* Use MAC OSX mach_time for timing */ 115 | struct OSQP_TIMER { 116 | uint64_t tic; 117 | uint64_t toc; 118 | mach_timebase_info_data_t tinfo; 119 | }; 120 | 121 | // Linux 122 | # else // ifdef IS_WINDOWS 123 | 124 | /* Use POSIX clock_gettime() for timing on non-Windows machines */ 125 | # include 126 | # include 127 | 128 | 129 | struct OSQP_TIMER { 130 | struct timespec tic; 131 | struct timespec toc; 132 | }; 133 | 134 | # endif // ifdef IS_WINDOWS 135 | 136 | /*! \endcond */ 137 | 138 | /** 139 | * Timer Methods 140 | */ 141 | 142 | /** 143 | * Start timer 144 | * @param t Timer object 145 | */ 146 | void osqp_tic(OSQPTimer *t); 147 | 148 | /** 149 | * Report time 150 | * @param t Timer object 151 | * @return Reported time 152 | */ 153 | c_float osqp_toc(OSQPTimer *t); 154 | 155 | # endif /* END #ifdef PROFILING */ 156 | 157 | 158 | /* ================================= DEBUG FUNCTIONS ======================= */ 159 | 160 | /*! \cond PRIVATE */ 161 | 162 | 163 | # ifndef EMBEDDED 164 | 165 | /* Compare CSC matrices */ 166 | c_int is_eq_csc(csc *A, 167 | csc *B, 168 | c_float tol); 169 | 170 | /* Convert sparse CSC to dense */ 171 | c_float* csc_to_dns(csc *M); 172 | 173 | # endif // #ifndef EMBEDDED 174 | 175 | 176 | # ifdef PRINTING 177 | # include 178 | 179 | 180 | /* Print a csc sparse matrix */ 181 | void print_csc_matrix(csc *M, 182 | const char *name); 183 | 184 | /* Dump csc sparse matrix to file */ 185 | void dump_csc_matrix(csc *M, 186 | const char *file_name); 187 | 188 | /* Print a triplet format sparse matrix */ 189 | void print_trip_matrix(csc *M, 190 | const char *name); 191 | 192 | /* Print a dense matrix */ 193 | void print_dns_matrix(c_float *M, 194 | c_int m, 195 | c_int n, 196 | const char *name); 197 | 198 | /* Print vector */ 199 | void print_vec(c_float *v, 200 | c_int n, 201 | const char *name); 202 | 203 | /* Dump vector to file */ 204 | void dump_vec(c_float *v, 205 | c_int len, 206 | const char *file_name); 207 | 208 | // Print int array 209 | void print_vec_int(c_int *x, 210 | c_int n, 211 | const char *name); 212 | 213 | # endif // ifdef PRINTING 214 | 215 | /*! \endcond */ 216 | 217 | 218 | # ifdef __cplusplus 219 | } 220 | # endif // ifdef __cplusplus 221 | 222 | #endif // ifndef UTIL_H 223 | --------------------------------------------------------------------------------