From f1d95ee5f886e87ba2dd0dbd3f78892fef7adbb8 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 27 May 2026 21:26:42 -0700 Subject: [PATCH] [upstream_utils] Upgrade to Sleipnir 0.6.0 (#8923) --- upstream_utils/sleipnir.py | 2 +- .../sleipnir_patches/0001-Use-fmtlib.patch | 67 +++++- .../0002-Use-wpi-SmallVector.patch | 30 +-- .../0004-Replace-std-to_underlying.patch | 20 +- .../0005-Replace-std-views-zip.patch | 26 +- ...ppress-GCC-12-warning-false-positive.patch | 2 +- ...ress-Doxygen-warning-false-positives.patch | 4 +- ...tead-of-multidimensional-array-subsc.patch | 14 +- .../wpilib/math/autodiff/VariableMatrix.java | 4 +- .../include/sleipnir/autodiff/expression.hpp | 47 ++-- .../autodiff/gradient_expression_graph.hpp | 37 +-- .../include/sleipnir/autodiff/hessian.hpp | 5 +- .../include/sleipnir/autodiff/jacobian.hpp | 4 +- .../sleipnir/autodiff/variable_matrix.hpp | 8 +- .../include/sleipnir/optimization/problem.hpp | 71 ++++-- .../optimization/solver/interior_point.hpp | 183 ++++++++------ .../interior_point_matrix_callbacks.hpp | 10 + .../sleipnir/optimization/solver/newton.hpp | 56 +++-- .../solver/newton_matrix_callbacks.hpp | 8 + .../sleipnir/optimization/solver/sqp.hpp | 106 ++++---- .../solver/sqp_matrix_callbacks.hpp | 10 + .../optimization/solver/util/all_finite.hpp | 4 + .../solver/util/append_as_triplets.hpp | 4 + .../solver/util/dense_regularized_ldlt.hpp | 210 ++++++++++++++++ .../solver/util/feasibility_restoration.hpp | 82 +++++-- .../optimization/solver/util/filter.hpp | 1 - .../optimization/solver/util/inertia.hpp | 49 +++- .../solver/util/is_locally_infeasible.hpp | 12 +- .../optimization/solver/util/kkt_error.hpp | 137 +++++++++-- .../util/lagrange_multiplier_estimate.hpp | 13 +- .../solver/util/problem_scaling.hpp | 116 +++++++++ .../solver/util/regularized_ldlt.hpp | 213 +++++----------- .../solver/util/sparse_inf_norms.hpp | 34 +++ .../solver/util/sparse_regularized_ldlt.hpp | 227 ++++++++++++++++++ .../sleipnir/util/print_diagnostics.hpp | 87 ++++--- .../math/optimization/FlywheelOCPTest.java | 2 +- .../optimization/solver/ExitStatusTest.java | 5 + 37 files changed, 1356 insertions(+), 554 deletions(-) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/problem_scaling.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_inf_norms.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index 49d27d17cd..a388c21168 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -46,7 +46,7 @@ using small_vector = wpi::util::SmallVector; def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - tag = "v0.5.2" + tag = "v0.6.0" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index d61a30a529..edd3969f37 100644 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -7,9 +7,10 @@ Subject: [PATCH 01/10] Use fmtlib include/sleipnir/autodiff/expression_type.hpp | 16 +++++++---- include/sleipnir/optimization/problem.hpp | 1 + .../optimization/solver/exit_status.hpp | 16 +++++++---- + .../optimization/solver/util/inertia.hpp | 22 ++++++++------- include/sleipnir/util/assert.hpp | 5 ++-- include/sleipnir/util/print.hpp | 27 ++++++++++--------- - 5 files changed, 40 insertions(+), 25 deletions(-) + 6 files changed, 53 insertions(+), 34 deletions(-) diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac8446a3ab 100644 @@ -66,7 +67,7 @@ index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac + +// @endcond diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index a9553ffbcfed568c48f7d789d8b127790dfddb91..3de6d5bf89a65fe07784350c3b1a4691dfc0c822 100644 +index ccff8b9425b45fd1c790bc4da87b81f501a70d9c..41b44578b1e2783dbceb300b7c72ee0c8d1202df 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp @@ -15,6 +15,7 @@ @@ -131,6 +132,68 @@ index 0a48df7423b5a3dccd8e611e91befd32487fafdc..8786d6d64ac44ac88133df65a79636ec }; + +// @endcond +diff --git a/include/sleipnir/optimization/solver/util/inertia.hpp b/include/sleipnir/optimization/solver/util/inertia.hpp +index 49ee606f7e3f02f2f5c24079eb27a412dad3055f..2d2ce114be9c40e645d685b63465b977e0b70439 100644 +--- a/include/sleipnir/optimization/solver/util/inertia.hpp ++++ b/include/sleipnir/optimization/solver/util/inertia.hpp +@@ -2,10 +2,10 @@ + + #pragma once + +-#include + #include + + #include ++#include + + namespace slp { + +@@ -77,14 +77,16 @@ class Inertia { + + } // namespace slp + ++// @cond Suppress Doxygen ++ + /// Formatter for Inertia. + template <> +-struct std::formatter { ++struct fmt::formatter { + /// Parses format string. + /// + /// @param ctx Format parse context. + /// @return Format parse context iterator. +- constexpr auto parse(std::format_parse_context& ctx) { ++ constexpr auto parse(fmt::format_parse_context& ctx) { + return m_underlying.parse(ctx); + } + +@@ -95,18 +97,20 @@ struct std::formatter { + /// @param ctx Format context. + /// @return Format context iterator. + template +- auto format(const slp::Inertia& inertia, FmtContext& ctx) const { ++ constexpr auto format(const slp::Inertia& inertia, FmtContext& ctx) const { + auto out = ctx.out(); + +- out = std::format_to(out, "("); ++ out = fmt::format_to(out, "("); + out = m_underlying.format(inertia.positive, ctx); +- out = std::format_to(out, ", "); ++ out = fmt::format_to(out, ", "); + out = m_underlying.format(inertia.negative, ctx); +- out = std::format_to(out, ", "); ++ out = fmt::format_to(out, ", "); + out = m_underlying.format(inertia.zero, ctx); +- return std::format_to(out, ")"); ++ return fmt::format_to(out, ")"); + } + + private: +- std::formatter m_underlying; ++ fmt::formatter m_underlying; + }; ++ ++// @endcond diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp index 29dfe5416530a476417de3a4b6434d3b28d73120..783a0e71a1cb54bd1d6d17fa9d2bedd2038a8ef5 100644 --- a/include/sleipnir/util/assert.hpp diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 30d9aa569c..6efb521a55 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -4,13 +4,12 @@ Date: Sun, 16 Jun 2024 12:08:49 -0700 Subject: [PATCH 02/10] Use wpi::SmallVector --- - include/sleipnir/autodiff/expression.hpp | 4 ++-- - include/sleipnir/autodiff/variable.hpp | 4 ++-- - include/sleipnir/autodiff/variable_matrix.hpp | 4 ++-- - 3 files changed, 6 insertions(+), 6 deletions(-) + include/sleipnir/autodiff/expression.hpp | 4 ++-- + include/sleipnir/autodiff/variable.hpp | 4 ++-- + 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp -index 76f6d4ed534ce685194ad7fc62d9b62eb2ca1096..10e20f5e2fb0d7459fcecac2f8ba1bdcc98efc3f 100644 +index af10b505ae44ca0eb656f66f3f49172d7fc98c6d..7a9dc089a9e2fb323ecc8725f924ebee1b213af0 100644 --- a/include/sleipnir/autodiff/expression.hpp +++ b/include/sleipnir/autodiff/expression.hpp @@ -34,7 +34,7 @@ struct Expression; @@ -22,7 +21,7 @@ index 76f6d4ed534ce685194ad7fc62d9b62eb2ca1096..10e20f5e2fb0d7459fcecac2f8ba1bdc /// Typedef for intrusive shared pointer to Expression. /// -@@ -736,7 +736,7 @@ constexpr void inc_ref_count(Expression* expr) { +@@ -744,7 +744,7 @@ constexpr void inc_ref_count(Expression* expr) { /// @tparam Scalar Scalar type. /// @param expr The shared pointer's managed object. template @@ -53,22 +52,3 @@ index 910ebcf5266bdf76711db7849dd6584549094e3b..3c4f67c1f6224226620e2ffcc5973364 : expr{std::move(expr)} {} /// Assignment operator for scalar. -diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 8ed39dfae69c70e0b32998eb5e0c5207383f4fa2..3eeaccac274cf38d554ea834b3ff666615939382 100644 ---- a/include/sleipnir/autodiff/variable_matrix.hpp -+++ b/include/sleipnir/autodiff/variable_matrix.hpp -@@ -1134,12 +1134,12 @@ class VariableMatrix : public SleipnirBase { - /// Returns const begin iterator. - /// - /// @return Const begin iterator. -- const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; } -+ const_iterator cbegin() const { return const_iterator{m_storage.begin()}; } - - /// Returns const end iterator. - /// - /// @return Const end iterator. -- const_iterator cend() const { return const_iterator{m_storage.cend()}; } -+ const_iterator cend() const { return const_iterator{m_storage.end()}; } - - /// Returns reverse begin iterator. - /// diff --git a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch index 4411ecafa0..0eb62d9e04 100644 --- a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch +++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch @@ -11,10 +11,10 @@ Subject: [PATCH 04/10] Replace std::to_underlying() create mode 100644 include/sleipnir/util/to_underlying.hpp diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb2094303ee2e 100644 +index 41b44578b1e2783dbceb300b7c72ee0c8d1202df..49921b98de3452b6ca9f2e33a86daa254c0d3a01 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp -@@ -37,6 +37,7 @@ +@@ -38,6 +38,7 @@ #include "sleipnir/util/profiler.hpp" #include "sleipnir/util/spy.hpp" #include "sleipnir/util/symbol_exports.hpp" @@ -22,7 +22,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209 namespace slp { -@@ -752,11 +753,11 @@ class Problem { +@@ -773,11 +774,11 @@ class Problem { // Print problem structure slp::println("\nProblem structure:"); slp::println(" ↳ {} cost function", @@ -37,7 +37,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209 if (m_decision_variables.size() == 1) { slp::print("\n1 decision variable\n"); -@@ -768,7 +769,7 @@ class Problem { +@@ -789,7 +790,7 @@ class Problem { [](const gch::small_vector>& constraints) { std::array counts{}; for (const auto& constraint : constraints) { @@ -47,7 +47,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209 for (const auto& [count, name] : std::views::zip(counts, std::array{"empty", "constant", "linear", diff --git a/include/sleipnir/util/print_diagnostics.hpp b/include/sleipnir/util/print_diagnostics.hpp -index e16ef1b03e675b7ccb8db6781568eec7a44aace7..69cb99957cc7da5abc4162c53a229f7f642bbe4b 100644 +index 87ccad9ef4b05ee5aeaaf0e5ff9cfbc7e995d5ed..831ae3e5d1002005a8aa00fc80accd705d5cca7d 100644 --- a/include/sleipnir/util/print_diagnostics.hpp +++ b/include/sleipnir/util/print_diagnostics.hpp @@ -17,6 +17,7 @@ @@ -58,15 +58,15 @@ index e16ef1b03e675b7ccb8db6781568eec7a44aace7..69cb99957cc7da5abc4162c53a229f7f namespace slp { -@@ -239,7 +240,7 @@ void print_iteration_diagnostics(int iterations, IterationType type, +@@ -230,7 +231,7 @@ void print_iteration_diagnostics(int iterations, IterationType type, slp::println( - "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} " - "{:.2e} {:2d}│", + "│{:4} {:1} {:9.3f} {:10.4e} {:11.4e} {:10.4e} {:8.2e} {:8.2e} {:<5} " + "{:<5} {:8.2e} {:8.2e} {:8.2e} {:8.2e} {:2d}│", - iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error, + iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error, - cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α, + cost, infeasibility, complementarity, μ, power_of_10(δ), power_of_10(γ), + full_primal_step_inf_norm, full_dual_step_inf_norm, primal_α, dual_α, backtracks); - } diff --git a/include/sleipnir/util/to_underlying.hpp b/include/sleipnir/util/to_underlying.hpp new file mode 100644 index 0000000000000000000000000000000000000000..3f9b4835b912c5a0d998c43828f255c61d0f573e diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch index 9ea42ad62c..17e36fbcf5 100644 --- a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch +++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch @@ -9,26 +9,26 @@ Subject: [PATCH 05/10] Replace std::views::zip() 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/include/sleipnir/autodiff/gradient_expression_graph.hpp b/include/sleipnir/autodiff/gradient_expression_graph.hpp -index 275c30b76d34efe7ee1608cd4eedfa54ab2dc1ec..c0e3c161343175837abcb25fb22da0c611a44799 100644 +index fda7931c686a315209acdbe6f1ac581944e2781c..d3e59068d04b3db01945c5657959b69cd4522cca 100644 --- a/include/sleipnir/autodiff/gradient_expression_graph.hpp +++ b/include/sleipnir/autodiff/gradient_expression_graph.hpp -@@ -161,7 +161,10 @@ class GradientExpressionGraph { - } +@@ -139,7 +139,10 @@ class GradientExpressionGraph { } - } else { -- for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) { -+ for (size_t i = 0; i < m_top_list.size(); ++i) { -+ const auto& col = m_col_list[i]; -+ const auto& node = m_top_list[i]; + } + +- for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) { ++ for (size_t i = 0; i < m_top_list.size(); ++i) { ++ const auto& col = m_col_list[i]; ++ const auto& node = m_top_list[i]; + - // Append adjoints of wrt to sparse matrix triplets - if (col != -1 && node->adjoint != Scalar(0)) { - triplets.emplace_back(row, col, node->adjoint); + // Append adjoints of wrt to sparse matrix triplets + if (col != -1) { + triplets.emplace_back(row, col, node->adjoint); diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 273363db2cf75c84d81f2fef36dcb2094303ee2e..c4a45f7f954c9e028b1c85c45d5034bde284474f 100644 +index 49921b98de3452b6ca9f2e33a86daa254c0d3a01..d3feadd577bd53147a8b07da76910e046ccbf95d 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp -@@ -771,9 +771,11 @@ class Problem { +@@ -792,9 +792,11 @@ class Problem { for (const auto& constraint : constraints) { ++counts[slp::to_underlying(constraint.type())]; } diff --git a/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch b/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch index c4101f404a..0ec037ffe0 100644 --- a/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch +++ b/upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch @@ -8,7 +8,7 @@ Subject: [PATCH 08/10] Suppress GCC 12 warning false positive 1 file changed, 7 insertions(+) diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 3eeaccac274cf38d554ea834b3ff666615939382..38b90b4a1c784cfd2fd806536d001db9335ebe2d 100644 +index 2cf6e00f16f58a5847a4515f13b3a91fffbb72b8..b5d15edc5b294a2ea50dca15a7612c61e5f2680b 100644 --- a/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -503,6 +503,10 @@ class VariableMatrix : public SleipnirBase { diff --git a/upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch b/upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch index 5e871cb648..4fac23d741 100644 --- a/upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch +++ b/upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch @@ -8,7 +8,7 @@ Subject: [PATCH 09/10] Suppress Doxygen warning false positives 1 file changed, 4 insertions(+) diff --git a/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp b/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp -index ec87b2b8160051224de870c22f5ded6b35f6dc2d..1c042f8599e8e179c1586b2648e1ff99e5b5d32a 100644 +index 6324ddcf2d24a0cd5367420af4330e1c59b58004..ed128e5a583d51bc963f03f6b88c0a15e6440545 100644 --- a/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp +++ b/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp @@ -96,6 +96,8 @@ compute_p_n(const Eigen::Vector& c, Scalar ρ, @@ -20,7 +20,7 @@ index ec87b2b8160051224de870c22f5ded6b35f6dc2d..1c042f8599e8e179c1586b2648e1ff99 /// Finds the iterate that minimizes the constraint violation while not /// deviating too far from the starting point. This is a fallback procedure when /// the normal Sequential Quadratic Programming method fails to converge to a -@@ -592,6 +594,8 @@ ExitStatus feasibility_restoration( +@@ -624,6 +626,8 @@ ExitStatus feasibility_restoration( } } diff --git a/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch b/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch index 6035ba27b7..a9dc822a34 100644 --- a/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch +++ b/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch @@ -15,10 +15,10 @@ Subject: [PATCH 10/10] Use operator() instead of multidimensional array 7 files changed, 135 insertions(+), 135 deletions(-) diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp -index 11af6033d1cf00b27cb9ff796c4a7f16b32df90a..d4cc02f54accd568d052ae5709213d45c4c8917a 100644 +index 2a8087eda23f49f8c01be35d5289568644a9742d..c48e01c7b89d80db8032f6d2e492a0ec1bfa9f42 100644 --- a/include/sleipnir/autodiff/hessian.hpp +++ b/include/sleipnir/autodiff/hessian.hpp -@@ -99,9 +99,9 @@ class Hessian { +@@ -100,9 +100,9 @@ class Hessian { auto grad = m_graphs[row].generate_tree(m_wrt); for (int col = 0; col < m_wrt.rows(); ++col) { if (grad[col].expr != nullptr) { @@ -31,7 +31,7 @@ index 11af6033d1cf00b27cb9ff796c4a7f16b32df90a..d4cc02f54accd568d052ae5709213d45 } } diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp -index f1915a0d08c509f4eb4d032d4e6a5e6099b68ebc..edd949eefd018af1b1e091b4b2780fc1a0a492cf 100644 +index 7a5c3798dab50f14db35bee430f1d5e8e32196ff..1a5b01de99a7c011ebf8c5ed9fc64942675fcb4b 100644 --- a/include/sleipnir/autodiff/jacobian.hpp +++ b/include/sleipnir/autodiff/jacobian.hpp @@ -104,9 +104,9 @@ class Jacobian { @@ -388,7 +388,7 @@ index 4018606df45941b578c861caf934495f8c9e368e..cf554832b82adb17b4b1d7b56842a77d } diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 38b90b4a1c784cfd2fd806536d001db9335ebe2d..e9f90fb12d230e40497e8557b985ff9c82ebaaeb 100644 +index b5d15edc5b294a2ea50dca15a7612c61e5f2680b..02bccb660474d9380444f5e20124fb687e372982 100644 --- a/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -154,7 +154,7 @@ class VariableMatrix : public SleipnirBase { @@ -959,10 +959,10 @@ index 5e20b9b8debc13611d5d719b589d8fd896c35a90..e5163f9c2dbbfa0a580d029e61c741e6 if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { x_end = rk4, diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index c4a45f7f954c9e028b1c85c45d5034bde284474f..4e38a082855c07c47f1aa191613114a5a392a4e4 100644 +index d3feadd577bd53147a8b07da76910e046ccbf95d..18afc996590de98050bfd3ddc629952bf0c8c2e2 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp -@@ -97,7 +97,7 @@ class Problem { +@@ -98,7 +98,7 @@ class Problem { for (int row = 0; row < rows; ++row) { for (int col = 0; col < cols; ++col) { m_decision_variables.emplace_back(); @@ -971,7 +971,7 @@ index c4a45f7f954c9e028b1c85c45d5034bde284474f..4e38a082855c07c47f1aa191613114a5 } } -@@ -132,8 +132,8 @@ class Problem { +@@ -133,8 +133,8 @@ class Problem { for (int row = 0; row < rows; ++row) { for (int col = 0; col <= row; ++col) { m_decision_variables.emplace_back(); diff --git a/wpimath/src/main/java/org/wpilib/math/autodiff/VariableMatrix.java b/wpimath/src/main/java/org/wpilib/math/autodiff/VariableMatrix.java index 1c7a7ffb4d..38ccb13709 100644 --- a/wpimath/src/main/java/org/wpilib/math/autodiff/VariableMatrix.java +++ b/wpimath/src/main/java/org/wpilib/math/autodiff/VariableMatrix.java @@ -904,11 +904,11 @@ public class VariableMatrix implements AutoCloseable, Iterable { } /** - * Returns a variable matrix filled with zeroes. + * Returns a variable matrix filled with zeros. * * @param rows The number of matrix rows. * @param cols The number of matrix columns. - * @return A variable matrix filled with zeroes. + * @return A variable matrix filled with zeros. */ public static VariableMatrix zero(int rows, int cols) { return new VariableMatrix(new SimpleMatrix(rows, cols)); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp index 10e20f5e2f..7a9dc089a9 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp @@ -153,14 +153,16 @@ struct Expression { // Prune expression if (lhs->is_constant(Scalar(0))) { - // Return zero + // Return zero, which lhs currently is return lhs; } else if (rhs->is_constant(Scalar(0))) { - // Return zero + // Return zero, which rhs currently is return rhs; } else if (lhs->is_constant(Scalar(1))) { + // Return rhs unmodified return rhs; } else if (rhs->is_constant(Scalar(1))) { + // Return lhs unmodified return lhs; } @@ -203,9 +205,10 @@ struct Expression { // Prune expression if (lhs->is_constant(Scalar(0))) { - // Return zero + // Return zero, which lhs currently is return lhs; } else if (rhs->is_constant(Scalar(1))) { + // Return lhs unmodified return lhs; } @@ -236,10 +239,13 @@ struct Expression { const ExpressionPtr& rhs) { using enum ExpressionType; - // Prune expression + // Prune expression. We check for nullptr because operator+ is used in + // adjoint accumulation, and child nodes can be null. if (lhs == nullptr || lhs->is_constant(Scalar(0))) { + // Return rhs unmodified return rhs; } else if (rhs == nullptr || rhs->is_constant(Scalar(0))) { + // Return lhs unmodified return lhs; } @@ -281,12 +287,14 @@ struct Expression { // Prune expression if (lhs->is_constant(Scalar(0))) { if (rhs->is_constant(Scalar(0))) { - // Return zero + // Return zero, which rhs currently is return rhs; } else { + // Return rhs negated return -rhs; } } else if (rhs->is_constant(Scalar(0))) { + // Return lhs unmodified return lhs; } @@ -316,7 +324,7 @@ struct Expression { // Prune expression if (lhs->is_constant(Scalar(0))) { - // Return zero + // Return zero, which lhs currently is return lhs; } @@ -825,7 +833,7 @@ ExpressionPtr abs(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -934,7 +942,7 @@ ExpressionPtr asin(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -988,7 +996,7 @@ ExpressionPtr atan(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -1056,7 +1064,7 @@ ExpressionPtr atan2(const ExpressionPtr& y, // Prune expression if (y->is_constant(Scalar(0))) { - // Return zero + // Return zero, which y currently is return y; } else if (x->is_constant(Scalar(0))) { return constant_ptr(Scalar(std::numbers::pi) / Scalar(2)); @@ -1223,7 +1231,7 @@ ExpressionPtr erf(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -1406,7 +1414,7 @@ ExpressionPtr log(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -1460,7 +1468,7 @@ ExpressionPtr log10(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -1708,15 +1716,16 @@ ExpressionPtr pow(const ExpressionPtr& base, // Prune expression if (base->is_constant(Scalar(0))) { - // Return zero + // Return zero, which base currently is return base; } else if (base->is_constant(Scalar(1))) { - // Return one + // Return one, which base currently is return base; } if (power->is_constant(Scalar(0))) { return constant_ptr(Scalar(1)); } else if (power->is_constant(Scalar(1))) { + // Return base unmodified return base; } @@ -1828,7 +1837,7 @@ ExpressionPtr sin(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -1883,7 +1892,7 @@ ExpressionPtr sinh(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -1997,7 +2006,7 @@ ExpressionPtr tan(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } @@ -2055,7 +2064,7 @@ ExpressionPtr tanh(const ExpressionPtr& x) { // Prune expression if (x->is_constant(Scalar(0))) { - // Return zero + // Return zero, which x currently is return x; } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp index c0e3c16134..d3e59068d0 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp @@ -102,22 +102,11 @@ class GradientExpressionGraph { /// /// @param triplets The sparse matrix triplets. /// @param row The row of wrt. - /// @param wrt Vector of variables with respect to which to compute the - /// Jacobian. void append_triplets(gch::small_vector>& triplets, - int row, const VariableMatrix& wrt) const { - slp_assert(wrt.cols() == 1); - + int row) const { // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation // for background on reverse accumulation automatic differentiation. - // If wrt has fewer nodes than graph, zero wrt's adjoints - if (static_cast(wrt.rows()) < m_top_list.size()) { - for (const auto& elem : wrt) { - elem.expr->adjoint = Scalar(0); - } - } - if (m_top_list.empty()) { return; } @@ -150,25 +139,13 @@ class GradientExpressionGraph { } } - // If wrt has fewer nodes than graph, iterate over wrt - if (static_cast(wrt.rows()) < m_top_list.size()) { - for (int col = 0; col < wrt.rows(); ++col) { - const auto& node = wrt[col].expr; + for (size_t i = 0; i < m_top_list.size(); ++i) { + const auto& col = m_col_list[i]; + const auto& node = m_top_list[i]; - // Append adjoints of wrt to sparse matrix triplets - if (node->adjoint != Scalar(0)) { - triplets.emplace_back(row, col, node->adjoint); - } - } - } else { - for (size_t i = 0; i < m_top_list.size(); ++i) { - const auto& col = m_col_list[i]; - const auto& node = m_top_list[i]; - - // Append adjoints of wrt to sparse matrix triplets - if (col != -1 && node->adjoint != Scalar(0)) { - triplets.emplace_back(row, col, node->adjoint); - } + // Append adjoints of wrt to sparse matrix triplets + if (col != -1) { + triplets.emplace_back(row, col, node->adjoint); } } } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp index d4cc02f54a..c48e01c7b8 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp @@ -23,6 +23,7 @@ namespace slp { /// /// @tparam Scalar Scalar type. /// @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper). +/// Default is Lower | Upper. template requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper)) class Hessian { @@ -69,7 +70,7 @@ class Hessian { // If the row is linear, compute its gradient once here and cache its // triplets. Constant rows are ignored because their gradients have no // nonzero triplets. - m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt); + m_graphs[row].append_triplets(m_cached_triplets, row); } else if (m_variables[row].type() > ExpressionType::LINEAR) { // If the row is quadratic or nonlinear, add it to the list of nonlinear // rows to be recomputed in value(). @@ -127,7 +128,7 @@ class Hessian { // Compute each nonlinear row of the Hessian for (int row : m_nonlinear_rows) { - m_graphs[row].append_triplets(triplets, row, m_wrt); + m_graphs[row].append_triplets(triplets, row); } m_H.setFromTriplets(triplets.begin(), triplets.end()); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp index edd949eefd..1a5b01de99 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp @@ -77,7 +77,7 @@ class Jacobian { // If the row is linear, compute its gradient once here and cache its // triplets. Constant rows are ignored because their gradients have no // nonzero triplets. - m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt); + m_graphs[row].append_triplets(m_cached_triplets, row); } else if (m_variables[row].type() > ExpressionType::LINEAR) { // If the row is quadratic or nonlinear, add it to the list of nonlinear // rows to be recomputed in value(). @@ -132,7 +132,7 @@ class Jacobian { // Compute each nonlinear row of the Jacobian for (int row : m_nonlinear_rows) { - m_graphs[row].append_triplets(triplets, row, m_wrt); + m_graphs[row].append_triplets(triplets, row); } m_J.setFromTriplets(triplets.begin(), triplets.end()); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp index e9f90fb12d..02bccb6604 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp @@ -1141,12 +1141,12 @@ class VariableMatrix : public SleipnirBase { /// Returns const begin iterator. /// /// @return Const begin iterator. - const_iterator cbegin() const { return const_iterator{m_storage.begin()}; } + const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; } /// Returns const end iterator. /// /// @return Const end iterator. - const_iterator cend() const { return const_iterator{m_storage.end()}; } + const_iterator cend() const { return const_iterator{m_storage.cend()}; } /// Returns reverse begin iterator. /// @@ -1191,11 +1191,11 @@ class VariableMatrix : public SleipnirBase { /// @return Number of elements in matrix. size_t size() const { return m_storage.size(); } - /// Returns a variable matrix filled with zeroes. + /// Returns a variable matrix filled with zeros. /// /// @param rows The number of matrix rows. /// @param cols The number of matrix columns. - /// @return A variable matrix filled with zeroes. + /// @return A variable matrix filled with zeros. static VariableMatrix zero(int rows, int cols) { VariableMatrix result{detail::empty, rows, cols}; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp index 4e38a08285..18afc99659 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp @@ -31,6 +31,7 @@ #include "sleipnir/optimization/solver/options.hpp" #include "sleipnir/optimization/solver/sqp.hpp" #include "sleipnir/optimization/solver/util/bounds.hpp" +#include "sleipnir/optimization/solver/util/problem_scaling.hpp" #include "sleipnir/util/empty.hpp" #include "sleipnir/util/print.hpp" #include "sleipnir/util/print_diagnostics.hpp" @@ -113,7 +114,7 @@ class Problem { /// Decision variables have an initial value of zero. /// /// @param rows Number of matrix rows. - /// @return A symmetric matrix of decision varaibles in the optimization + /// @return A symmetric matrix of decision variables in the optimization /// problem. [[nodiscard]] VariableMatrix symmetric_decision_variable(int rows) { @@ -377,20 +378,26 @@ class Problem { } #endif + // Automatically scale the cost. The problem scaling procedure is + // described in more detail in docs/algorithms.md#problem-scaling. + x_ad.set_value(x); + const ProblemScaling scaling{g.value()}; + NewtonMatrixCallbacks matrix_callbacks{ num_decision_variables, [&](const DenseVector& x) -> Scalar { x_ad.set_value(x); - return f.value(); + return scaling.f * f.value(); }, [&](const DenseVector& x) -> SparseVector { x_ad.set_value(x); - return g.value(); + return scaling.f * g.value(); }, [&](const DenseVector& x) -> SparseMatrix { x_ad.set_value(x); - return H.value(); - }}; + return scaling.f * H.value(); + }, + scaling}; // Invoke Newton solver status = @@ -465,35 +472,42 @@ class Problem { } #endif + // Automatically scale the cost and constraints. The problem scaling + // procedure is described in more detail in + // docs/algorithms.md#problem-scaling. + x_ad.set_value(x); + const ProblemScaling scaling{g.value(), A_e.value()}; + SQPMatrixCallbacks matrix_callbacks{ num_decision_variables, num_equality_constraints, [&](const DenseVector& x) -> Scalar { x_ad.set_value(x); - return f.value(); + return scaling.f * f.value(); }, [&](const DenseVector& x) -> SparseVector { x_ad.set_value(x); - return g.value(); + return scaling.f * g.value(); }, [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { x_ad.set_value(x); - y_ad.set_value(y); - return H_f.value() + H_c.value(); + y_ad.set_value(scaling.c_e.cwiseProduct(y)); + return scaling.f * H_f.value() + H_c.value(); }, [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { x_ad.set_value(x); - y_ad.set_value(y); + y_ad.set_value(scaling.c_e.cwiseProduct(y)); return H_c.value(); }, [&](const DenseVector& x) -> DenseVector { x_ad.set_value(x); - return c_e_ad.value(); + return scaling.c_e.cwiseProduct(c_e_ad.value()); }, [&](const DenseVector& x) -> SparseMatrix { x_ad.set_value(x); - return A_e.value(); - }}; + return scaling.c_e.asDiagonal() * A_e.value(); + }, + scaling}; // Invoke SQP solver status = sqp(matrix_callbacks, iteration_callbacks, options, x); @@ -597,48 +611,55 @@ class Problem { project_onto_bounds(x, bounds); #endif + // Automatically scale the cost and constraints. The problem scaling + // procedure is described in more detail in + // docs/algorithms.md#problem-scaling. + x_ad.set_value(x); + const ProblemScaling scaling{g.value(), A_e.value(), A_i.value()}; + InteriorPointMatrixCallbacks matrix_callbacks{ num_decision_variables, num_equality_constraints, num_inequality_constraints, [&](const DenseVector& x) -> Scalar { x_ad.set_value(x); - return f.value(); + return scaling.f * f.value(); }, [&](const DenseVector& x) -> SparseVector { x_ad.set_value(x); - return g.value(); + return scaling.f * g.value(); }, [&](const DenseVector& x, const DenseVector& y, const DenseVector& z) -> SparseMatrix { x_ad.set_value(x); - y_ad.set_value(y); - z_ad.set_value(z); - return H_f.value() + H_c.value(); + y_ad.set_value(scaling.c_e.cwiseProduct(y)); + z_ad.set_value(scaling.c_i.cwiseProduct(z)); + return scaling.f * H_f.value() + H_c.value(); }, [&](const DenseVector& x, const DenseVector& y, const DenseVector& z) -> SparseMatrix { x_ad.set_value(x); - y_ad.set_value(y); - z_ad.set_value(z); + y_ad.set_value(scaling.c_e.cwiseProduct(y)); + z_ad.set_value(scaling.c_i.cwiseProduct(z)); return H_c.value(); }, [&](const DenseVector& x) -> DenseVector { x_ad.set_value(x); - return c_e_ad.value(); + return scaling.c_e.cwiseProduct(c_e_ad.value()); }, [&](const DenseVector& x) -> SparseMatrix { x_ad.set_value(x); - return A_e.value(); + return scaling.c_e.asDiagonal() * A_e.value(); }, [&](const DenseVector& x) -> DenseVector { x_ad.set_value(x); - return c_i_ad.value(); + return scaling.c_i.cwiseProduct(c_i_ad.value()); }, [&](const DenseVector& x) -> SparseMatrix { x_ad.set_value(x); - return A_i.value(); - }}; + return scaling.c_i.asDiagonal() * A_i.value(); + }, + scaling}; // Invoke interior-point method solver status = diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp index 034edc1691..f9a8aec568 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include @@ -77,13 +76,14 @@ ExitStatus interior_point( DenseVector y = DenseVector::Zero(matrix_callbacks.num_equality_constraints); DenseVector z = DenseVector::Ones(matrix_callbacks.num_inequality_constraints); - Scalar μ(0.1); + Scalar μ = Scalar(0.1) * matrix_callbacks.scaling.f; + int iterations = 0; return interior_point(matrix_callbacks, iteration_callbacks, options, false, #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION bound_constraint_mask, #endif - x, s, y, z, μ); + x, s, y, z, μ, iterations); } /// Finds the optimal solution to a nonlinear program using the interior-point @@ -117,6 +117,7 @@ ExitStatus interior_point( /// constraint dual variables. /// @param[in,out] μ The initial guess and output location for the barrier /// parameter. +/// @param[in,out] iterations The iteration counter. /// @return The exit status. template ExitStatus interior_point( @@ -130,7 +131,7 @@ ExitStatus interior_point( Eigen::Vector& x, Eigen::Vector& s, Eigen::Vector& y, - Eigen::Vector& z, Scalar& μ) { + Eigen::Vector& z, Scalar& μ, int& iterations) { using DenseVector = Eigen::Vector; using SparseMatrix = Eigen::SparseMatrix; using SparseVector = Eigen::SparseVector; @@ -162,7 +163,7 @@ ExitStatus interior_point( solve_profilers.emplace_back(" ↳ KKT system solve"); solve_profilers.emplace_back(" ↳ line search"); solve_profilers.emplace_back(" ↳ SOC"); - solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ feas. restoration"); solve_profilers.emplace_back(" ↳ f(x)"); solve_profilers.emplace_back(" ↳ ∇f(x)"); solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); @@ -182,7 +183,7 @@ ExitStatus interior_point( auto& kkt_system_solve_prof = solve_profilers[7]; auto& line_search_prof = solve_profilers[8]; auto& soc_prof = solve_profilers[9]; - auto& next_iter_prep_prof = solve_profilers[10]; + auto& feasibility_restoration_prof = solve_profilers[10]; // Set up profiled matrix callbacks #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS @@ -232,7 +233,8 @@ ExitStatus interior_point( [&](const DenseVector& x) -> SparseMatrix { ScopedProfiler prof{A_i_prof}; return matrix_callbacks.A_i(x); - }}; + }, + matrix_callbacks.scaling}; #else const auto& matrices = matrix_callbacks; #endif @@ -259,6 +261,15 @@ ExitStatus interior_point( slp_assert(A_i.rows() == matrices.num_inequality_constraints); slp_assert(A_i.cols() == matrices.num_decision_variables); + DenseVector trial_x; + DenseVector trial_s; + DenseVector trial_y; + DenseVector trial_z; + + Scalar trial_f; + DenseVector trial_c_e; + DenseVector trial_c_i; + // Check for overconstrained problem if (matrices.num_equality_constraints > matrices.num_decision_variables) { if (options.diagnostics) { @@ -279,10 +290,9 @@ ExitStatus interior_point( s = bound_constraint_mask.select(c_i, s); #endif - int iterations = 0; - // Barrier parameter minimum - const Scalar μ_min = Scalar(options.tolerance) / Scalar(10); + const Scalar μ_min = + matrices.scaling.f * Scalar(options.tolerance) / Scalar(10); // Fraction-to-the-boundary rule scale factor minimum constexpr Scalar τ_min(0.99); @@ -325,10 +335,20 @@ ExitStatus interior_point( // Kept outside the loop so its storage can be reused gch::small_vector> triplets; - // Constraint regularization is forced to zero in feasibility restoration - // because the equality constraint Jacobian cannot be rank-deficient + const int lhs_rows = + matrices.num_decision_variables + matrices.num_equality_constraints; RegularizedLDLT solver{ + // Use sparse solver if lower triangle fills < 25% of system + H.nonZeros() + + (A_i.transpose() * A_i) + .template triangularView() + .eval() + .nonZeros() + + A_e.nonZeros() < + 0.25 * lhs_rows * lhs_rows, matrices.num_decision_variables, matrices.num_equality_constraints, + // Constraint regularization is forced to zero in feasibility restoration + // because the equality constraint Jacobian cannot be rank-deficient in_feasibility_restoration ? Scalar(0) : Scalar(1e-10)}; // Variables for determining when a step is acceptable @@ -338,7 +358,8 @@ ExitStatus interior_point( int full_step_rejected_counter = 0; // Error - Scalar E_0 = std::numeric_limits::infinity(); + Scalar E_0 = unscaled_kkt_error( + matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0)); setup_prof.stop(); @@ -479,13 +500,22 @@ ExitStatus interior_point( // Loop until a step is accepted while (1) { - DenseVector trial_x = x + α * step.p_x; - DenseVector trial_y = y + α_z * step.p_y; - DenseVector trial_z = z + α_z * step.p_z; + trial_x = x + α * step.p_x; + if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) { + // If the inequality constraints are all feasible, prevent them from + // becoming infeasible again. + // + // See equation (19.30) in [1]. + trial_s = trial_c_i; + } else { + trial_s = s + α * step.p_s; + } + trial_y = y + α_z * step.p_y; + trial_z = z + α_z * step.p_z; - Scalar trial_f = matrices.f(trial_x); - DenseVector trial_c_e = matrices.c_e(trial_x); - DenseVector trial_c_i = matrices.c_i(trial_x); + trial_f = matrices.f(trial_x); + trial_c_e = matrices.c_e(trial_x); + trial_c_i = matrices.c_i(trial_x); // If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce // step size immediately @@ -501,17 +531,6 @@ ExitStatus interior_point( continue; } - DenseVector trial_s; - if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) { - // If the inequality constraints are all feasible, prevent them from - // becoming infeasible again. - // - // See equation (19.30) in [1]. - trial_s = trial_c_i; - } else { - trial_s = s + α * step.p_s; - } - // Check whether filter accepts trial iterate FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ}; if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) { @@ -538,6 +557,8 @@ ExitStatus interior_point( Scalar α_z_soc = α_z; DenseVector c_e_soc = c_e; + Scalar soc_constraint_violation = next_constraint_violation; + bool step_acceptable = false; for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable; ++soc_iteration) { @@ -546,19 +567,22 @@ ExitStatus interior_point( scope_exit soc_exit{[&] { soc_profiler.stop(); - if (options.diagnostics) { + if (options.diagnostics && step_acceptable) { print_iteration_diagnostics( - iterations, - step_acceptable ? IterationType::ACCEPTED_SOC - : IterationType::REJECTED_SOC, + iterations, IterationType::SECOND_ORDER_CORRECTION, soc_profiler.current_duration(), - kkt_error( - g, A_e, trial_c_e, A_i, trial_c_i, trial_s, trial_y, - trial_z, Scalar(0)), + unscaled_kkt_error( + matrices.scaling, g, A_e, trial_c_e, A_i, trial_c_i, + trial_s, trial_y, trial_z, Scalar(0)), trial_f, trial_c_e.template lpNorm<1>() + (trial_c_i - trial_s).template lpNorm<1>(), trial_s.dot(trial_z), μ, solver.hessian_regularization(), + solver.constraint_jacobian_regularization(), + std::max(soc_step.p_x.template lpNorm(), + soc_step.p_s.template lpNorm()), + std::max(soc_step.p_y.template lpNorm(), + soc_step.p_z.template lpNorm()), α_soc, Scalar(1), α_reduction_factor, α_z_soc); } }}; @@ -576,12 +600,12 @@ ExitStatus interior_point( compute_step(soc_step); // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ) + // αₖᶻˢᵒᶜ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) α_soc = fraction_to_the_boundary_rule(s, soc_step.p_s, τ); + α_z_soc = fraction_to_the_boundary_rule(z, soc_step.p_z, τ); + trial_x = x + α_soc * soc_step.p_x; trial_s = s + α_soc * soc_step.p_s; - - // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) - α_z_soc = fraction_to_the_boundary_rule(z, soc_step.p_z, τ); trial_y = y + α_z_soc * soc_step.p_y; trial_z = z + α_z_soc * soc_step.p_z; @@ -589,6 +613,16 @@ ExitStatus interior_point( trial_c_e = matrices.c_e(trial_x); trial_c_i = matrices.c_i(trial_x); + // Check whether filter accepts trial iterate + FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ}; + if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) { + step = soc_step; + α = α_soc; + α_z = α_z_soc; + step_acceptable = true; + break; + } + // Constraint violation scale factor for second-order corrections constexpr Scalar κ_soc(0.99); @@ -597,18 +631,11 @@ ExitStatus interior_point( next_constraint_violation = trial_c_e.template lpNorm<1>() + (trial_c_i - trial_s).template lpNorm<1>(); - if (next_constraint_violation > κ_soc * prev_constraint_violation) { + if (next_constraint_violation > κ_soc * soc_constraint_violation) { break; } - // Check whether filter accepts trial iterate - FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ}; - if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) { - step = soc_step; - α = α_soc; - α_z = α_z_soc; - step_acceptable = true; - } + soc_constraint_violation = next_constraint_violation; } if (step_acceptable) { @@ -648,21 +675,19 @@ ExitStatus interior_point( trial_x = x + α_max * step.p_x; trial_s = s + α_max * step.p_s; - trial_y = y + α_z * step.p_y; trial_z = z + α_z * step.p_z; + trial_f = matrices.f(trial_x); trial_c_e = matrices.c_e(trial_x); trial_c_i = matrices.c_i(trial_x); Scalar next_kkt_error = kkt_error( - matrices.g(trial_x), matrices.A_e(trial_x), matrices.c_e(trial_x), + matrices.g(trial_x), matrices.A_e(trial_x), trial_c_e, matrices.A_i(trial_x), trial_c_i, trial_s, trial_y, trial_z, μ); // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway if (next_kkt_error <= Scalar(0.999) * current_kkt_error) { - α = α_max; - // Accept step break; } @@ -675,6 +700,9 @@ ExitStatus interior_point( line_search_profiler.stop(); if (call_feasibility_restoration) { + ScopedProfiler feasibility_restoration_profiler{ + feasibility_restoration_prof}; + // If already in feasibility restoration mode, running it again won't help if (in_feasibility_restoration) { return ExitStatus::FEASIBILITY_RESTORATION_FAILED; @@ -705,27 +733,32 @@ ExitStatus interior_point( Scalar(0.9) * initial_entry.constraint_violation && filter.try_add(initial_entry, trial_entry, trial_x - x, g, α); }); - auto status = feasibility_restoration(matrices, callbacks, - options, x, s, y, z, μ); + auto status = + feasibility_restoration(matrices, callbacks, options, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + bound_constraint_mask, +#endif + x, s, y, z, μ, iterations); if (status != ExitStatus::SUCCESS) { // Report failure return status; } + + f = matrices.f(x); + c_e = matrices.c_e(x); + c_i = matrices.c_i(x); } else { // If full step was accepted, reset full-step rejected counter if (α == α_max) { full_step_rejected_counter = 0; } - // xₖ₊₁ = xₖ + αₖpₖˣ - // sₖ₊₁ = sₖ + αₖpₖˢ - // yₖ₊₁ = yₖ + αₖᶻpₖʸ - // zₖ₊₁ = zₖ + αₖᶻpₖᶻ - x += α * step.p_x; - s += α * step.p_s; - y += α_z * step.p_y; - z += α_z * step.p_z; + // Update iterates + x = trial_x; + s = trial_s; + y = trial_y; + z = trial_z; // A requirement for the convergence proof is that the primal-dual barrier // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal @@ -745,23 +778,21 @@ ExitStatus interior_point( z[row] = std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]); } + + f = trial_f; + c_e = trial_c_e; + c_i = trial_c_i; } // Update autodiff for Jacobians and Hessian - f = matrices.f(x); A_e = matrices.A_e(x); A_i = matrices.A_i(x); g = matrices.g(x); H = matrices.H(x, y, z); - ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; - - c_e = matrices.c_e(x); - c_i = matrices.c_i(x); - // Update the error - E_0 = kkt_error( - g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0)); + E_0 = unscaled_kkt_error( + matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0)); // Update the barrier parameter if necessary if (E_0 > Scalar(options.tolerance)) { @@ -779,7 +810,6 @@ ExitStatus interior_point( } } - next_iter_prep_profiler.stop(); inner_iter_profiler.stop(); if (options.diagnostics) { @@ -789,8 +819,13 @@ ExitStatus interior_point( : IterationType::NORMAL, inner_iter_profiler.current_duration(), E_0, f, c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>(), s.dot(z), - μ, solver.hessian_regularization(), α, α_max, α_reduction_factor, - α_z); + μ, solver.hessian_regularization(), + solver.constraint_jacobian_regularization(), + std::max(step.p_x.template lpNorm(), + step.p_s.template lpNorm()), + std::max(step.p_y.template lpNorm(), + step.p_z.template lpNorm()), + α, α_max, α_reduction_factor, α_z); } ++iterations; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp index 94895dc848..496f40deba 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp @@ -7,6 +7,8 @@ #include #include +#include "sleipnir/optimization/solver/util/problem_scaling.hpp" + namespace slp { /// Matrix callbacks for the interior-point method solver. @@ -76,6 +78,8 @@ struct InteriorPointMatrixCallbacks { /// /// L(x, y, z) = f(x) − yᵀcₑ(x) − zᵀcᵢ(x) /// + /// Only the lower triangle is used. + /// /// /// /// @@ -109,6 +113,8 @@ struct InteriorPointMatrixCallbacks { /// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x) − zᵀcᵢ(x)) getter. /// + /// Only the lower triangle is used. + /// ///
Variable
/// /// @@ -237,6 +243,10 @@ struct InteriorPointMatrixCallbacks { /// ///
Variable
std::function A_i; + + /// Automatic problem scaling factors. Used to scale the cost, constraints, + /// and tolerance inside the interior-point solver. + ProblemScaling scaling; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp index ebc7d0b62c..beb11b8e0d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include @@ -71,7 +70,6 @@ ExitStatus newton( solve_profilers.emplace_back(" ↳ KKT matrix decomp"); solve_profilers.emplace_back(" ↳ KKT system solve"); solve_profilers.emplace_back(" ↳ line search"); - solve_profilers.emplace_back(" ↳ next iter prep"); solve_profilers.emplace_back(" ↳ f(x)"); solve_profilers.emplace_back(" ↳ ∇f(x)"); solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); @@ -84,13 +82,12 @@ ExitStatus newton( auto& kkt_matrix_decomp_prof = solve_profilers[5]; auto& kkt_system_solve_prof = solve_profilers[6]; auto& line_search_prof = solve_profilers[7]; - auto& next_iter_prep_prof = solve_profilers[8]; // Set up profiled matrix callbacks #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - auto& f_prof = solve_profilers[9]; - auto& g_prof = solve_profilers[10]; - auto& H_prof = solve_profilers[11]; + auto& f_prof = solve_profilers[8]; + auto& g_prof = solve_profilers[9]; + auto& H_prof = solve_profilers[10]; NewtonMatrixCallbacks matrices{ matrix_callbacks.num_decision_variables, @@ -105,7 +102,8 @@ ExitStatus newton( [&](const DenseVector& x) -> SparseMatrix { ScopedProfiler prof{H_prof}; return matrix_callbacks.H(x); - }}; + }, + matrix_callbacks.scaling}; #else const auto& matrices = matrix_callbacks; #endif @@ -122,6 +120,10 @@ ExitStatus newton( slp_assert(H.rows() == matrices.num_decision_variables); slp_assert(H.cols() == matrices.num_decision_variables); + DenseVector trial_x; + + Scalar trial_f; + // Check whether initial guess has finite cost and derivatives if (!isfinite(f) || !all_finite(g) || !all_finite(H)) { return ExitStatus::NONFINITE_INITIAL_GUESS; @@ -131,14 +133,17 @@ ExitStatus newton( Filter filter; - RegularizedLDLT solver{matrices.num_decision_variables, 0}; + RegularizedLDLT solver{ + // Use sparse solver if lower triangle fills < 25% of system + H.nonZeros() < 0.25 * H.size(), matrices.num_decision_variables, 0}; // Variables for determining when a step is acceptable constexpr Scalar α_reduction_factor(0.5); constexpr Scalar α_min(1e-20); // Error - Scalar E_0 = std::numeric_limits::infinity(); + Scalar E_0 = unscaled_kkt_error( + matrices.scaling, g); setup_prof.stop(); @@ -194,9 +199,9 @@ ExitStatus newton( // Loop until a step is accepted. If a step becomes acceptable, the loop // will exit early. while (1) { - DenseVector trial_x = x + α * p_x; + trial_x = x + α * p_x; - Scalar trial_f = matrices.f(trial_x); + trial_f = matrices.f(trial_x); // If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately if (!isfinite(trial_f)) { @@ -223,14 +228,14 @@ ExitStatus newton( if (α < α_min) { Scalar current_kkt_error = kkt_error(g); - DenseVector trial_x = x + α_max * p_x; + trial_x = x + α_max * p_x; Scalar next_kkt_error = kkt_error(matrices.g(trial_x)); // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway if (next_kkt_error <= Scalar(0.999) * current_kkt_error) { - α = α_max; + trial_f = matrices.f(trial_x); // Accept step break; @@ -242,28 +247,29 @@ ExitStatus newton( line_search_profiler.stop(); - // xₖ₊₁ = xₖ + αₖpₖˣ - x += α * p_x; + // Update iterates + x = trial_x; + + f = trial_f; // Update autodiff for Hessian - f = matrices.f(x); g = matrices.g(x); H = matrices.H(x); - ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; - // Update the error - E_0 = kkt_error(g); + E_0 = unscaled_kkt_error( + matrices.scaling, g); - next_iter_prep_profiler.stop(); inner_iter_profiler.stop(); if (options.diagnostics) { - print_iteration_diagnostics(iterations, IterationType::NORMAL, - inner_iter_profiler.current_duration(), E_0, - f, Scalar(0), Scalar(0), Scalar(0), - solver.hessian_regularization(), α, α_max, - α_reduction_factor, Scalar(1)); + print_iteration_diagnostics( + iterations, IterationType::NORMAL, + inner_iter_profiler.current_duration(), E_0, f, Scalar(0), Scalar(0), + Scalar(0), solver.hessian_regularization(), + solver.constraint_jacobian_regularization(), + p_x.template lpNorm(), Scalar(1), α, α_max, + α_reduction_factor, Scalar(1)); } ++iterations; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp index 81bd721f32..3eb8e818bc 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp @@ -7,6 +7,8 @@ #include #include +#include "sleipnir/optimization/solver/util/problem_scaling.hpp" + namespace slp { /// Matrix callbacks for the Newton's method solver. @@ -70,6 +72,8 @@ struct NewtonMatrixCallbacks { /// /// L(x) = f(x) /// + /// Only the lower triangle is used. + /// /// /// /// @@ -88,6 +92,10 @@ struct NewtonMatrixCallbacks { /// ///
Variable
std::function H; + + /// Automatic problem scaling factors. Used to scale the cost and tolerance + /// inside the Newton solver. + ProblemScaling scaling; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp index 3a48e2e59a..72eaaa66c0 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include @@ -121,7 +120,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, solve_profilers.emplace_back(" ↳ KKT system solve"); solve_profilers.emplace_back(" ↳ line search"); solve_profilers.emplace_back(" ↳ SOC"); - solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ feas. restoration"); solve_profilers.emplace_back(" ↳ f(x)"); solve_profilers.emplace_back(" ↳ ∇f(x)"); solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); @@ -139,7 +138,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, auto& kkt_system_solve_prof = solve_profilers[7]; auto& line_search_prof = solve_profilers[8]; auto& soc_prof = solve_profilers[9]; - auto& next_iter_prep_prof = solve_profilers[10]; + auto& feasibility_restoration_prof = solve_profilers[10]; // Set up profiled matrix callbacks #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS @@ -176,7 +175,8 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, [&](const DenseVector& x) -> SparseMatrix { ScopedProfiler prof{A_e_prof}; return matrix_callbacks.A_e(x); - }}; + }, + matrix_callbacks.scaling}; #else const auto& matrices = matrix_callbacks; #endif @@ -198,6 +198,12 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, slp_assert(A_e.rows() == matrices.num_equality_constraints); slp_assert(A_e.cols() == matrices.num_decision_variables); + DenseVector trial_x; + DenseVector trial_y; + + Scalar trial_f; + DenseVector trial_c_e; + // Check for overconstrained problem if (matrices.num_equality_constraints > matrices.num_decision_variables) { if (options.diagnostics) { @@ -220,8 +226,12 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, // Kept outside the loop so its storage can be reused gch::small_vector> triplets; - RegularizedLDLT solver{matrices.num_decision_variables, - matrices.num_equality_constraints}; + const int lhs_rows = + matrices.num_decision_variables + matrices.num_equality_constraints; + RegularizedLDLT solver{ + // Use sparse solver if lower triangle fills < 25% of system + H.nonZeros() + A_e.nonZeros() < 0.25 * lhs_rows * lhs_rows, + matrices.num_decision_variables, matrices.num_equality_constraints}; // Variables for determining when a step is acceptable constexpr Scalar α_reduction_factor(0.5); @@ -230,7 +240,8 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, int full_step_rejected_counter = 0; // Error - Scalar E_0 = std::numeric_limits::infinity(); + Scalar E_0 = unscaled_kkt_error( + matrices.scaling, g, A_e, c_e, y); setup_prof.stop(); @@ -331,11 +342,11 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, // Loop until a step is accepted while (1) { - DenseVector trial_x = x + α * step.p_x; - DenseVector trial_y = y + α * step.p_y; + trial_x = x + α * step.p_x; + trial_y = y + α * step.p_y; - Scalar trial_f = matrices.f(trial_x); - DenseVector trial_c_e = matrices.c_e(trial_x); + trial_f = matrices.f(trial_x); + trial_c_e = matrices.c_e(trial_x); // If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size // immediately @@ -372,6 +383,8 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, Scalar α_soc = α; DenseVector c_e_soc = c_e; + Scalar soc_constraint_violation = next_constraint_violation; + bool step_acceptable = false; for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable; ++soc_iteration) { @@ -380,17 +393,18 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, scope_exit soc_exit{[&] { soc_profiler.stop(); - if (options.diagnostics) { + if (options.diagnostics && step_acceptable) { print_iteration_diagnostics( - iterations, - step_acceptable ? IterationType::ACCEPTED_SOC - : IterationType::REJECTED_SOC, + iterations, IterationType::SECOND_ORDER_CORRECTION, soc_profiler.current_duration(), kkt_error( g, A_e, trial_c_e, trial_y), trial_f, trial_c_e.template lpNorm<1>(), Scalar(0), Scalar(0), - solver.hessian_regularization(), α_soc, Scalar(1), - α_reduction_factor, Scalar(1)); + solver.hessian_regularization(), + solver.constraint_jacobian_regularization(), + soc_step.p_x.template lpNorm(), + soc_step.p_y.template lpNorm(), α_soc, + Scalar(1), α_reduction_factor, Scalar(1)); } }}; @@ -412,23 +426,26 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, trial_f = matrices.f(trial_x); trial_c_e = matrices.c_e(trial_x); + // Check whether the filter accepts trial iterate + FilterEntry trial_entry{trial_f, trial_c_e}; + if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) { + step = soc_step; + α = α_soc; + step_acceptable = true; + break; + } + // Constraint violation scale factor for second-order corrections constexpr Scalar κ_soc(0.99); // If constraint violation hasn't been sufficiently reduced, stop // making second-order corrections next_constraint_violation = trial_c_e.template lpNorm<1>(); - if (next_constraint_violation > κ_soc * prev_constraint_violation) { + if (next_constraint_violation > κ_soc * soc_constraint_violation) { break; } - // Check whether filter accepts trial iterate - FilterEntry trial_entry{trial_f, trial_c_e}; - if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) { - step = soc_step; - α = α_soc; - step_acceptable = true; - } + soc_constraint_violation = next_constraint_violation; } if (step_acceptable) { @@ -469,6 +486,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, trial_x = x + α_max * step.p_x; trial_y = y + α_max * step.p_y; + trial_f = matrices.f(trial_x); trial_c_e = matrices.c_e(trial_x); Scalar next_kkt_error = kkt_error( @@ -476,8 +494,6 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway if (next_kkt_error <= Scalar(0.999) * current_kkt_error) { - α = α_max; - // Accept step break; } @@ -490,6 +506,9 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, line_search_profiler.stop(); if (call_feasibility_restoration) { + ScopedProfiler feasibility_restoration_profiler{ + feasibility_restoration_prof}; + FilterEntry initial_entry{matrices.f(x), c_e}; // Feasibility restoration phase @@ -512,47 +531,50 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, Scalar(0.9) * initial_entry.constraint_violation && filter.try_add(initial_entry, trial_entry, trial_x - x, g, α); }); - auto status = - feasibility_restoration(matrices, callbacks, options, x, y); + auto status = feasibility_restoration(matrices, callbacks, + options, x, y, iterations); if (status != ExitStatus::SUCCESS) { // Report failure return status; } + + f = matrices.f(x); + c_e = matrices.c_e(x); } else { // If full step was accepted, reset full-step rejected counter if (α == α_max) { full_step_rejected_counter = 0; } - // xₖ₊₁ = xₖ + αₖpₖˣ - // yₖ₊₁ = yₖ + αₖpₖʸ - x += α * step.p_x; - y += α * step.p_y; + // Update iterates + x = trial_x; + y = trial_y; + + f = trial_f; + c_e = trial_c_e; } // Update autodiff for Jacobians and Hessian - f = matrices.f(x); A_e = matrices.A_e(x); g = matrices.g(x); H = matrices.H(x, y); - ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; - - c_e = matrices.c_e(x); - // Update the error - E_0 = kkt_error(g, A_e, c_e, y); + E_0 = unscaled_kkt_error( + matrices.scaling, g, A_e, c_e, y); - next_iter_prep_profiler.stop(); inner_iter_profiler.stop(); if (options.diagnostics) { print_iteration_diagnostics(iterations, IterationType::NORMAL, inner_iter_profiler.current_duration(), E_0, f, c_e.template lpNorm<1>(), Scalar(0), - Scalar(0), solver.hessian_regularization(), α, - α_max, α_reduction_factor, α); + Scalar(0), solver.hessian_regularization(), + solver.constraint_jacobian_regularization(), + step.p_x.template lpNorm(), + step.p_y.template lpNorm(), + α, α_max, α_reduction_factor, α); } ++iterations; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp index de2f2ed884..62f96195ef 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp @@ -7,6 +7,8 @@ #include #include +#include "sleipnir/optimization/solver/util/problem_scaling.hpp" + namespace slp { /// Matrix callbacks for the Sequential Quadratic Programming (SQP) solver. @@ -73,6 +75,8 @@ struct SQPMatrixCallbacks { /// /// L(x, y) = f(x) − yᵀcₑ(x) /// + /// Only the lower triangle is used. + /// /// /// /// @@ -99,6 +103,8 @@ struct SQPMatrixCallbacks { /// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x)) getter. /// + /// Only the lower triangle is used. + /// ///
Variable
/// /// @@ -171,6 +177,10 @@ struct SQPMatrixCallbacks { /// ///
Variable
std::function A_e; + + /// Automatic problem scaling factors. Used to scale the cost, constraints, + /// and tolerance inside the SQP solver. + ProblemScaling scaling; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp index 57b47fcd99..15b2684968 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp @@ -6,6 +6,8 @@ #include +namespace slp { + /// Returns true if elements of sparse matrix are all finite. /// /// @param mat Sparse matrix. @@ -24,3 +26,5 @@ bool all_finite(const Eigen::SparseCompressedBase& mat) { return true; } + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp index 7e96216277..a0b04b93cd 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp @@ -7,6 +7,8 @@ #include #include +namespace slp { + /// Appends sparse matrices to list of triplets at the given offset. /// /// The triplets are appended in column-major order (e.g., first column of mat1, @@ -58,3 +60,5 @@ void append_diagonal_as_triplets( triplets.emplace_back(row_offset + row, col_offset + row, diag[row]); } } + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp new file mode 100644 index 0000000000..dea28cd6e0 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp @@ -0,0 +1,210 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include + +#include "sleipnir/optimization/solver/util/inertia.hpp" + +namespace slp { + +/// Solves dense systems of linear equations using a regularized LDLT +/// factorization. +/// +/// @tparam Scalar Scalar type. +template +class DenseRegularizedLDLT { + public: + /// Type alias for dense matrix. + using DenseMatrix = Eigen::Matrix; + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + + /// Constructs a DenseRegularizedLDLT instance. + /// + /// @param num_decision_variables The number of decision variables in the + /// system. + /// @param num_equality_constraints The number of equality constraints in the + /// system. + DenseRegularizedLDLT(int num_decision_variables, int num_equality_constraints) + : m_num_decision_variables{num_decision_variables}, + m_num_equality_constraints{num_equality_constraints} {} + + /// Constructs a DenseRegularizedLDLT instance. + /// + /// @param num_decision_variables The number of decision variables in the + /// system. + /// @param num_equality_constraints The number of equality constraints in the + /// system. + /// @param γ_min The minimum constraint regularization. + DenseRegularizedLDLT(int num_decision_variables, int num_equality_constraints, + Scalar γ_min) + : m_num_decision_variables{num_decision_variables}, + m_num_equality_constraints{num_equality_constraints}, + m_γ_min{γ_min} {} + + /// Reports whether previous computation was successful. + /// + /// @return Whether previous computation was successful. + Eigen::ComputationInfo info() const { return m_info; } + + /// Computes the regularized LDLT factorization of a matrix. + /// + /// @param lhs Left-hand side of the system. + /// @return The factorization. + DenseRegularizedLDLT& compute(const DenseMatrix& lhs) { + m_info = m_solver.compute(lhs).info(); + + if (m_info == Eigen::Success) { + auto D = m_solver.vectorD(); + + // If the inertia is ideal and D from LDLT is sufficiently far from zero, + // don't regularize the system + if (Inertia{D} == ideal_inertia && + (D.cwiseAbs().array() >= Scalar(1e-4)).all()) { + m_prev_δ = Scalar(0); + m_prev_γ = Scalar(0); + return *this; + } + } + + // We'll give lhs the correct inertia by adding [δI, 0; 0, −γI] where δ and + // γ regularize the Hessian and equality constraint Jacobian respectively. + + // If the previous δ was zero, attempt a small value. Otherwise, attempt a + // smaller value than the previous δ so δ trends downward. + Scalar δ = m_prev_δ == Scalar(0) + ? Scalar(1e-4) + : std::max(m_prev_δ / Scalar(2), + std::numeric_limits::epsilon()); + + // Start γ at the minimum to minimize equality constraint Jacobian + // distortion. + Scalar γ = m_γ_min; + + while (true) { + m_info = m_solver.compute(lhs + regularization(δ, γ)).info(); + + if (m_info == Eigen::Success) { + Inertia inertia{m_solver.vectorD()}; + + if (inertia == ideal_inertia) { + // If the inertia is ideal, report success + m_prev_δ = δ; + m_prev_γ = γ; + return *this; + } else if (inertia.zero > 0) { + if (γ == Scalar(0)) { + // If there's zero eigenvalues and γ = 0, increase γ to potentially + // compensate for a rank-deficient equality constraint Jacobian + γ = Scalar(1e-10); + } else { + // If there's zero eigenvalues and γ > 0, increase δ and γ to drive + // all eigenvalues away from zero + δ *= Scalar(10); + γ *= Scalar(10); + } + } else if (inertia.negative > ideal_inertia.negative) { + // If there's too many negative eigenvalues, increase δ to add more + // positive eigenvalues + δ *= Scalar(10); + } else if (inertia.positive > ideal_inertia.positive) { + // If there's too many positive eigenvalues, increase γ to add more + // negative eigenvalues + γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10); + } + } else { + // If the decomposition failed, increase δ and γ to drive all + // eigenvalues away from zero + δ *= Scalar(10); + γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10); + } + + // If the lhs perturbation is too high, report failure. This can be caused + // by ill-conditioning. + if (δ > Scalar(1e20) || γ > Scalar(1e20)) { + m_info = Eigen::NumericalIssue; + m_prev_δ = δ; + m_prev_γ = γ; + return *this; + } + } + } + + /// Solves the system of equations using a regularized LDLT factorization. + /// + /// @param rhs Right-hand side of the system. + /// @return The solution. + template + DenseVector solve(const Eigen::MatrixBase& rhs) const { + return m_solver.solve(rhs); + } + + /// Solves the system of equations using a regularized LDLT factorization. + /// + /// @param rhs Right-hand side of the system. + /// @return The solution. + template + DenseVector solve(const Eigen::SparseMatrixBase& rhs) const { + return m_solver.solve(rhs.toDense()); + } + + /// Returns the Hessian regularization factor. + /// + /// @return Hessian regularization factor. + Scalar hessian_regularization() const { return m_prev_δ; } + + /// Returns the constraint Jacobian regularization factor. + /// + /// @return Constraint Jacobian regularization factor. + Scalar constraint_jacobian_regularization() const { return m_prev_γ; } + + private: + using Solver = Eigen::LDLT; + + Solver m_solver; + + Eigen::ComputationInfo m_info = Eigen::Success; + + /// The number of decision variables in the system. + int m_num_decision_variables = 0; + + /// The number of equality constraints in the system. + int m_num_equality_constraints = 0; + + /// The minimum constraint regularization. + Scalar m_γ_min{1e-10}; + + /// The ideal system inertia. + Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints, + 0}; + + /// The value of δ from the previous run of compute(). + Scalar m_prev_δ{0}; + + /// The value of γ from the previous run of compute(). + Scalar m_prev_γ{0}; + + /// Returns regularization matrix. + /// + /// [δI 0] + /// [ 0 −γI] + /// + /// @param δ The Hessian regularization factor. + /// @param γ The equality constraint Jacobian regularization factor. + /// @return Regularization matrix. + DenseMatrix regularization(Scalar δ, Scalar γ) const { + DenseVector vec{m_num_decision_variables + m_num_equality_constraints}; + vec.segment(0, m_num_decision_variables).setConstant(δ); + vec.segment(m_num_decision_variables, m_num_equality_constraints) + .setConstant(-γ); + + return vec.asDiagonal().toDenseMatrix(); + } +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp index 1c042f8599..ed128e5a58 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp @@ -33,7 +33,7 @@ ExitStatus interior_point( Eigen::Vector& x, Eigen::Vector& s, Eigen::Vector& y, - Eigen::Vector& z, Scalar& μ); + Eigen::Vector& z, Scalar& μ, int& iterations); /// Computes initial values for p and n in feasibility restoration. /// @@ -111,6 +111,7 @@ compute_p_n(const Eigen::Vector& c, Scalar ρ, /// @param[in,out] x The decision variables from the normal solve. /// @param[in,out] y The equality constraint dual variables from the normal /// solve. +/// @param[in,out] iterations The iteration counter. /// @return The exit status. template ExitStatus feasibility_restoration( @@ -118,7 +119,7 @@ ExitStatus feasibility_restoration( std::span& info)>> iteration_callbacks, const Options& options, Eigen::Vector& x, - Eigen::Vector& y) { + Eigen::Vector& y, int& iterations) { // Feasibility restoration // // min ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ) @@ -148,12 +149,14 @@ ExitStatus feasibility_restoration( constexpr Scalar ρ(1e3); const Scalar μ(options.tolerance / 10.0); - const Scalar ζ = sqrt(μ); const DenseVector c_e = matrices.c_e(x); + Scalar fr_μ = std::max(μ, c_e.template lpNorm()); + const Scalar ζ = sqrt(fr_μ); + const auto& x_r = x; - const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ); + const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, fr_μ); // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows()) const DiagonalMatrix D_r = @@ -166,10 +169,15 @@ ExitStatus feasibility_restoration( DenseVector fr_y = DenseVector::Zero(num_eq); + // Force the duals to start with perfect complementarity with the slacks DenseVector fr_z{2 * num_eq}; - fr_z << μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse(); + fr_z << fr_μ * p_e_0.cwiseInverse(), fr_μ * n_e_0.cwiseInverse(); - Scalar fr_μ = std::max(μ, c_e.template lpNorm()); + // Inherit the parent problem's scaling for the constraints, and use no + // scaling for the cost function since it has changed. The new rows introduced + // are not scaled. + const ProblemScaling fr_scaling{Scalar(1), matrices.scaling.c_e, + DenseVector::Ones(2 * num_eq)}; InteriorPointMatrixCallbacks fr_matrix_callbacks{ static_cast(fr_x.rows()), @@ -289,14 +297,15 @@ ExitStatus feasibility_restoration( SparseMatrix A_i_p{2 * num_eq, x_p.rows()}; A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end()); return A_i_p; - }}; + }, + fr_scaling}; - auto status = interior_point(fr_matrix_callbacks, iteration_callbacks, - options, true, + auto status = interior_point( + fr_matrix_callbacks, iteration_callbacks, options, true, #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION - {}, + Eigen::ArrayX::Constant(2 * num_eq, true), #endif - fr_x, fr_s, fr_y, fr_z, fr_μ); + fr_x, fr_s, fr_y, fr_z, fr_μ, iterations); x = fr_x.segment(0, x.rows()); @@ -331,16 +340,21 @@ ExitStatus feasibility_restoration( /// @param[in,out] z The current inequality constraint duals from the normal /// solve. /// @param[in] μ Barrier parameter. +/// @param[in,out] iterations The iteration counter. /// @return The exit status. template ExitStatus feasibility_restoration( const InteriorPointMatrixCallbacks& matrix_callbacks, std::span& info)>> iteration_callbacks, - const Options& options, Eigen::Vector& x, + const Options& options, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + const Eigen::ArrayX& bound_constraint_mask, +#endif + Eigen::Vector& x, Eigen::Vector& s, Eigen::Vector& y, - Eigen::Vector& z, Scalar μ) { + Eigen::Vector& z, Scalar μ, int& iterations) { // Feasibility restoration // // min ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ) @@ -374,14 +388,17 @@ ExitStatus feasibility_restoration( const auto& num_ineq = matrices.num_inequality_constraints; constexpr Scalar ρ(1e3); - const Scalar ζ = sqrt(μ); const DenseVector c_e = matrices.c_e(x); const DenseVector c_i = matrices.c_i(x); + Scalar fr_μ = std::max({μ, c_e.template lpNorm(), + (c_i - s).template lpNorm()}); + const Scalar ζ = sqrt(fr_μ); + const auto& x_r = x; - const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ); - const auto [p_i_0, n_i_0] = compute_p_n((c_i - s).eval(), ρ, μ); + const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, fr_μ); + const auto [p_i_0, n_i_0] = compute_p_n((c_i - s).eval(), ρ, fr_μ); // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows()) const DiagonalMatrix D_r = @@ -396,12 +413,20 @@ ExitStatus feasibility_restoration( DenseVector fr_y = DenseVector::Zero(c_e.rows()); + // Force the duals to start with perfect complementarity with the slacks DenseVector fr_z{c_i.rows() + 2 * num_eq + 2 * num_ineq}; - fr_z << z.cwiseMin(ρ), μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse(), - μ * p_i_0.cwiseInverse(), μ * n_i_0.cwiseInverse(); + fr_z << fr_μ * s.cwiseInverse(), fr_μ * p_e_0.cwiseInverse(), + fr_μ * n_e_0.cwiseInverse(), fr_μ * p_i_0.cwiseInverse(), + fr_μ * n_i_0.cwiseInverse(); - Scalar fr_μ = std::max({μ, c_e.template lpNorm(), - (c_i - s).template lpNorm()}); + // Inherit the parent problem's scaling for the constraints, and use no + // scaling for the cost function since it has changed. The new rows introduced + // are not scaled. + DenseVector fr_d_c_i{c_i.rows() + 2 * num_eq + 2 * num_ineq}; + fr_d_c_i << matrices.scaling.c_i, + DenseVector::Ones(2 * num_eq + 2 * num_ineq); + const ProblemScaling fr_scaling{Scalar(1), matrices.scaling.c_e, + fr_d_c_i}; InteriorPointMatrixCallbacks fr_matrix_callbacks{ static_cast(fr_x.rows()), @@ -564,14 +589,21 @@ ExitStatus feasibility_restoration( SparseMatrix A_i_p{2 * num_eq + 3 * num_ineq, x_p.rows()}; A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end()); return A_i_p; - }}; + }, + fr_scaling}; - auto status = interior_point(fr_matrix_callbacks, iteration_callbacks, - options, true, #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION - {}, + Eigen::ArrayX fr_bound_constraint_mask{2 * num_eq + 3 * num_ineq}; + fr_bound_constraint_mask.segment(0, num_ineq) = bound_constraint_mask; + fr_bound_constraint_mask.segment(num_ineq, 2 * num_eq + 2 * num_ineq) = true; #endif - fr_x, fr_s, fr_y, fr_z, fr_μ); + + auto status = interior_point( + fr_matrix_callbacks, iteration_callbacks, options, true, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + fr_bound_constraint_mask, +#endif + fr_x, fr_s, fr_y, fr_z, fr_μ, iterations); x = fr_x.segment(0, x.rows()); s = fr_s.segment(0, s.rows()); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp index 8d98cbee4c..c2f10138df 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp @@ -4,7 +4,6 @@ #include #include -#include #include #include diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp index bd1a1a43d5..2d2ce114be 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp @@ -2,7 +2,10 @@ #pragma once +#include + #include +#include namespace slp { @@ -36,9 +39,9 @@ class Inertia { template explicit Inertia(const Eigen::Vector& D) { for (const auto& elem : D) { - if (elem > Scalar(0)) { + if (elem > std::numeric_limits::epsilon()) { ++positive; - } else if (elem < Scalar(0)) { + } else if (elem < -std::numeric_limits::epsilon()) { ++negative; } else { ++zero; @@ -56,9 +59,9 @@ class Inertia { const Eigen::Diagonal< const Eigen::Matrix>& D) { for (const auto& elem : D) { - if (elem > Scalar(0)) { + if (elem > std::numeric_limits::epsilon()) { ++positive; - } else if (elem < Scalar(0)) { + } else if (elem < -std::numeric_limits::epsilon()) { ++negative; } else { ++zero; @@ -73,3 +76,41 @@ class Inertia { }; } // namespace slp + +// @cond Suppress Doxygen + +/// Formatter for Inertia. +template <> +struct fmt::formatter { + /// Parses format string. + /// + /// @param ctx Format parse context. + /// @return Format parse context iterator. + constexpr auto parse(fmt::format_parse_context& ctx) { + return m_underlying.parse(ctx); + } + + /// Formats Inertia. + /// + /// @tparam FmtContext Format context type. + /// @param inertia Inertia. + /// @param ctx Format context. + /// @return Format context iterator. + template + constexpr auto format(const slp::Inertia& inertia, FmtContext& ctx) const { + auto out = ctx.out(); + + out = fmt::format_to(out, "("); + out = m_underlying.format(inertia.positive, ctx); + out = fmt::format_to(out, ", "); + out = m_underlying.format(inertia.negative, ctx); + out = fmt::format_to(out, ", "); + out = m_underlying.format(inertia.zero, ctx); + return fmt::format_to(out, ")"); + } + + private: + fmt::formatter m_underlying; +}; + +// @endcond diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp index c2a9748ad4..4b5b2cca57 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp @@ -12,10 +12,8 @@ namespace slp { /// Returns true if the problem's equality constraints are locally infeasible. /// /// @tparam Scalar Scalar type. -/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the -/// current iterate. -/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current -/// iterate. +/// @param A_e Equality constraint Jacobian Aₑ(x). +/// @param c_e Equality constraints cₑ(x). template bool is_equality_locally_infeasible( const Eigen::SparseMatrix& A_e, @@ -33,10 +31,8 @@ bool is_equality_locally_infeasible( /// Returns true if the problem's inequality constraints are locally infeasible. /// /// @tparam Scalar Scalar type. -/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at -/// the current iterate. -/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the -/// current iterate. +/// @param A_i Inequality constraint Jacobian Aᵢ(x). +/// @param c_i Inequality constraints cᵢ(x). template bool is_inequality_locally_infeasible( const Eigen::SparseMatrix& A_i, diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp index 8f429be5f3..1e391ad97e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp @@ -7,6 +7,8 @@ #include #include +#include "sleipnir/optimization/solver/util/problem_scaling.hpp" + // See docs/algorithms.md#Works_cited for citation definitions namespace slp { @@ -22,8 +24,8 @@ enum class KKTErrorType { /// Returns the KKT error for Newton's method. /// /// @tparam Scalar Scalar type. -/// @tparam T The type of KKT error to compute. -/// @param g Gradient of the cost function ∇f. +/// @tparam T Type of KKT error to compute. +/// @param g Cost function gradient ∇f. template Scalar kkt_error(const Eigen::Vector& g) { // The KKT conditions from docs/algorithms.md: @@ -40,12 +42,10 @@ Scalar kkt_error(const Eigen::Vector& g) { /// Returns the KKT error for Sequential Quadratic Programming. /// /// @tparam Scalar Scalar type. -/// @tparam T The type of KKT error to compute. -/// @param g Gradient of the cost function ∇f. -/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the -/// current iterate. -/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current -/// iterate. +/// @tparam T Type of KKT error to compute. +/// @param g Cost function gradient ∇f. +/// @param A_e Equality constraint Jacobian Aₑ(x). +/// @param c_e Equality constraints cₑ(x). /// @param y Equality constraint dual variables. template Scalar kkt_error(const Eigen::Vector& g, @@ -79,16 +79,12 @@ Scalar kkt_error(const Eigen::Vector& g, /// Returns the KKT error for the interior-point method. /// /// @tparam Scalar Scalar type. -/// @tparam T The type of KKT error to compute. -/// @param g Gradient of the cost function ∇f. -/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the -/// current iterate. -/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current -/// iterate. -/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at -/// the current iterate. -/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the -/// current iterate. +/// @tparam T Type of KKT error to compute. +/// @param g Cost function gradient ∇f. +/// @param A_e Equality constraint Jacobian Aₑ(x). +/// @param c_e Equality constraints cₑ(x). +/// @param A_i Inequality constraint Jacobian Aᵢ(x). +/// @param c_i Inequality constraints cᵢ(x). /// @param s Inequality constraint slack variables. /// @param y Equality constraint dual variables. /// @param z Inequality constraint dual variables. @@ -149,4 +145,109 @@ Scalar kkt_error(const Eigen::Vector& g, } } +/// Returns the unscaled KKT error for Newton's method. +/// +/// @tparam Scalar Scalar type. +/// @tparam T Type of KKT error to compute. +/// @param scaling Problem scaling. +/// @param g Scaled cost function gradient d_f·∇f. +template +Scalar unscaled_kkt_error(const ProblemScaling& scaling, + const Eigen::Vector& g) { + using DenseVector = Eigen::Vector; + + if (scaling.is_identity()) { + return kkt_error(g); + } + + const DenseVector g_unscaled = (Scalar(1) / scaling.f) * g; + + return kkt_error(g_unscaled); +} + +/// Returns the unscaled KKT error for Sequential Quadratic Programming. +/// +/// @tparam Scalar Scalar type. +/// @tparam T Type of KKT error to compute. +/// @param scaling Problem scaling. +/// @param g Scaled cost function gradient d_f·∇f. +/// @param A_e Scaled equality constraint Jacobian D_cₑ·Aₑ(x). +/// @param c_e Scaled equality constraints D_cₑ·cₑ(x). +/// @param y Scaled equality constraint dual variables. +template +Scalar unscaled_kkt_error(const ProblemScaling& scaling, + const Eigen::Vector& g, + const Eigen::SparseMatrix& A_e, + const Eigen::Vector& c_e, + const Eigen::Vector& y) { + using DenseVector = Eigen::Vector; + using SparseMatrix = Eigen::SparseMatrix; + + if (scaling.is_identity()) { + return kkt_error(g, A_e, c_e, y); + } + + const Scalar inv_d_f = Scalar(1) / scaling.f; + const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse(); + + const DenseVector g_unscaled = inv_d_f * g; + const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e; + const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e); + const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f; + + return kkt_error(g_unscaled, A_e_unscaled, c_e_unscaled, + y_unscaled); +} + +/// Returns the unscaled KKT error for the interior-point method. +/// +/// @tparam Scalar Scalar type. +/// @tparam T Type of KKT error to compute. +/// @param scaling Problem scaling. +/// @param g Scaled cost function gradient d_f·∇f. +/// @param A_e Scaled equality constraint Jacobian D_cₑ·Aₑ(x). +/// @param c_e Scaled equality constraints D_cₑ·cₑ(x). +/// @param A_i Scaled inequality constraint Jacobian D_cᵢ·Aᵢ(x). +/// @param c_i Scaled inequality constraints D_cᵢ·cᵢ(x). +/// @param s Scaled inequality constraint slack variables. +/// @param y Scaled equality constraint dual variables. +/// @param z Scaled inequality constraint dual variables. +/// @param μ Scaled barrier parameter. +template +Scalar unscaled_kkt_error(const ProblemScaling& scaling, + const Eigen::Vector& g, + const Eigen::SparseMatrix& A_e, + const Eigen::Vector& c_e, + const Eigen::SparseMatrix& A_i, + const Eigen::Vector& c_i, + const Eigen::Vector& s, + const Eigen::Vector& y, + const Eigen::Vector& z, + Scalar μ) { + using DenseVector = Eigen::Vector; + using SparseMatrix = Eigen::SparseMatrix; + + if (scaling.is_identity()) { + return kkt_error(g, A_e, c_e, A_i, c_i, s, y, z, μ); + } + + const Scalar inv_d_f = Scalar(1) / scaling.f; + const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse(); + const DenseVector inv_d_c_i = scaling.c_i.cwiseInverse(); + + const DenseVector g_unscaled = inv_d_f * g; + const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e; + const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e); + const SparseMatrix A_i_unscaled = inv_d_c_i.asDiagonal() * A_i; + const DenseVector c_i_unscaled = inv_d_c_i.cwiseProduct(c_i); + const DenseVector s_unscaled = inv_d_c_i.cwiseProduct(s); + const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f; + const DenseVector z_unscaled = scaling.c_i.cwiseProduct(z) * inv_d_f; + const Scalar μ_unscaled = inv_d_f * μ; + + return kkt_error(g_unscaled, A_e_unscaled, c_e_unscaled, + A_i_unscaled, c_i_unscaled, s_unscaled, + y_unscaled, z_unscaled, μ_unscaled); +} + } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp index 55ad3d28b8..16fa1b7e93 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp @@ -27,9 +27,8 @@ struct LagrangeMultiplierEstimate { /// Estimates Lagrange multipliers for SQP. /// /// @tparam Scalar Scalar type. -/// @param g Gradient of the cost function ∇f. -/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the -/// current iterate. +/// @param g Cost function gradient ∇f. +/// @param A_e Equality constraint Jacobian Aₑ(x). template Eigen::Vector lagrange_multiplier_estimate( const Eigen::SparseVector& g, @@ -47,11 +46,9 @@ Eigen::Vector lagrange_multiplier_estimate( /// Estimates Lagrange multipliers for interior-point method. /// /// @tparam Scalar Scalar type. -/// @param g Gradient of the cost function ∇f. -/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the -/// current iterate. -/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at -/// the current iterate. +/// @param g Cost function gradient ∇f. +/// @param A_e Equality constraint Jacobian Aₑ(x). +/// @param A_i Inequality constraint Jacobian Aᵢ(x). /// @param s Inequality constraint slack variables. /// @param μ Barrier parameter. template diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/problem_scaling.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/problem_scaling.hpp new file mode 100644 index 0000000000..83e3a6d4fc --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/problem_scaling.hpp @@ -0,0 +1,116 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include + +#include "sleipnir/optimization/solver/util/sparse_inf_norms.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions + +namespace slp { + +/// Automatic problem scaling factors. +/// +/// @tparam Scalar Scalar type. +template +struct ProblemScaling { + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + /// Type alias for sparse matrix. + using SparseMatrix = Eigen::SparseMatrix; + /// Type alias for sparse vector. + using SparseVector = Eigen::SparseVector; + + /// Cost scaling factor d_f. + Scalar f = Scalar(1); + + /// Equality constraint scaling factors d_cₑ. + DenseVector c_e; + + /// Inequality constraint scaling factors d_cᵢ. + DenseVector c_i; + + /// Constructs identity problem scaling. + ProblemScaling() = default; + + /// Constructs problem scaling from explicit factors. + /// + /// @param f Cost scaling factor d_f. + /// @param c_e Equality constraint scaling factors d_cₑ. + /// @param c_i Inequality constraint scaling factors d_cᵢ. + ProblemScaling(Scalar f, const DenseVector& c_e, const DenseVector& c_i) + : f{f}, c_e{c_e}, c_i{c_i} {} + + /// Computes Newton problem scaling. + /// + /// Scales the cost so the largest gradient component at the starting point + /// is at most gₘₐₓ: + /// + /// d_f = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞) + /// + /// See §3.8 Automatic Scaling of the Problem Statement in [2]. + /// + /// @param g Cost gradient ∇f, evaluated at the starting point. + explicit ProblemScaling(const DenseVector& g) { + constexpr Scalar g_max(100); + + f = std::min(Scalar(1), g_max / g.template lpNorm()); + } + + /// Computes SQP problem scaling. + /// + /// Scales the cost and each equality constraint so the largest gradient + /// component at the starting point is at most gₘₐₓ: + /// + /// d_f = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞) + /// d_cₑ[j] = min(1, gₘₐₓ / ‖∇cₑⱼ(x₀)‖_∞) + /// + /// See §3.8 Automatic Scaling of the Problem Statement in [2]. + /// + /// @param g Cost gradient ∇f, evaluated at the starting point. + /// @param A_e Equality constraint Jacobian Aₑ, evaluated at the starting + /// point. + ProblemScaling(const DenseVector& g, const SparseMatrix& A_e) { + constexpr Scalar g_max(100); + + f = std::min(Scalar(1), g_max / g.template lpNorm()); + c_e = (g_max / sparse_inf_norms(A_e).array()).min(Scalar(1)).matrix(); + } + + /// Computes interior-point problem scaling. + /// + /// Scales the cost and each constraint so the largest gradient + /// component at the starting point is at most gₘₐₓ: + /// + /// d_f = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞) + /// d_c[j] = min(1, gₘₐₓ / ‖∇cⱼ(x₀)‖_∞) + /// + /// See §3.8 Automatic Scaling of the Problem Statement in [2]. + /// + /// @param g Cost gradient ∇f, evaluated at the starting point. + /// @param A_e Equality constraint Jacobian Aₑ, evaluated at the starting + /// point. + /// @param A_i Inequality constraint Jacobian Aᵢ, evaluated at the starting + /// point. + ProblemScaling(const DenseVector& g, const SparseMatrix& A_e, + const SparseMatrix& A_i) { + constexpr Scalar g_max(100); + + f = std::min(Scalar(1), g_max / g.template lpNorm()); + c_e = (g_max / sparse_inf_norms(A_e).array()).min(Scalar(1)).matrix(); + c_i = (g_max / sparse_inf_norms(A_i).array()).min(Scalar(1)).matrix(); + } + + /// Whether the problem scaling is identity. + /// + /// @return True if the problem scaling is identity. + bool is_identity() const { + return f == Scalar(1) && c_e.size() == 0 && c_i.size() == 0; + } +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp index 4f9e7a7180..619d17998a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp @@ -2,14 +2,11 @@ #pragma once -#include #include -#include #include -#include "sleipnir/optimization/solver/util/inertia.hpp" - -// See docs/algorithms.md#Works_cited for citation definitions +#include "sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp" +#include "sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp" namespace slp { @@ -19,8 +16,6 @@ namespace slp { template class RegularizedLDLT { public: - /// Type alias for dense matrix. - using DenseMatrix = Eigen::Matrix; /// Type alias for dense vector. using DenseVector = Eigen::Vector; /// Type alias for sparse matrix. @@ -28,118 +23,60 @@ class RegularizedLDLT { /// Constructs a RegularizedLDLT instance. /// + /// @param use_sparse_solver Whether to use sparse or dense solver. /// @param num_decision_variables The number of decision variables in the /// system. /// @param num_equality_constraints The number of equality constraints in the /// system. - RegularizedLDLT(int num_decision_variables, int num_equality_constraints) - : m_num_decision_variables{num_decision_variables}, - m_num_equality_constraints{num_equality_constraints} {} + RegularizedLDLT(bool use_sparse_solver, int num_decision_variables, + int num_equality_constraints) + : m_use_sparse_solver{use_sparse_solver}, + m_sparse_solver{num_decision_variables, num_equality_constraints}, + m_dense_solver{num_decision_variables, num_equality_constraints} {} /// Constructs a RegularizedLDLT instance. /// + /// @param use_sparse_solver Whether to use sparse or dense solver. /// @param num_decision_variables The number of decision variables in the /// system. /// @param num_equality_constraints The number of equality constraints in the /// system. /// @param γ_min The minimum constraint regularization. - RegularizedLDLT(int num_decision_variables, int num_equality_constraints, - Scalar γ_min) - : m_num_decision_variables{num_decision_variables}, - m_num_equality_constraints{num_equality_constraints}, - m_γ_min{γ_min} {} + RegularizedLDLT(bool use_sparse_solver, int num_decision_variables, + int num_equality_constraints, Scalar γ_min) + : m_use_sparse_solver{use_sparse_solver}, + m_sparse_solver{num_decision_variables, num_equality_constraints, + γ_min}, + m_dense_solver{num_decision_variables, num_equality_constraints, + γ_min} {} /// Reports whether previous computation was successful. /// /// @return Whether previous computation was successful. - Eigen::ComputationInfo info() const { return m_info; } + Eigen::ComputationInfo info() const { + if (m_use_sparse_solver) { + return m_sparse_solver.info(); + } else { + return m_dense_solver.info(); + } + } /// Computes the regularized LDLT factorization of a matrix. /// + /// In sparse mode, the matrix's symbolic decomposition is reused in + /// subsequent calls, so subsequent calls must be given a matrix with the same + /// sparsity pattern. + /// /// @param lhs Left-hand side of the system. /// @return The factorization. RegularizedLDLT& compute(const SparseMatrix& lhs) { - // The regularization procedure is based on algorithm B.1 of [1] - - // Max density is 50% due to the caller only providing the lower triangle. - // We consider less than 25% to be sparse. - m_is_sparse = lhs.nonZeros() < 0.25 * lhs.size(); - - m_info = m_is_sparse ? compute_sparse(lhs).info() - : m_dense_solver.compute(lhs).info(); - - if (m_info == Eigen::Success) { - auto D = - m_is_sparse ? m_sparse_solver.vectorD() : m_dense_solver.vectorD(); - - // If the inertia is ideal and D from LDLT is sufficiently far from zero, - // don't regularize the system - if (Inertia{D} == ideal_inertia && - (D.cwiseAbs().array() >= Scalar(1e-4)).all()) { - m_prev_δ = Scalar(0); - return *this; - } + if (m_use_sparse_solver) { + m_sparse_solver.compute(lhs); + } else { + m_dense_solver.compute(lhs); } - // Also regularize the Hessian. If the Hessian wasn't regularized in a - // previous run of compute(), start at small values of δ and γ. Otherwise, - // attempt a δ and γ half as big as the previous run so δ and γ can trend - // downwards over time. - Scalar δ = m_prev_δ == Scalar(0) ? Scalar(1e-4) : m_prev_δ / Scalar(2); - Scalar γ = m_γ_min; - - while (true) { - Inertia inertia; - - // Regularize lhs by adding a multiple of the identity matrix - // - // lhs = [H + AᵢᵀΣAᵢ + δI Aₑᵀ] - // [ Aₑ −γI] - if (m_is_sparse) { - m_info = compute_sparse(lhs + regularization(δ, γ)).info(); - if (m_info == Eigen::Success) { - inertia = Inertia{m_sparse_solver.vectorD()}; - } - } else { - m_info = m_dense_solver.compute(lhs + regularization(δ, γ)).info(); - if (m_info == Eigen::Success) { - inertia = Inertia{m_dense_solver.vectorD()}; - } - } - - if (m_info == Eigen::Success) { - if (inertia == ideal_inertia) { - // If the inertia is ideal, store δ and return - m_prev_δ = δ; - return *this; - } else if (inertia.zero > 0) { - // If there's zero eigenvalues, increase δ and γ by an order of - // magnitude and try again - δ *= Scalar(10); - γ *= Scalar(10); - } else if (inertia.negative > ideal_inertia.negative) { - // If there's too many negative eigenvalues, increase δ by an order of - // magnitude and try again - δ *= Scalar(10); - } else if (inertia.positive > ideal_inertia.positive) { - // If there's too many positive eigenvalues, increase γ by an order of - // magnitude and try again - γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10); - } - } else { - // If the decomposition failed, increase δ and γ by an order of - // magnitude and try again - δ *= Scalar(10); - γ *= Scalar(10); - } - - // If the Hessian perturbation is too high, report failure. This can be - // caused by ill-conditioning. - if (δ > Scalar(1e20) || γ > Scalar(1e20)) { - m_info = Eigen::NumericalIssue; - return *this; - } - } + return *this; } /// Solves the system of equations using a regularized LDLT factorization. @@ -148,7 +85,7 @@ class RegularizedLDLT { /// @return The solution. template DenseVector solve(const Eigen::MatrixBase& rhs) const { - if (m_is_sparse) { + if (m_use_sparse_solver) { return m_sparse_solver.solve(rhs); } else { return m_dense_solver.solve(rhs); @@ -161,77 +98,39 @@ class RegularizedLDLT { /// @return The solution. template DenseVector solve(const Eigen::SparseMatrixBase& rhs) const { - if (m_is_sparse) { + if (m_use_sparse_solver) { return m_sparse_solver.solve(rhs); } else { - return m_dense_solver.solve(rhs.toDense()); + return m_dense_solver.solve(rhs); } } /// Returns the Hessian regularization factor. /// /// @return Hessian regularization factor. - Scalar hessian_regularization() const { return m_prev_δ; } + Scalar hessian_regularization() const { + if (m_use_sparse_solver) { + return m_sparse_solver.hessian_regularization(); + } else { + return m_dense_solver.hessian_regularization(); + } + } + + /// Returns the constraint Jacobian regularization factor. + /// + /// @return Constraint Jacobian regularization factor. + Scalar constraint_jacobian_regularization() const { + if (m_use_sparse_solver) { + return m_sparse_solver.constraint_jacobian_regularization(); + } else { + return m_dense_solver.constraint_jacobian_regularization(); + } + } private: - using SparseSolver = Eigen::SimplicialLDLT; - using DenseSolver = Eigen::LDLT; - - SparseSolver m_sparse_solver; - DenseSolver m_dense_solver; - bool m_is_sparse = true; - - Eigen::ComputationInfo m_info = Eigen::Success; - - /// The number of decision variables in the system. - int m_num_decision_variables = 0; - - /// The number of equality constraints in the system. - int m_num_equality_constraints = 0; - - /// The minimum constraint regularization. - Scalar m_γ_min{1e-10}; - - /// The ideal system inertia. - Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints, - 0}; - - /// The value of δ from the previous run of compute(). - Scalar m_prev_δ{0}; - - // Number of non-zeros in LHS. - int m_non_zeros = -1; - - /// Computes factorization of a sparse matrix. - /// - /// @param lhs Matrix to factorize. - /// @return The factorization. - SparseSolver& compute_sparse(const SparseMatrix& lhs) { - // Reanalize lhs's sparsity pattern if it changed - int non_zeros = lhs.nonZeros(); - if (m_non_zeros != non_zeros) { - m_sparse_solver.analyzePattern(lhs); - m_non_zeros = non_zeros; - } - - m_sparse_solver.factorize(lhs); - - return m_sparse_solver; - } - - /// Returns regularization matrix. - /// - /// @param δ The Hessian regularization factor. - /// @param γ The equality constraint Jacobian regularization factor. - /// @return Regularization matrix. - SparseMatrix regularization(Scalar δ, Scalar γ) const { - DenseVector vec{m_num_decision_variables + m_num_equality_constraints}; - vec.segment(0, m_num_decision_variables).setConstant(δ); - vec.segment(m_num_decision_variables, m_num_equality_constraints) - .setConstant(-γ); - - return SparseMatrix{vec.asDiagonal()}; - } + bool m_use_sparse_solver; + SparseRegularizedLDLT m_sparse_solver; + DenseRegularizedLDLT m_dense_solver; }; } // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_inf_norms.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_inf_norms.hpp new file mode 100644 index 0000000000..18ba9db897 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_inf_norms.hpp @@ -0,0 +1,34 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include + +namespace slp { + +/// Computes the row-wise infinity norm of a sparse matrix. +/// +/// @param mat Sparse matrix. +/// @return A dense vector where each element is the infinity norm of the +/// corresponding row of the sparse matrix. +template +auto sparse_inf_norms(const Eigen::SparseCompressedBase& mat) + -> Eigen::Vector { + using Scalar = typename Derived::Scalar; + using DenseVector = Eigen::Vector; + + DenseVector norms = DenseVector::Zero(mat.rows()); + for (int k = 0; k < mat.outerSize(); ++k) { + for (typename Derived::InnerIterator it{mat, k}; it; ++it) { + using std::abs; + norms[it.row()] = std::max(norms[it.row()], abs(it.value())); + } + } + + return norms; +} + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp new file mode 100644 index 0000000000..377a0635b4 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp @@ -0,0 +1,227 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include +#include + +#include "sleipnir/optimization/solver/util/inertia.hpp" + +namespace slp { + +/// Solves sparse systems of linear equations using a regularized LDLT +/// factorization. +/// +/// @tparam Scalar Scalar type. +template +class SparseRegularizedLDLT { + public: + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + /// Type alias for sparse matrix. + using SparseMatrix = Eigen::SparseMatrix; + + /// Constructs a SparseRegularizedLDLT instance. + /// + /// @param num_decision_variables The number of decision variables in the + /// system. + /// @param num_equality_constraints The number of equality constraints in the + /// system. + SparseRegularizedLDLT(int num_decision_variables, + int num_equality_constraints) + : m_num_decision_variables{num_decision_variables}, + m_num_equality_constraints{num_equality_constraints} {} + + /// Constructs a SparseRegularizedLDLT instance. + /// + /// @param num_decision_variables The number of decision variables in the + /// system. + /// @param num_equality_constraints The number of equality constraints in the + /// system. + /// @param γ_min The minimum constraint regularization. + SparseRegularizedLDLT(int num_decision_variables, + int num_equality_constraints, Scalar γ_min) + : m_num_decision_variables{num_decision_variables}, + m_num_equality_constraints{num_equality_constraints}, + m_γ_min{γ_min} {} + + /// Reports whether previous computation was successful. + /// + /// @return Whether previous computation was successful. + Eigen::ComputationInfo info() const { return m_info; } + + /// Computes the regularized LDLT factorization of a matrix. + /// + /// The matrix's symbolic decomposition is reused in subsequent calls, so + /// subsequent calls must be given a matrix with the same sparsity pattern. + /// + /// @param lhs Left-hand side of the system. + /// @return The factorization. + SparseRegularizedLDLT& compute(const SparseMatrix& lhs) { + // Regularization with zeros ensures the pattern analysis in the sparse + // solver is reused by all factorizations + SparseMatrix unregularized_lhs = lhs + regularization(Scalar(0), Scalar(0)); + + if (!m_analyzed_pattern) { + m_solver.analyzePattern(unregularized_lhs); + m_analyzed_pattern = true; + } + + m_solver.factorize(unregularized_lhs); + m_info = m_solver.info(); + + if (m_info == Eigen::Success) { + auto D = m_solver.vectorD(); + + // If the inertia is ideal and D from LDLT is sufficiently far from zero, + // don't regularize the system + if (Inertia{D} == ideal_inertia && + (D.cwiseAbs().array() >= Scalar(1e-4)).all()) { + m_prev_δ = Scalar(0); + m_prev_γ = Scalar(0); + return *this; + } + } + + // We'll give lhs the correct inertia by adding [δI, 0; 0, −γI] where δ and + // γ regularize the Hessian and equality constraint Jacobian respectively. + + // If the previous δ was zero, attempt a small value. Otherwise, attempt a + // smaller value than the previous δ so δ trends downward. + Scalar δ = m_prev_δ == Scalar(0) + ? Scalar(1e-4) + : std::max(m_prev_δ / Scalar(2), + std::numeric_limits::epsilon()); + + // Start γ at the minimum to minimize equality constraint Jacobian + // distortion. + Scalar γ = m_γ_min; + + while (true) { + m_solver.factorize(lhs + regularization(δ, γ)); + m_info = m_solver.info(); + + if (m_info == Eigen::Success) { + Inertia inertia{m_solver.vectorD()}; + + if (inertia == ideal_inertia) { + // If the inertia is ideal, report success + m_prev_δ = δ; + m_prev_γ = γ; + return *this; + } else if (inertia.zero > 0) { + if (γ == Scalar(0)) { + // If there's zero eigenvalues and γ = 0, increase γ to potentially + // compensate for a rank-deficient equality constraint Jacobian + γ = Scalar(1e-10); + } else { + // If there's zero eigenvalues and γ > 0, increase δ and γ to drive + // all eigenvalues away from zero + δ *= Scalar(10); + γ *= Scalar(10); + } + } else if (inertia.negative > ideal_inertia.negative) { + // If there's too many negative eigenvalues, increase δ to add more + // positive eigenvalues + δ *= Scalar(10); + } else if (inertia.positive > ideal_inertia.positive) { + // If there's too many positive eigenvalues, increase γ to add more + // negative eigenvalues + γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10); + } + } else { + // If the decomposition failed, increase δ and γ to drive all + // eigenvalues away from zero + δ *= Scalar(10); + γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10); + } + + // If the lhs perturbation is too high, report failure. This can be caused + // by ill-conditioning. + if (δ > Scalar(1e20) || γ > Scalar(1e20)) { + m_info = Eigen::NumericalIssue; + m_prev_δ = δ; + m_prev_γ = γ; + return *this; + } + } + } + + /// Solves the system of equations using a regularized LDLT factorization. + /// + /// @param rhs Right-hand side of the system. + /// @return The solution. + template + DenseVector solve(const Eigen::MatrixBase& rhs) const { + return m_solver.solve(rhs); + } + + /// Solves the system of equations using a regularized LDLT factorization. + /// + /// @param rhs Right-hand side of the system. + /// @return The solution. + template + DenseVector solve(const Eigen::SparseMatrixBase& rhs) const { + return m_solver.solve(rhs); + } + + /// Returns the Hessian regularization factor. + /// + /// @return Hessian regularization factor. + Scalar hessian_regularization() const { return m_prev_δ; } + + /// Returns the constraint Jacobian regularization factor. + /// + /// @return Constraint Jacobian regularization factor. + Scalar constraint_jacobian_regularization() const { return m_prev_γ; } + + private: + using Solver = Eigen::SimplicialLDLT; + + Solver m_solver; + bool m_analyzed_pattern = false; + + Eigen::ComputationInfo m_info = Eigen::Success; + + /// The number of decision variables in the system. + int m_num_decision_variables = 0; + + /// The number of equality constraints in the system. + int m_num_equality_constraints = 0; + + /// The minimum constraint regularization. + Scalar m_γ_min{1e-10}; + + /// The ideal system inertia. + Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints, + 0}; + + /// The value of δ from the previous run of compute(). + Scalar m_prev_δ{0}; + + /// The value of γ from the previous run of compute(). + Scalar m_prev_γ{0}; + + /// Returns regularization matrix. + /// + /// [δI 0] + /// [ 0 −γI] + /// + /// @param δ The Hessian regularization factor. + /// @param γ The equality constraint Jacobian regularization factor. + /// @return Regularization matrix. + SparseMatrix regularization(Scalar δ, Scalar γ) const { + DenseVector vec{m_num_decision_variables + m_num_equality_constraints}; + vec.segment(0, m_num_decision_variables).setConstant(δ); + vec.segment(m_num_decision_variables, m_num_equality_constraints) + .setConstant(-γ); + + return SparseMatrix{vec.asDiagonal()}; + } +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp index 69cb99957c..831ae3e5d1 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp @@ -25,10 +25,8 @@ namespace slp { enum class IterationType : uint8_t { /// Normal iteration. NORMAL, - /// Accepted second-order correction iteration. - ACCEPTED_SOC, - /// Rejected second-order correction iteration. - REJECTED_SOC, + /// Second-order correction iteration. + SECOND_ORDER_CORRECTION, /// Feasibility restoration iteration. FEASIBILITY_RESTORATION }; @@ -73,8 +71,8 @@ std::string power_of_10(Scalar value) { if (exponent < 0) { output += "⁻"; } - constexpr std::array strs = {"⁰", "¹", "²", "³", "⁴", - "⁵", "⁶", "⁷", "⁸", "⁹"}; + constexpr std::array strs{"⁰", "¹", "²", "³", "⁴", + "⁵", "⁶", "⁷", "⁸", "⁹"}; for (const auto& digit : digits | std::views::reverse) { output += strs[digit]; } @@ -183,6 +181,9 @@ inline void print_bound_constraint_global_infeasibility_error( /// @param complementarity The complementarity. /// @param μ The barrier parameter. /// @param δ The Hessian regularization factor. +/// @param γ The constraint Jacobian regularization factor. +/// @param full_primal_step_inf_norm The infinity norm of the full primal step. +/// @param full_dual_step_inf_norm The infinity norm of the full dual step. /// @param primal_α The primal step size. /// @param primal_α_max The max primal step size. /// @param α_reduction_factor Factor by which primal_α is reduced during @@ -193,33 +194,23 @@ void print_iteration_diagnostics(int iterations, IterationType type, const std::chrono::duration& time, Scalar error, Scalar cost, Scalar infeasibility, Scalar complementarity, - Scalar μ, Scalar δ, Scalar primal_α, - Scalar primal_α_max, Scalar α_reduction_factor, - Scalar dual_α) { + Scalar μ, Scalar δ, Scalar γ, + Scalar full_primal_step_inf_norm, + Scalar full_dual_step_inf_norm, + Scalar primal_α, Scalar primal_α_max, + Scalar α_reduction_factor, Scalar dual_α) { if (iterations % 20 == 0) { if (iterations == 0) { - slp::print("┏"); + slp::println("┏{:━^119}┓", ""); } else { - slp::print("┢"); - } - slp::print( - "{:━^4}┯{:━^4}┯{:━^9}┯{:━^12}┯{:━^13}┯{:━^12}┯{:━^12}┯{:━^8}┯{:━^5}┯" - "{:━^8}┯{:━^8}┯{:━^2}", - "", "", "", "", "", "", "", "", "", "", "", ""); - if (iterations == 0) { - slp::println("┓"); - } else { - slp::println("┪"); + slp::println("┢{:━^119}┪", ""); } slp::println( - "┃{:^4}│{:^4}│{:^9}│{:^12}│{:^13}│{:^12}│{:^12}│{:^8}│{:^5}│{:^8}│{:^8}" - "│{:^2}┃", - "iter", "type", "time (ms)", "error", "cost", "infeas.", "complement.", - "μ", "reg", "primal α", "dual α", "↩"); - slp::println( - "┡{:━^4}┷{:━^4}┷{:━^9}┷{:━^12}┷{:━^13}┷{:━^12}┷{:━^12}┷{:━^8}┷{:━^5}┷" - "{:━^8}┷{:━^8}┷{:━^2}┩", - "", "", "", "", "", "", "", "", "", "", "", ""); + "┃{:^4} {:^9} {:^10} {:^11} {:^10} {:^8} {:^8} {:^5} {:^5} {:^8} " + "{:^8} {:^8} {:^8} {:^2}┃", + "iter", "duration", "error", "cost", "infeas.", "complem.", "μ", "δ", + "γ", "|p_pr|", "|p_du|", "α_pr", "α_du", "↩"); + slp::println("┡{:━^119}┩", ""); } // For the number of backtracks, we want x such that: @@ -236,12 +227,13 @@ void print_iteration_diagnostics(int iterations, IterationType type, int backtracks = static_cast(log(primal_α / primal_α_max) / log(α_reduction_factor)); - constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC", "rest"}; + constexpr std::array ITERATION_TYPES{" ", "s", "r"}; slp::println( - "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} " - "{:.2e} {:2d}│", + "│{:4} {:1} {:9.3f} {:10.4e} {:11.4e} {:10.4e} {:8.2e} {:8.2e} {:<5} " + "{:<5} {:8.2e} {:8.2e} {:8.2e} {:8.2e} {:2d}│", iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error, - cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α, + cost, infeasibility, complementarity, μ, power_of_10(δ), power_of_10(γ), + full_primal_step_inf_norm, full_dual_step_inf_norm, primal_α, dual_α, backtracks); } #else @@ -251,7 +243,7 @@ void print_iteration_diagnostics(int iterations, IterationType type, #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS /// Prints bottom of iteration diagnostics table. inline void print_bottom_iteration_diagnostics() { - slp::println("└{:─^108}┘", ""); + slp::println("└{:─^119}┘", ""); } #else #define print_bottom_iteration_diagnostics(...) @@ -269,7 +261,7 @@ std::string histogram(double value) { double ipart; int fpart = static_cast(std::modf(value * Width, &ipart) * 8); - constexpr std::array strs = {" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"}; + constexpr std::array strs{" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"}; std::string hist; int index = 0; @@ -297,10 +289,10 @@ inline void print_solver_diagnostics( const gch::small_vector& solve_profilers) { auto solve_duration = to_ms(solve_profilers[0].total_duration()); - slp::println("┏{:━^21}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); - slp::println("┃{:^21}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent", - "total (ms)", "each (ms)", "runs"); - slp::println("┡{:━^21}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); + slp::println("┏{:━^66}┓", ""); + slp::println("┃{:^21} {:^18} {:^10} {:^9} {:^4}┃", "time trace", "percentage", + "total", "each", "runs"); + slp::println("┡{:━^66}┩", ""); for (auto& profiler : solve_profilers) { double norm = solve_duration == 0.0 @@ -326,23 +318,26 @@ inline void print_setup_diagnostics( const gch::small_vector& setup_profilers) { auto setup_duration = to_ms(setup_profilers[0].duration()); + // Print link to diagnostic output description + slp::println( + "See https://sleipnirgroup.github.io/Sleipnir/md_usage.html#output for " + "diagnostic output description.\n"); + // Print heading - slp::println("┏{:━^21}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); - slp::println("┃{:^21}│{:^18}│{:^10}│{:^9}│{:^4}┃", "setup trace", "percent", - "total (ms)", "each (ms)", "runs"); - slp::println("┡{:━^21}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); + slp::println("┏{:━^50}┓", ""); + slp::println("┃{:^21} {:^18} {:^9}┃", "time trace", "percentage", "duration"); + slp::println("┡{:━^50}┩", ""); // Print setup profilers for (auto& profiler : setup_profilers) { double norm = setup_duration == 0.0 ? (&profiler == &setup_profilers[0] ? 1.0 : 0.0) : to_ms(profiler.duration()) / setup_duration; - slp::println("│{:<21} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", - profiler.name(), norm * 100.0, histogram<9>(norm), - to_ms(profiler.duration()), to_ms(profiler.duration()), "1"); + slp::println("│{:<21} {:>6.2f}%▕{}▏ {:>9.3f}│", profiler.name(), + norm * 100.0, histogram<9>(norm), to_ms(profiler.duration())); } - slp::println("└{:─^66}┘", ""); + slp::println("└{:─^50}┘", ""); } #else #define print_setup_diagnostics(...) diff --git a/wpimath/src/test/java/org/wpilib/math/optimization/FlywheelOCPTest.java b/wpimath/src/test/java/org/wpilib/math/optimization/FlywheelOCPTest.java index 79856580c0..d9664e0997 100644 --- a/wpimath/src/test/java/org/wpilib/math/optimization/FlywheelOCPTest.java +++ b/wpimath/src/test/java/org/wpilib/math/optimization/FlywheelOCPTest.java @@ -103,7 +103,7 @@ class FlywheelOCPTest { // steady-states. assertEquals(u, problem.U().value(0, k), 2.0); } else { - assertEquals(u, problem.U().value(0, k), 1e-4); + assertEquals(u, problem.U().value(0, k), 2e-4); } } diff --git a/wpimath/src/test/java/org/wpilib/math/optimization/solver/ExitStatusTest.java b/wpimath/src/test/java/org/wpilib/math/optimization/solver/ExitStatusTest.java index 38d37076b3..3f88707e89 100644 --- a/wpimath/src/test/java/org/wpilib/math/optimization/solver/ExitStatusTest.java +++ b/wpimath/src/test/java/org/wpilib/math/optimization/solver/ExitStatusTest.java @@ -27,13 +27,16 @@ class ExitStatusTest { problem.minimize(x.times(x)); problem.addCallback(info -> false); + x.setValue(1.0); assertEquals(ExitStatus.SUCCESS, problem.solve()); problem.addCallback(info -> true); + x.setValue(1.0); assertEquals(ExitStatus.CALLBACK_REQUESTED_STOP, problem.solve()); problem.clearCallbacks(); problem.addCallback(info -> false); + x.setValue(1.0); assertEquals(ExitStatus.SUCCESS, problem.solve()); } @@ -187,6 +190,7 @@ class ExitStatusTest { try (var problem = new Problem()) { var x = problem.decisionVariable(); + x.setValue(1.0); problem.minimize(x.times(x)); @@ -207,6 +211,7 @@ class ExitStatusTest { try (var problem = new Problem()) { var x = problem.decisionVariable(); + x.setValue(1.0); problem.minimize(x.times(x));