From 4cad2430f1f72e3c0757c019ccb7afabe62ceb49 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Tue, 1 Aug 2023 13:26:22 -0500 Subject: [PATCH] Remove osqp headers from osqp_eigen_solver (#344) --- .../trajopt_sqp/src/osqp_eigen_solver.cpp | 34 +++++++++++++------ 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp index afc8bc56..001f2f92 100644 --- a/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp @@ -26,8 +26,6 @@ #include #include -#include -#include #include const bool OSQP_COMPARE_DEBUG_MODE = true; @@ -190,12 +188,26 @@ static void* csc_calloc(c_int n, c_int size) return c_calloc(n, size); // NOLINT } +void csc_spfree_fix(csc* A) +{ + if (A) // NOLINT + { + if (A->p) // NOLINT + c_free(A->p); // NOLINT + if (A->i) // NOLINT + c_free(A->i); // NOLINT + if (A->x) // NOLINT + c_free(A->x); // NOLINT + c_free(A); // NOLINT + } +} + csc* csc_spalloc_fix(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet) { csc* A = (csc*)(csc_calloc(1, sizeof(csc))); /* allocate the csc struct */ // NOLINT - if (!A) // NOLINT - return OSQP_NULL; /* out of memory */ + if (!A) // NOLINT + return nullptr; /* out of memory */ A->m = m; /* define dimensions and nzmax */ A->n = n; @@ -204,12 +216,12 @@ csc* csc_spalloc_fix(c_int m, c_int n, c_int nzmax, c_int values, c_int triplet) A->p = (c_int*)(csc_malloc(triplet ? nzmax : n + 1, sizeof(c_int))); // NOLINT for (int i = 0; i < n + 1; ++i) A->p[i] = 0; - A->i = values ? (c_int*)(csc_malloc(nzmax, sizeof(c_int))) : OSQP_NULL; // NOLINT - A->x = values ? (c_float*)(csc_malloc(nzmax, sizeof(c_float))) : OSQP_NULL; // NOLINT - if (!A->p || (values && !A->i) || (values && !A->x)) // NOLINT + A->i = values ? (c_int*)(csc_malloc(nzmax, sizeof(c_int))) : nullptr; // NOLINT + A->x = values ? (c_float*)(csc_malloc(nzmax, sizeof(c_float))) : nullptr; // NOLINT + if (!A->p || (values && !A->i) || (values && !A->x)) // NOLINT { - csc_spfree(A); - return OSQP_NULL; + csc_spfree_fix(A); + return nullptr; } return A; // NOLINT @@ -227,7 +239,7 @@ bool OSQPEigenSolver::updateHessianMatrix(const SparseMatrix& hessian) bool success = solver_.updateHessianMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { - csc_spfree(solver_.data()->getData()->P); + csc_spfree_fix(solver_.data()->getData()->P); solver_.data()->getData()->P = nullptr; solver_.data()->getData()->P = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); } @@ -238,7 +250,7 @@ bool OSQPEigenSolver::updateHessianMatrix(const SparseMatrix& hessian) bool success = solver_.data()->setHessianMatrix(cleaned); if (cleaned.nonZeros() == 0) /** @todo Remove when upgrading to OSQP 1.0.0 */ { - csc_spfree(solver_.data()->getData()->P); + csc_spfree_fix(solver_.data()->getData()->P); solver_.data()->getData()->P = nullptr; solver_.data()->getData()->P = csc_spalloc_fix(cleaned.rows(), cleaned.cols(), 0, 1, 0); }