diff --git a/CHANGELOG.md b/CHANGELOG.md index c95665016..0df499d8a 100644 --- a/CHANGELOG.md +++ b/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)) diff --git a/include/proxsuite/proxqp/dense/solver.hpp b/include/proxsuite/proxqp/dense/solver.hpp index 0e544c3ac..6d428ae64 100644 --- a/include/proxsuite/proxqp/dense/solver.hpp +++ b/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& 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& 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: diff --git a/test/src/dense_qp_wrapper.cpp b/test/src/dense_qp_wrapper.cpp index d51f8609a..353fb7dce 100644 --- a/test/src/dense_qp_wrapper.cpp +++ b/test/src/dense_qp_wrapper.cpp @@ -1,5 +1,5 @@ // -// Copyright (c) 2022 INRIA +// Copyright (c) 2022-2024 INRIA // #include #include @@ -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 qp_random = proxqp::utils::dense_strongly_convex_qp( + dim, n_eq, n_in, sparsity_factor, strong_convexity_factor); + dense::QP 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(); + T dua_res = (qp_random.H * qp.results.x + qp_random.g + + qp_random.C.transpose() * qp.results.z) + .lpNorm(); + 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; +}