Merge pull request #349 from fabinsch/topic/fix-ldlt

Fix `mu_update` for `PrimalLDLT` backend

CHANGELOG.md

@@ -7,6 +7,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 ## [Unreleased]
 
 ### Added
+* Fix mu update function for PrimalLDLT backend ([#349](https://github.com/Simple-Robotics/proxsuite/pull/349))
 * Allow use of installed pybind11, cereal and jrl-cmakemodules via cmake
 * Add compatibility with jrl-cmakemodules workspace ([#339](https://github.com/Simple-Robotics/proxsuite/pull/339))
 * Specifically mention that timings are in microseconds ([#340](https://github.com/Simple-Robotics/proxsuite/pull/340))

include/proxsuite/proxqp/dense/solver.hpp

@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2022 INRIA
+// Copyright (c) 2022-2024 INRIA
 //
 /**
  * @file solver.hpp
@@ -195,7 +195,7 @@ mu_update(const Model<T>& qpmodel,
       {
         LDLT_TEMP_MAT_UNINIT(T, new_cols, qpmodel.dim, qpwork.n_c, stack);
         qpwork.dw_aug.head(qpmodel.dim).setOnes();
-        T delta_mu(mu_in_new - qpresults.info.mu_in_inv);
+        T delta_mu(T(1) / mu_in_new - qpresults.info.mu_in_inv);
         qpwork.dw_aug.head(qpmodel.dim).array() *= delta_mu;
         for (isize i = 0; i < n_constraints; ++i) {
           isize j = qpwork.current_bijection_map[i];
@@ -212,14 +212,17 @@ mu_update(const Model<T>& qpmodel,
           }
         }
         qpwork.ldl.rank_r_update(
-          new_cols, qpwork.dw_aug.head(qpmodel.dim), stack);
+          new_cols, qpwork.dw_aug.head(qpwork.n_c), stack);
       }
       // mu update for A
       {
         LDLT_TEMP_MAT_UNINIT(T, new_cols, qpmodel.dim, qpmodel.n_eq, stack);
+        qpwork.dw_aug.head(qpmodel.n_eq).setOnes();
+        T delta_mu(1 / mu_eq_new - qpresults.info.mu_eq_inv);
+        qpwork.dw_aug.head(qpmodel.n_eq).array() *= delta_mu;
         new_cols = qpwork.A_scaled.transpose();
         qpwork.ldl.rank_r_update(
-          new_cols, qpwork.dw_aug.head(qpmodel.dim), stack);
+          new_cols, qpwork.dw_aug.head(qpmodel.n_eq), stack);
       }
     } break;
     case DenseBackend::Automatic:

test/src/dense_qp_wrapper.cpp

@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2022 INRIA
+// Copyright (c) 2022-2024 INRIA
 //
 #include <iostream>
 #include <doctest.hpp>
@@ -7614,3 +7614,60 @@ TEST_CASE("ProxQP::dense: test memory allocation when estimating biggest "
   dense::power_iteration(H, dw, rhs, err_v, 1.E-6, 10000);
   PROXSUITE_EIGEN_MALLOC_ALLOWED();
 }
+
+TEST_CASE("ProxQP::dense: sparse random strongly convex qp with"
+          "inequality constraints: test PrimalLDLT backend mu update")
+{
+
+  std::cout << "---testing sparse random strongly convex qp with"
+               "inequality constraints: test PrimalLDLT backend mu update---"
+            << std::endl;
+  double sparsity_factor = 1;
+  utils::rand::set_seed(1);
+  isize dim = 3;
+  isize n_eq(0);
+  isize n_in(9);
+  T strong_convexity_factor(1.e-2);
+  proxqp::dense::Model<T> qp_random = proxqp::utils::dense_strongly_convex_qp(
+    dim, n_eq, n_in, sparsity_factor, strong_convexity_factor);
+  dense::QP<T> qp{
+    dim,
+    n_eq,
+    n_in,
+    false,
+    proxsuite::proxqp::HessianType::Dense,
+    proxsuite::proxqp::DenseBackend::PrimalLDLT
+  }; // creating QP object
+  T eps_abs = T(1e-7);
+  qp.settings.eps_abs = eps_abs;
+  qp.settings.eps_rel = 0;
+  qp.settings.compute_timings = true;
+  qp.settings.verbose = true;
+  qp.init(qp_random.H,
+          qp_random.g,
+          nullopt,
+          nullopt,
+          qp_random.C,
+          nullopt,
+          qp_random.u);
+  qp.solve();
+
+  DOCTEST_CHECK(qp.results.info.mu_updates > 0);
+
+  T pri_res = (helpers::negative_part(qp_random.C * qp.results.x - qp_random.l))
+                .lpNorm<Eigen::Infinity>();
+  T dua_res = (qp_random.H * qp.results.x + qp_random.g +
+               qp_random.C.transpose() * qp.results.z)
+                .lpNorm<Eigen::Infinity>();
+  DOCTEST_CHECK(pri_res <= eps_abs);
+  DOCTEST_CHECK(dua_res <= eps_abs);
+
+  std::cout << "------using API solving qp with dim: " << dim
+            << " neq: " << n_eq << " nin: " << n_in << std::endl;
+  std::cout << "primal residual: " << pri_res << std::endl;
+  std::cout << "dual residual: " << dua_res << std::endl;
+  std::cout << "total number of iteration: " << qp.results.info.iter
+            << std::endl;
+  std::cout << "setup timing " << qp.results.info.setup_time << " solve time "
+            << qp.results.info.solve_time << std::endl;
+}