From 8f5fcb30942b8b9c12c8a0eca41438537b9b0061 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 22 Jan 2025 10:31:32 +0100 Subject: [PATCH 1/3] Update OSQP workspace, instead of recreating --- .../include/trajopt_sco/osqp_interface.hpp | 4 +- trajopt_sco/src/osqp_interface.cpp | 88 +++++++++++++++---- 2 files changed, 75 insertions(+), 17 deletions(-) diff --git a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp index d77ed2b3..7fb5a70b 100644 --- a/trajopt_sco/include/trajopt_sco/osqp_interface.hpp +++ b/trajopt_sco/include/trajopt_sco/osqp_interface.hpp @@ -49,12 +49,12 @@ class OSQPModel : public Model /** Updates OSQP quadratic cost matrix from QuadExpr expression. * Transforms QuadExpr objective_ into the OSQP CSC matrix P_ */ - void updateObjective(); + bool updateObjective(); /** Updates qpOASES constraints from AffExpr expression. * Transforms AffExpr cntr_exprs_ and box bounds lbs_ and ubs_ into the * OSQP CSC matrix A_, and vectors lbA_ and ubA_ */ - void updateConstraints(); + bool updateConstraints(); /** Creates or updates the solver and its workspace */ void createOrUpdateSolver(); diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index 0926c239..5241ed1b 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -108,7 +108,7 @@ void OSQPModel::removeCnts(const CntVector& cnts) cnt.cnt_rep->removed = true; } -void OSQPModel::updateObjective() +bool OSQPModel::updateObjective() { const std::size_t n = vars_.size(); osqp_data_.n = static_cast(n); @@ -122,6 +122,16 @@ void OSQPModel::updateObjective() eigenToCSC(triangular_sm, P_row_indices_, P_column_pointers_, P_csc_data_); + // Check if sparsity has changed + bool sparsity_equal = false; + if (osqp_data_.P != nullptr && osqp_data_.P->n == osqp_data_.n && osqp_data_.P->m == osqp_data_.n && + osqp_data_.P->nzmax == P_csc_data_.size()) + { + sparsity_equal = memcmp(osqp_data_.P->p, P_column_pointers_.data(), static_cast(osqp_data_.P->n) + 1) == 0; + sparsity_equal = sparsity_equal && + (memcmp(osqp_data_.P->i, P_row_indices_.data(), static_cast(osqp_data_.P->nzmax)) == 0); + } + P_.reset(csc_matrix(osqp_data_.n, osqp_data_.n, static_cast(P_csc_data_.size()), @@ -131,9 +141,11 @@ void OSQPModel::updateObjective() osqp_data_.P = P_.get(); osqp_data_.q = q_.data(); + + return sparsity_equal; } -void OSQPModel::updateConstraints() +bool OSQPModel::updateConstraints() { const std::size_t n = vars_.size(); const std::size_t m = cnts_.size(); @@ -174,6 +186,16 @@ void OSQPModel::updateConstraints() eigenToCSC(sm, A_row_indices_, A_column_pointers_, A_csc_data_); + // Check if sparsity has changed + bool sparsity_equal = false; + if (osqp_data_.A != nullptr && osqp_data_.A->n == osqp_data_.n && osqp_data_.A->m == osqp_data_.m && + osqp_data_.A->nzmax == A_csc_data_.size()) + { + sparsity_equal = memcmp(osqp_data_.A->p, A_column_pointers_.data(), static_cast(osqp_data_.A->n) + 1) == 0; + sparsity_equal = sparsity_equal && + (memcmp(osqp_data_.A->i, A_row_indices_.data(), static_cast(osqp_data_.A->nzmax)) == 0); + } + A_.reset(csc_matrix(osqp_data_.m, osqp_data_.n, static_cast(A_csc_data_.size()), @@ -185,26 +207,62 @@ void OSQPModel::updateConstraints() osqp_data_.l = l_.data(); osqp_data_.u = u_.data(); + + return sparsity_equal; } void OSQPModel::createOrUpdateSolver() { - updateObjective(); - updateConstraints(); // NOLINT(clang-analyzer-core.UndefinedBinaryOperatorResult,clang-analyzer-core.uninitialized.Assign) + bool P_sparsity_equal = updateObjective(); + bool A_sparsity_equal = updateConstraints(); - // TODO atm we are not updating the workspace, but recreating it each time. - // In the future, we will checking sparsity did not change and update instead - if (osqp_workspace_ != nullptr) - osqp_cleanup(osqp_workspace_); + // If sparsity did not change, update data, otherwise cleanup and setup + bool need_setup = true; + if ((osqp_workspace_ != nullptr) && P_sparsity_equal && A_sparsity_equal) + { + LOG_DEBUG("OSQP update (warm start = %lli).", config_.settings.warm_start); + need_setup = false; + + if (osqp_update_lin_cost(osqp_workspace_, osqp_data_.q) != 0) + { + need_setup = true; + LOG_WARN("OSQP updating linear costs failed."); + } + if (osqp_update_bounds(osqp_workspace_, osqp_data_.l, osqp_data_.u) != 0) + { + need_setup = true; + LOG_WARN("OSQP updating bounds failed."); + } + if (osqp_update_P_A(osqp_workspace_, + osqp_data_.P->x, + OSQP_NULL, + osqp_data_.P->nzmax, + osqp_data_.A->x, + OSQP_NULL, + osqp_data_.A->nzmax) != 0) + { + need_setup = true; + LOG_WARN("OSQP updating P and A matrices failed."); + } + } - // Setup workspace - this should be called only once - auto ret = osqp_setup(&osqp_workspace_, &osqp_data_, &config_.settings); - if (ret != 0) + if (need_setup) { - // In this case, no data got allocated, so don't try to free it. - if (ret == OSQP_DATA_VALIDATION_ERROR || ret == OSQP_SETTINGS_VALIDATION_ERROR) - osqp_workspace_ = nullptr; - throw std::runtime_error("Could not initialize OSQP: error " + std::to_string(ret)); + if (osqp_workspace_ != nullptr) + { + LOG_DEBUG("OSQP cleanup (cold start)."); + osqp_cleanup(osqp_workspace_); + } + + // Setup workspace - this should be called only once + auto ret = osqp_setup(&osqp_workspace_, &osqp_data_, &config_.settings); + if (ret != 0) + { + // In this case, no data got allocated, so don't try to free it. + if (ret == OSQP_DATA_VALIDATION_ERROR || ret == OSQP_SETTINGS_VALIDATION_ERROR) + osqp_workspace_ = nullptr; + throw std::runtime_error("Could not initialize OSQP: error " + std::to_string(ret)); + } } } From 946e148546b802f7ce37176b5d86751b53a31632 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 22 Jan 2025 10:49:23 +0100 Subject: [PATCH 2/3] clang-tidy --- trajopt_sco/src/osqp_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index 5241ed1b..9e3ad3a5 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -213,8 +213,8 @@ bool OSQPModel::updateConstraints() void OSQPModel::createOrUpdateSolver() { - bool P_sparsity_equal = updateObjective(); - bool A_sparsity_equal = updateConstraints(); + const bool P_sparsity_equal = updateObjective(); + const bool A_sparsity_equal = updateConstraints(); // If sparsity did not change, update data, otherwise cleanup and setup bool need_setup = true; From bc611fa09267486ad4ffde7462a17e1f93e5445a Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Wed, 22 Jan 2025 22:34:40 +0100 Subject: [PATCH 3/3] Reorder updating --- trajopt_sco/src/osqp_interface.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/trajopt_sco/src/osqp_interface.cpp b/trajopt_sco/src/osqp_interface.cpp index 9e3ad3a5..e1efe0de 100644 --- a/trajopt_sco/src/osqp_interface.cpp +++ b/trajopt_sco/src/osqp_interface.cpp @@ -223,15 +223,15 @@ void OSQPModel::createOrUpdateSolver() LOG_DEBUG("OSQP update (warm start = %lli).", config_.settings.warm_start); need_setup = false; - if (osqp_update_lin_cost(osqp_workspace_, osqp_data_.q) != 0) + if (osqp_update_bounds(osqp_workspace_, osqp_data_.l, osqp_data_.u) != 0) { need_setup = true; - LOG_WARN("OSQP updating linear costs failed."); + LOG_WARN("OSQP updating bounds failed."); } - if (osqp_update_bounds(osqp_workspace_, osqp_data_.l, osqp_data_.u) != 0) + if (osqp_update_lin_cost(osqp_workspace_, osqp_data_.q) != 0) { need_setup = true; - LOG_WARN("OSQP updating bounds failed."); + LOG_WARN("OSQP updating linear costs failed."); } if (osqp_update_P_A(osqp_workspace_, osqp_data_.P->x,