From 3aa64921e9d76991fdb092e66cb0664e20ae23d2 Mon Sep 17 00:00:00 2001 From: Roelof Oomen Date: Thu, 7 Mar 2024 15:29:54 +0100 Subject: [PATCH] Also set matrix diagonal using triplets --- trajopt_sco/src/solver_utils.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/trajopt_sco/src/solver_utils.cpp b/trajopt_sco/src/solver_utils.cpp index d3b40512..12521af6 100644 --- a/trajopt_sco/src/solver_utils.cpp +++ b/trajopt_sco/src/solver_utils.cpp @@ -94,6 +94,11 @@ void exprToEigen(const QuadExpr& expr, } } } + + if (force_diagonal) + for (int k = 0; k < n_vars; ++k) + triplets.emplace_back(k, k, 0.0); + sparse_matrix.setFromTriplets(triplets.begin(), triplets.end()); auto sparse_matrix_T = Eigen::SparseMatrix(sparse_matrix.transpose()); @@ -101,10 +106,6 @@ void exprToEigen(const QuadExpr& expr, if (!matrix_is_halved) sparse_matrix = 0.5 * sparse_matrix; - - if (force_diagonal) - for (int k = 0; k < n_vars; ++k) - sparse_matrix.coeffRef(k, k) += 0.0; } void exprToEigen(const AffExprVector& expr_vec,