From f3757bdeae7cab6df648dafa4add9d53dd881dd2 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 29 Mar 2026 20:39:18 -0700 Subject: [PATCH] [upstream_utils] Upgrade to Sleipnir 0.5.0 (#8711) --- upstream_utils/sleipnir.py | 2 +- .../sleipnir_patches/0001-Use-fmtlib.patch | 60 +- .../0002-Use-wpi-SmallVector.patch | 8 +- .../0003-Use-wpi-byteswap.patch | 2 +- .../0004-Replace-std-to_underlying.patch | 74 ++- .../0005-Replace-std-views-zip.patch | 10 +- .../0006-Replace-std-unreachable.patch | 10 +- ...-Suppress-clang-tidy-false-positives.patch | 6 +- ...ppress-GCC-12-warning-false-positive.patch | 4 +- ...ress-Doxygen-warning-false-positives.patch | 31 + ...ead-of-multidimensional-array-subsc.patch} | 38 +- .../include/sleipnir/autodiff/expression.hpp | 159 +++++ .../sleipnir/autodiff/expression_graph.hpp | 4 +- .../sleipnir/autodiff/expression_type.hpp | 9 +- .../autodiff/gradient_expression_graph.hpp | 6 +- .../include/sleipnir/autodiff/hessian.hpp | 9 +- .../include/sleipnir/autodiff/jacobian.hpp | 8 +- .../include/sleipnir/autodiff/variable.hpp | 90 +++ .../sleipnir/autodiff/variable_matrix.hpp | 22 +- .../include/sleipnir/optimization/ocp.hpp | 38 +- .../include/sleipnir/optimization/problem.hpp | 258 ++++---- .../optimization/solver/exit_status.hpp | 32 +- .../optimization/solver/interior_point.hpp | 334 ++++++---- .../interior_point_matrix_callbacks.hpp | 60 +- .../optimization/solver/iteration_info.hpp | 9 + .../sleipnir/optimization/solver/newton.hpp | 42 +- .../solver/newton_matrix_callbacks.hpp | 5 +- .../sleipnir/optimization/solver/options.hpp | 72 +-- .../sleipnir/optimization/solver/sqp.hpp | 201 ++++-- .../solver/sqp_matrix_callbacks.hpp | 42 +- .../optimization/solver/util/all_finite.hpp | 26 + .../solver/util/append_as_triplets.hpp | 60 ++ .../solver/util/error_estimate.hpp | 130 ---- .../solver/util/feasibility_restoration.hpp | 601 ++++++++++++++++++ .../optimization/solver/util/filter.hpp | 211 +++--- .../optimization/solver/util/kkt_error.hpp | 96 ++- .../util/lagrange_multiplier_estimate.hpp | 135 ++++ .../solver/util/regularized_ldlt.hpp | 40 +- .../sleipnir/include/sleipnir/util/assert.hpp | 4 +- .../include/sleipnir/util/function_ref.hpp | 2 +- .../sleipnir/util/print_diagnostics.hpp | 16 +- .../include/sleipnir/util/profiler.hpp | 187 ++++++ .../include/sleipnir/util/scoped_profiler.hpp | 66 -- .../include/sleipnir/util/setup_profiler.hpp | 56 -- .../include/sleipnir/util/solve_profiler.hpp | 87 --- .../include/sleipnir/util/to_underlying.hpp | 14 + .../cpp/optimization/NonlinearProblemTest.cpp | 17 +- 47 files changed, 2413 insertions(+), 980 deletions(-) create mode 100644 upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch rename upstream_utils/sleipnir_patches/{0009-Use-operator-instead-of-multidimensional-array-subsc.patch => 0010-Use-operator-instead-of-multidimensional-array-subsc.patch} (96%) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/profiler.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp delete mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index 27a77430ee..b4bd6ddbc8 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.3.3" + tag = "v0.5.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 9bda09e602..4b8fc3c136 100644 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -1,18 +1,18 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 29 May 2024 16:29:55 -0700 -Subject: [PATCH 1/9] Use fmtlib +Subject: [PATCH 01/10] Use fmtlib --- - include/sleipnir/autodiff/expression_type.hpp | 9 ++++--- + include/sleipnir/autodiff/expression_type.hpp | 16 +++++++---- include/sleipnir/optimization/problem.hpp | 1 + - .../optimization/solver/exit_status.hpp | 9 ++++--- + .../optimization/solver/exit_status.hpp | 16 +++++++---- include/sleipnir/util/assert.hpp | 5 ++-- include/sleipnir/util/print.hpp | 27 ++++++++++--------- - 5 files changed, 28 insertions(+), 23 deletions(-) + 5 files changed, 40 insertions(+), 25 deletions(-) diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp -index 200324a2c38c58dbe07e96c061658c9e65aacce6..2fde6bdf3cf72f46fb633b72e29e66209e9f6a83 100644 +index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac8446a3ab 100644 --- a/include/sleipnir/autodiff/expression_type.hpp +++ b/include/sleipnir/autodiff/expression_type.hpp @@ -4,9 +4,10 @@ @@ -27,8 +27,12 @@ index 200324a2c38c58dbe07e96c061658c9e65aacce6..2fde6bdf3cf72f46fb633b72e29e6620 namespace slp { /// Expression type. -@@ -29,12 +30,12 @@ enum class ExpressionType : uint8_t { +@@ -27,14 +28,16 @@ enum class ExpressionType : uint8_t { + } // namespace slp + ++// @cond Suppress Doxygen ++ /// Formatter for ExpressionType. template <> -struct std::formatter { @@ -42,15 +46,27 @@ index 200324a2c38c58dbe07e96c061658c9e65aacce6..2fde6bdf3cf72f46fb633b72e29e6620 return m_underlying.parse(ctx); } -@@ -65,5 +66,5 @@ struct std::formatter { +@@ -45,7 +48,8 @@ struct std::formatter { + /// @param ctx Format context. + /// @return Format context iterator. + template +- auto format(const slp::ExpressionType& type, FmtContext& ctx) const { ++ constexpr auto format(const slp::ExpressionType& type, ++ FmtContext& ctx) const { + using enum slp::ExpressionType; + + switch (type) { +@@ -65,5 +69,7 @@ struct std::formatter { } private: - std::formatter m_underlying; + fmt::formatter m_underlying; }; ++ ++// @endcond diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 59acc732dbc5bfe520745089abd156da9a8a2e51..ba45ead1500b45616b74e1d108959127484cf4d6 100644 +index 42e572b6c13bba6d5f7592d2b7ffbe1879b1d99b..bb0bb21bb932f77f95c7779241784c4c54d6fe93 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp @@ -15,6 +15,7 @@ @@ -62,7 +78,7 @@ index 59acc732dbc5bfe520745089abd156da9a8a2e51..ba45ead1500b45616b74e1d108959127 #include "sleipnir/autodiff/expression_type.hpp" diff --git a/include/sleipnir/optimization/solver/exit_status.hpp b/include/sleipnir/optimization/solver/exit_status.hpp -index e17f4490cc0f6704f0b5d29340ec6dfbbcbb8aee..c1e4d319c197bdad2ace19500fb3a0426060e87b 100644 +index 01dddd0d4967268cbbb2c808f7973fafc6be081d..8a7904dd5664dcd66d1332f38902df2252387a58 100644 --- a/include/sleipnir/optimization/solver/exit_status.hpp +++ b/include/sleipnir/optimization/solver/exit_status.hpp @@ -4,9 +4,10 @@ @@ -77,13 +93,17 @@ index e17f4490cc0f6704f0b5d29340ec6dfbbcbb8aee..c1e4d319c197bdad2ace19500fb3a042 namespace slp { /// Solver exit status. Negative values indicate failure. -@@ -43,12 +44,12 @@ enum class ExitStatus : int8_t { +@@ -42,14 +43,16 @@ enum class ExitStatus : int8_t { + } // namespace slp + ++// @cond Suppress Doxygen ++ /// Formatter for ExitStatus. template <> -struct std::formatter { +struct fmt::formatter { - /// Parse format string. + /// Parses format string. /// /// @param ctx Format parse context. /// @return Format parse context iterator. @@ -92,15 +112,27 @@ index e17f4490cc0f6704f0b5d29340ec6dfbbcbb8aee..c1e4d319c197bdad2ace19500fb3a042 return m_underlying.parse(ctx); } -@@ -92,5 +93,5 @@ struct std::formatter { +@@ -60,7 +63,8 @@ struct std::formatter { + /// @param ctx Format context. + /// @return Format context iterator. + template +- auto format(const slp::ExitStatus& exit_status, FmtContext& ctx) const { ++ constexpr auto format(const slp::ExitStatus& exit_status, ++ FmtContext& ctx) const { + using enum slp::ExitStatus; + + switch (exit_status) { +@@ -93,5 +97,7 @@ struct std::formatter { } 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 dd479f11080d67b03dbe72475be974908e96fc2f..4f0203a4b8df2f86e1e15cf1102a51c340665722 100644 +index 29dfe5416530a476417de3a4b6434d3b28d73120..783a0e71a1cb54bd1d6d17fa9d2bedd2038a8ef5 100644 --- a/include/sleipnir/util/assert.hpp +++ b/include/sleipnir/util/assert.hpp @@ -3,16 +3,17 @@ @@ -113,7 +145,7 @@ index dd479f11080d67b03dbe72475be974908e96fc2f..4f0203a4b8df2f86e1e15cf1102a51c3 +#include + - /// Throw an exception in Python. + /// Throws an exception in Python. #define slp_assert(condition) \ do { \ if (!(condition)) { \ diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 4c6e3f00ab..30d9aa569c 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 16 Jun 2024 12:08:49 -0700 -Subject: [PATCH 2/9] Use wpi::SmallVector +Subject: [PATCH 02/10] Use wpi::SmallVector --- include/sleipnir/autodiff/expression.hpp | 4 ++-- @@ -10,7 +10,7 @@ Subject: [PATCH 2/9] Use wpi::SmallVector 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp -index 36d7e7248a77aaa843f02b262d3865711097a2cb..0e6f31bf77beb6759b8907da9a05e8405c5c5cca 100644 +index 76f6d4ed534ce685194ad7fc62d9b62eb2ca1096..10e20f5e2fb0d7459fcecac2f8ba1bdcc98efc3f 100644 --- a/include/sleipnir/autodiff/expression.hpp +++ b/include/sleipnir/autodiff/expression.hpp @@ -34,7 +34,7 @@ struct Expression; @@ -32,7 +32,7 @@ index 36d7e7248a77aaa843f02b262d3865711097a2cb..0e6f31bf77beb6759b8907da9a05e840 // Expression destructor when expr's refcount reaches zero can cause a stack // overflow. Instead, we iterate over its children to decrement their diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp -index cca246d6c33f4a7e3d0a89f5d9bd21a3f7916aeb..40d18f2a680d6537d698c1c774d6996c65a173d8 100644 +index 910ebcf5266bdf76711db7849dd6584549094e3b..3c4f67c1f6224226620e2ffcc597336435943ce8 100644 --- a/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp @@ -53,7 +53,7 @@ class Variable : public SleipnirBase { @@ -54,7 +54,7 @@ index cca246d6c33f4a7e3d0a89f5d9bd21a3f7916aeb..40d18f2a680d6537d698c1c774d6996c /// Assignment operator for scalar. diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 4c5a98311b1ea542877f38db086b51fd0b7c00f1..88122498f384a68b2537b24e57c4a291a951e6dd 100644 +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 { diff --git a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch index 5805aa0b25..15eacc8b43 100644 --- a/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch +++ b/upstream_utils/sleipnir_patches/0003-Use-wpi-byteswap.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 28 Jan 2025 22:19:14 -0800 -Subject: [PATCH 3/9] Use wpi::byteswap() +Subject: [PATCH 03/10] Use wpi::byteswap() --- include/sleipnir/util/spy.hpp | 3 ++- 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 130d27ae08..39ba8a6681 100644 --- a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch +++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch @@ -1,55 +1,89 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 28 Jan 2025 22:19:31 -0800 -Subject: [PATCH 4/9] Replace std::to_underlying() +Subject: [PATCH 04/10] Replace std::to_underlying() --- - include/sleipnir/optimization/problem.hpp | 8 ++++---- - include/sleipnir/util/print_diagnostics.hpp | 6 +++--- - 2 files changed, 7 insertions(+), 7 deletions(-) + include/sleipnir/optimization/problem.hpp | 9 +++++---- + include/sleipnir/util/print_diagnostics.hpp | 3 ++- + include/sleipnir/util/to_underlying.hpp | 14 ++++++++++++++ + 3 files changed, 21 insertions(+), 5 deletions(-) + create mode 100644 include/sleipnir/util/to_underlying.hpp diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index ba45ead1500b45616b74e1d108959127484cf4d6..22e7cdaa945648e2b9effcc30533d6602c839c22 100644 +index bb0bb21bb932f77f95c7779241784c4c54d6fe93..61c5de59d8c5f4d582eff4f650c66c8d50e181a0 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp -@@ -657,11 +657,11 @@ class Problem { +@@ -37,6 +37,7 @@ + #include "sleipnir/util/profiler.hpp" + #include "sleipnir/util/spy.hpp" + #include "sleipnir/util/symbol_exports.hpp" ++#include "sleipnir/util/to_underlying.hpp" + + namespace slp { + +@@ -696,11 +697,11 @@ class Problem { // Print problem structure slp::println("\nProblem structure:"); slp::println(" ↳ {} cost function", - types[std::to_underlying(cost_function_type())]); -+ types[static_cast(cost_function_type())]); ++ types[slp::to_underlying(cost_function_type())]); slp::println(" ↳ {} equality constraints", - types[std::to_underlying(equality_constraint_type())]); -+ types[static_cast(equality_constraint_type())]); ++ types[slp::to_underlying(equality_constraint_type())]); slp::println(" ↳ {} inequality constraints", - types[std::to_underlying(inequality_constraint_type())]); -+ types[static_cast(inequality_constraint_type())]); ++ types[slp::to_underlying(inequality_constraint_type())]); if (m_decision_variables.size() == 1) { slp::print("\n1 decision variable\n"); -@@ -673,7 +673,7 @@ class Problem { +@@ -712,7 +713,7 @@ class Problem { [](const gch::small_vector>& constraints) { std::array counts{}; for (const auto& constraint : constraints) { - ++counts[std::to_underlying(constraint.type())]; -+ ++counts[static_cast(constraint.type())]; ++ ++counts[slp::to_underlying(constraint.type())]; } 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 54277542c46e9490a9ef01c43dc8572e9367c97e..c50d7e171bc90ea47108de90471e7d778885f6af 100644 +index f44663c6d283e6b1658a330c79e39de0dc9b428e..698a8df43d4236bb6740e8cf7872d2e1f5df8c31 100644 --- a/include/sleipnir/util/print_diagnostics.hpp +++ b/include/sleipnir/util/print_diagnostics.hpp -@@ -238,9 +238,9 @@ void print_iteration_diagnostics(int iterations, IterationType type, +@@ -17,6 +17,7 @@ + + #include "sleipnir/util/print.hpp" + #include "sleipnir/util/profiler.hpp" ++#include "sleipnir/util/to_underlying.hpp" + + namespace slp { + +@@ -239,7 +240,7 @@ void print_iteration_diagnostics(int iterations, IterationType type, slp::println( "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} " "{:.2e} {:2d}│", - iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error, -- cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α, -- backtracks); -+ iterations, ITERATION_TYPES[static_cast(type)], to_ms(time), -+ error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, -+ dual_α, backtracks); ++ iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error, + cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α, + backtracks); } - #else - #define print_iteration_diagnostics(...) +diff --git a/include/sleipnir/util/to_underlying.hpp b/include/sleipnir/util/to_underlying.hpp +new file mode 100644 +index 0000000000000000000000000000000000000000..3f9b4835b912c5a0d998c43828f255c61d0f573e +--- /dev/null ++++ b/include/sleipnir/util/to_underlying.hpp +@@ -0,0 +1,14 @@ ++// Copyright (c) Sleipnir contributors ++ ++#pragma once ++ ++#include ++ ++namespace slp { ++ ++template ++constexpr std::underlying_type_t to_underlying(Enum e) noexcept { ++ return static_cast>(e); ++} ++ ++} // namespace slp 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 fb38c351e7..4559bea611 100644 --- a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch +++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 8 Feb 2025 13:42:36 -0800 -Subject: [PATCH 5/9] Replace std::views::zip() +Subject: [PATCH 05/10] Replace std::views::zip() --- include/sleipnir/autodiff/gradient_expression_graph.hpp | 5 ++++- @@ -9,7 +9,7 @@ Subject: [PATCH 5/9] 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 414b98971580a6e3209eabfffd082de1dd5711c7..d21c1b5ad1b2f79ac4ec4e525e9b7fe7789a0461 100644 +index 275c30b76d34efe7ee1608cd4eedfa54ab2dc1ec..c0e3c161343175837abcb25fb22da0c611a44799 100644 --- a/include/sleipnir/autodiff/gradient_expression_graph.hpp +++ b/include/sleipnir/autodiff/gradient_expression_graph.hpp @@ -161,7 +161,10 @@ class GradientExpressionGraph { @@ -25,12 +25,12 @@ index 414b98971580a6e3209eabfffd082de1dd5711c7..d21c1b5ad1b2f79ac4ec4e525e9b7fe7 if (col != -1 && node->adjoint != Scalar(0)) { triplets.emplace_back(row, col, node->adjoint); diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 22e7cdaa945648e2b9effcc30533d6602c839c22..70955fdc7148e5af737d3094a5602024df790b3d 100644 +index 61c5de59d8c5f4d582eff4f650c66c8d50e181a0..6acd2f655a4b35d087ff365c2a2917b015b03b32 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp -@@ -675,9 +675,11 @@ class Problem { +@@ -715,9 +715,11 @@ class Problem { for (const auto& constraint : constraints) { - ++counts[static_cast(constraint.type())]; + ++counts[slp::to_underlying(constraint.type())]; } - for (const auto& [count, name] : - std::views::zip(counts, std::array{"empty", "constant", "linear", diff --git a/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch b/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch index 0ac2097033..91f23f31fe 100644 --- a/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch +++ b/upstream_utils/sleipnir_patches/0006-Replace-std-unreachable.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 3 Dec 2025 23:38:53 -0800 -Subject: [PATCH 6/9] Replace std::unreachable() +Subject: [PATCH 06/10] Replace std::unreachable() --- include/sleipnir/autodiff/expression_type.hpp | 6 +++--- @@ -11,7 +11,7 @@ Subject: [PATCH 6/9] Replace std::unreachable() create mode 100644 include/sleipnir/util/unreachable.hpp diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp -index 2fde6bdf3cf72f46fb633b72e29e66209e9f6a83..8a2ae457808b1ae505c19c8f65a5c68e6e11a082 100644 +index d06d32dac6c7b6faeedefeaa107cedac8446a3ab..1957fc0339b5538fbdc56a8ea2d8503c89ed4b59 100644 --- a/include/sleipnir/autodiff/expression_type.hpp +++ b/include/sleipnir/autodiff/expression_type.hpp @@ -4,10 +4,10 @@ @@ -27,7 +27,7 @@ index 2fde6bdf3cf72f46fb633b72e29e66209e9f6a83..8a2ae457808b1ae505c19c8f65a5c68e namespace slp { /// Expression type. -@@ -61,7 +61,7 @@ struct fmt::formatter { +@@ -64,7 +64,7 @@ struct fmt::formatter { case NONLINEAR: return m_underlying.format("nonlinear", ctx); default: @@ -37,7 +37,7 @@ index 2fde6bdf3cf72f46fb633b72e29e66209e9f6a83..8a2ae457808b1ae505c19c8f65a5c68e } diff --git a/include/sleipnir/optimization/solver/exit_status.hpp b/include/sleipnir/optimization/solver/exit_status.hpp -index c1e4d319c197bdad2ace19500fb3a0426060e87b..4d43cea244afd6c8b0235b84e9140359451fbf2f 100644 +index 8a7904dd5664dcd66d1332f38902df2252387a58..ee9bfbc7630513a33297dbeaf39fd695509b5a97 100644 --- a/include/sleipnir/optimization/solver/exit_status.hpp +++ b/include/sleipnir/optimization/solver/exit_status.hpp @@ -4,10 +4,10 @@ @@ -53,7 +53,7 @@ index c1e4d319c197bdad2ace19500fb3a0426060e87b..4d43cea244afd6c8b0235b84e9140359 namespace slp { /// Solver exit status. Negative values indicate failure. -@@ -88,7 +88,7 @@ struct fmt::formatter { +@@ -92,7 +92,7 @@ struct fmt::formatter { case TIMEOUT: return m_underlying.format("timeout", ctx); default: diff --git a/upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch b/upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch index 7e4126c87f..b95a1f11d0 100644 --- a/upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch +++ b/upstream_utils/sleipnir_patches/0007-Suppress-clang-tidy-false-positives.patch @@ -1,17 +1,17 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 10 Feb 2025 11:37:02 -0800 -Subject: [PATCH 7/9] Suppress clang-tidy false positives +Subject: [PATCH 07/10] Suppress clang-tidy false positives --- include/sleipnir/autodiff/variable.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp -index 40d18f2a680d6537d698c1c774d6996c65a173d8..2091f339744f8f8980ccb1e44b0b54b06eb32418 100644 +index 3c4f67c1f6224226620e2ffcc597336435943ce8..3d3b355f9fef7d5b6b57521317f67041e0fb1ad6 100644 --- a/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp -@@ -745,7 +745,7 @@ struct InequalityConstraints { +@@ -835,7 +835,7 @@ struct InequalityConstraints { /// /// @param inequality_constraints The list of InequalityConstraints to /// concatenate. 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 da79024ea8..c4101f404a 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 @@ -1,14 +1,14 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 24 Feb 2025 15:12:03 -0800 -Subject: [PATCH 8/9] Suppress GCC 12 warning false positive +Subject: [PATCH 08/10] Suppress GCC 12 warning false positive --- include/sleipnir/autodiff/variable_matrix.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 88122498f384a68b2537b24e57c4a291a951e6dd..10b3a4cc5ab22e93fe4e0b44d4664adce9228867 100644 +index 3eeaccac274cf38d554ea834b3ff666615939382..38b90b4a1c784cfd2fd806536d001db9335ebe2d 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 new file mode 100644 index 0000000000..5e871cb648 --- /dev/null +++ b/upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch @@ -0,0 +1,31 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Tyler Veness +Date: Sun, 29 Mar 2026 16:09:30 -0700 +Subject: [PATCH 09/10] Suppress Doxygen warning false positives + +--- + .../optimization/solver/util/feasibility_restoration.hpp | 4 ++++ + 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 +--- 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 ρ, + return {std::move(p), std::move(n)}; + } + ++// @cond Suppress Doxygen ++ + /// 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( + } + } + ++// @endcond ++ + } // namespace slp + + #include "sleipnir/optimization/solver/interior_point.hpp" diff --git a/upstream_utils/sleipnir_patches/0009-Use-operator-instead-of-multidimensional-array-subsc.patch b/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch similarity index 96% rename from upstream_utils/sleipnir_patches/0009-Use-operator-instead-of-multidimensional-array-subsc.patch rename to upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch index 1513532d7e..d0e46a8fe9 100644 --- a/upstream_utils/sleipnir_patches/0009-Use-operator-instead-of-multidimensional-array-subsc.patch +++ b/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch @@ -1,7 +1,7 @@ From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 12 Apr 2025 16:28:47 -0700 -Subject: [PATCH 9/9] Use operator() instead of multidimensional array +Subject: [PATCH 10/10] Use operator() instead of multidimensional array subscript operator --- @@ -15,10 +15,10 @@ Subject: [PATCH 9/9] 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 5c8d18b41c4025f4492e7216f94299785a513552..b52371b182b755f87f4d038dbeb1795704ab730e 100644 +index 11af6033d1cf00b27cb9ff796c4a7f16b32df90a..d4cc02f54accd568d052ae5709213d45c4c8917a 100644 --- a/include/sleipnir/autodiff/hessian.hpp +++ b/include/sleipnir/autodiff/hessian.hpp -@@ -98,9 +98,9 @@ class Hessian { +@@ -99,9 +99,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 5c8d18b41c4025f4492e7216f94299785a513552..b52371b182b755f87f4d038dbeb17957 } } diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp -index bb3a0319880bd240497e1e27ca26f0105b50b803..082a6cf6782f376cbb6ce05809e62e8021feb321 100644 +index f1915a0d08c509f4eb4d032d4e6a5e6099b68ebc..edd949eefd018af1b1e091b4b2780fc1a0a492cf 100644 --- a/include/sleipnir/autodiff/jacobian.hpp +++ b/include/sleipnir/autodiff/jacobian.hpp @@ -104,9 +104,9 @@ class Jacobian { @@ -47,7 +47,7 @@ index bb3a0319880bd240497e1e27ca26f0105b50b803..082a6cf6782f376cbb6ce05809e62e80 } } diff --git a/include/sleipnir/autodiff/variable.hpp b/include/sleipnir/autodiff/variable.hpp -index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29cd55d91d 100644 +index 3d3b355f9fef7d5b6b57521317f67041e0fb1ad6..130a4f057ea08ab0565b54f8a3556bd81fbc6825 100644 --- a/include/sleipnir/autodiff/variable.hpp +++ b/include/sleipnir/autodiff/variable.hpp @@ -68,7 +68,7 @@ class Variable : public SleipnirBase { @@ -59,7 +59,7 @@ index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29 slp_assert(value.rows() == 1 && value.cols() == 1); } -@@ -635,7 +635,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -725,7 +725,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) { for (int row = 0; row < rhs.rows(); ++row) { for (int col = 0; col < rhs.cols(); ++col) { // Make right-hand side zero @@ -68,7 +68,7 @@ index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29 } } -@@ -651,7 +651,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -741,7 +741,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) { for (int row = 0; row < lhs.rows(); ++row) { for (int col = 0; col < lhs.cols(); ++col) { // Make right-hand side zero @@ -77,7 +77,7 @@ index 2091f339744f8f8980ccb1e44b0b54b06eb32418..7616c02b456c755a56f68de86a755d29 } } -@@ -669,7 +669,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) { +@@ -759,7 +759,7 @@ auto make_constraints(LHS&& lhs, RHS&& rhs) { for (int row = 0; row < lhs.rows(); ++row) { for (int col = 0; col < lhs.cols(); ++col) { // Make right-hand side zero @@ -388,7 +388,7 @@ index 4018606df45941b578c861caf934495f8c9e368e..cf554832b82adb17b4b1d7b56842a77d } diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp -index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08739318cc 100644 +index 38b90b4a1c784cfd2fd806536d001db9335ebe2d..e9f90fb12d230e40497e8557b985ff9c82ebaaeb 100644 --- a/include/sleipnir/autodiff/variable_matrix.hpp +++ b/include/sleipnir/autodiff/variable_matrix.hpp @@ -154,7 +154,7 @@ class VariableMatrix : public SleipnirBase { @@ -772,7 +772,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08 } } -@@ -1253,7 +1253,7 @@ VariableMatrix cwise_reduce( +@@ -1269,7 +1269,7 @@ VariableMatrix cwise_reduce( for (int row = 0; row < lhs.rows(); ++row) { for (int col = 0; col < lhs.cols(); ++col) { @@ -781,7 +781,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08 } } -@@ -1386,17 +1386,17 @@ VariableMatrix solve(const VariableMatrix& A, +@@ -1402,17 +1402,17 @@ VariableMatrix solve(const VariableMatrix& A, if (A.rows() == 1 && A.cols() == 1) { // Compute optimal inverse instead of using Eigen's general solver @@ -804,7 +804,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08 VariableMatrix adj_A{{d, -b}, {-c, a}}; auto det_A = a * d - b * c; -@@ -1413,15 +1413,15 @@ VariableMatrix solve(const VariableMatrix& A, +@@ -1429,15 +1429,15 @@ VariableMatrix solve(const VariableMatrix& A, // // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%7D%2C+%7Bd%2C+e%2C+f%7D%2C+%7Bg%2C+h%2C+i%7D%7D @@ -829,7 +829,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08 auto ae = a * e; auto af = a * f; -@@ -1461,22 +1461,22 @@ VariableMatrix solve(const VariableMatrix& A, +@@ -1477,22 +1477,22 @@ VariableMatrix solve(const VariableMatrix& A, // // https://www.wolframalpha.com/input?i=inverse+%7B%7Ba%2C+b%2C+c%2C+d%7D%2C+%7Be%2C+f%2C+g%2C+h%7D%2C+%7Bi%2C+j%2C+k%2C+l%7D%2C+%7Bm%2C+n%2C+o%2C+p%7D%7D @@ -868,7 +868,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08 auto afk = a * f * k; auto afl = a * f * l; -@@ -1607,14 +1607,14 @@ VariableMatrix solve(const VariableMatrix& A, +@@ -1623,14 +1623,14 @@ VariableMatrix solve(const VariableMatrix& A, MatrixXv eigen_A{A.rows(), A.cols()}; for (int row = 0; row < A.rows(); ++row) { for (int col = 0; col < A.cols(); ++col) { @@ -885,7 +885,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08 } } -@@ -1623,7 +1623,7 @@ VariableMatrix solve(const VariableMatrix& A, +@@ -1639,7 +1639,7 @@ VariableMatrix solve(const VariableMatrix& A, VariableMatrix X{detail::empty, A.cols(), B.cols()}; for (int row = 0; row < X.rows(); ++row) { for (int col = 0; col < X.cols(); ++col) { @@ -895,7 +895,7 @@ index 10b3a4cc5ab22e93fe4e0b44d4664adce9228867..d90645910d1db545fb34a726aa801e08 } diff --git a/include/sleipnir/optimization/ocp.hpp b/include/sleipnir/optimization/ocp.hpp -index 091d985234ac8be5c177b9269c7608036f0daddb..e906c194e93a9e26b9cb5c20b720b87828463e80 100644 +index 5e20b9b8debc13611d5d719b589d8fd896c35a90..e5163f9c2dbbfa0a580d029e61c741e6c9d00fd9 100644 --- a/include/sleipnir/optimization/ocp.hpp +++ b/include/sleipnir/optimization/ocp.hpp @@ -123,7 +123,7 @@ class OCP : public Problem { @@ -959,10 +959,10 @@ index 091d985234ac8be5c177b9269c7608036f0daddb..e906c194e93a9e26b9cb5c20b720b878 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 70955fdc7148e5af737d3094a5602024df790b3d..4243d13c9d04eeee948f8e0e7a21d72b6ef0eaa6 100644 +index 6acd2f655a4b35d087ff365c2a2917b015b03b32..e05ef004e999787a923860b2d173702e3ca4045a 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp -@@ -92,7 +92,7 @@ class Problem { +@@ -97,7 +97,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 70955fdc7148e5af737d3094a5602024df790b3d..4243d13c9d04eeee948f8e0e7a21d72b } } -@@ -125,8 +125,8 @@ class Problem { +@@ -132,8 +132,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/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp index 0e6f31bf77..10e20f5e2f 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 @@ -1472,6 +1472,165 @@ ExpressionPtr log10(const ExpressionPtr& x) { return make_expression_ptr>(x); } +/// Derived expression type for max(). +/// +/// Returns the greater of a and b. If the values are equivalent, returns a. +/// +/// @tparam Scalar Scalar type. +template +struct MaxExpression final : Expression { + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. + constexpr MaxExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + Scalar value(Scalar a, Scalar b) const override { + using std::max; + return max(a, b); + } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + std::string_view name() const override { return "max"; } + + Scalar grad_l(Scalar a, Scalar b, Scalar parent_adjoint) const override { + if (a >= b) { + return parent_adjoint; + } else { + return Scalar(0); + } + } + + Scalar grad_r(Scalar a, Scalar b, Scalar parent_adjoint) const override { + if (b > a) { + return parent_adjoint; + } else { + return Scalar(0); + } + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& a, const ExpressionPtr& b, + const ExpressionPtr& parent_adjoint) const override { + if (a->val >= b->val) { + return parent_adjoint; + } else { + return constant_ptr(Scalar(0)); + } + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& a, const ExpressionPtr& b, + const ExpressionPtr& parent_adjoint) const override { + if (b->val > a->val) { + return parent_adjoint; + } else { + return constant_ptr(Scalar(0)); + } + } +}; + +/// max() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param a The a argument. +/// @param b The b argument. +template +ExpressionPtr max(const ExpressionPtr& a, + const ExpressionPtr& b) { + using enum ExpressionType; + using std::max; + + // Evaluate constant + if (a->type() == CONSTANT && b->type() == CONSTANT) { + return constant_ptr(max(a->val, b->val)); + } + + return make_expression_ptr>(a, b); +} + +/// Derived expression type for min(). +/// +/// Returns the lesser of a and b. If the values are equivalent, returns a. +/// +/// @tparam Scalar Scalar type. +template +struct MinExpression final : Expression { + /// Constructs a binary expression (an operator with two arguments). + /// + /// @param lhs Binary operator's left operand. + /// @param rhs Binary operator's right operand. + constexpr MinExpression(ExpressionPtr lhs, ExpressionPtr rhs) + : Expression{std::move(lhs), std::move(rhs)} {} + + Scalar value(Scalar a, Scalar b) const override { + using std::min; + return min(a, b); + } + + ExpressionType type() const override { return ExpressionType::NONLINEAR; } + + std::string_view name() const override { return "min"; } + + Scalar grad_l(Scalar a, Scalar b, Scalar parent_adjoint) const override { + if (a <= b) { + return parent_adjoint; + } else { + return Scalar(0); + } + } + + Scalar grad_r([[maybe_unused]] Scalar a, [[maybe_unused]] Scalar b, + Scalar parent_adjoint) const override { + if (b < a) { + return parent_adjoint; + } else { + return Scalar(0); + } + } + + ExpressionPtr grad_expr_l( + const ExpressionPtr& a, const ExpressionPtr& b, + const ExpressionPtr& parent_adjoint) const override { + if (a->val <= b->val) { + return parent_adjoint; + } else { + return constant_ptr(Scalar(0)); + } + } + + ExpressionPtr grad_expr_r( + const ExpressionPtr& a, const ExpressionPtr& b, + const ExpressionPtr& parent_adjoint) const override { + if (b->val < a->val) { + return parent_adjoint; + } else { + return constant_ptr(Scalar(0)); + } + } +}; + +/// min() for Expressions. +/// +/// @tparam Scalar Scalar type. +/// @param a The a argument. +/// @param b The b argument. +template +ExpressionPtr min(const ExpressionPtr& a, + const ExpressionPtr& b) { + using enum ExpressionType; + using std::min; + + // Evaluate constant + if (a->type() == CONSTANT && b->type() == CONSTANT) { + return constant_ptr(min(a->val, b->val)); + } + + return make_expression_ptr>(a, b); +} + template ExpressionPtr pow(const ExpressionPtr& base, const ExpressionPtr& power); diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp index bb542696fa..387c53ae3f 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp @@ -10,7 +10,7 @@ namespace slp::detail { -/// Generate a topological sort of an expression graph from parent to child. +/// Generates a topological sort of an expression graph from parent to child. /// /// https://en.wikipedia.org/wiki/Topological_sorting /// @@ -68,7 +68,7 @@ gch::small_vector*> topological_sort( return list; } -/// Update the values of all nodes in this graph based on the values of +/// Updates the values of all nodes in this graph based on the values of /// their dependent nodes. /// /// @tparam Scalar Scalar type. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp index 8a2ae45780..1957fc0339 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp @@ -28,6 +28,8 @@ enum class ExpressionType : uint8_t { } // namespace slp +// @cond Suppress Doxygen + /// Formatter for ExpressionType. template <> struct fmt::formatter { @@ -39,14 +41,15 @@ struct fmt::formatter { return m_underlying.parse(ctx); } - /// Format ExpressionType. + /// Formats ExpressionType. /// /// @tparam FmtContext Format context type. /// @param type Expression type. /// @param ctx Format context. /// @return Format context iterator. template - auto format(const slp::ExpressionType& type, FmtContext& ctx) const { + constexpr auto format(const slp::ExpressionType& type, + FmtContext& ctx) const { using enum slp::ExpressionType; switch (type) { @@ -68,3 +71,5 @@ struct fmt::formatter { private: fmt::formatter m_underlying; }; + +// @endcond 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 d21c1b5ad1..c0e3c16134 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 @@ -34,7 +34,7 @@ class GradientExpressionGraph { } } - /// Update the values of all nodes in this graph based on the values of their + /// Updates the values of all nodes in this graph based on the values of their /// dependent nodes. void update_values() { detail::update_values(m_top_list); } @@ -174,10 +174,10 @@ class GradientExpressionGraph { } private: - // Topological sort of graph from parent to child + /// Topological sort of graph from parent to child gch::small_vector*> m_top_list; - // List that maps nodes to their respective column + /// List that maps nodes to their respective column gch::small_vector m_col_list; }; 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 b52371b182..d4cc02f54a 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 @@ -40,8 +40,9 @@ class Hessian { /// @param wrt Vector of variables with respect to which to compute the /// Hessian. Hessian(Variable variable, SleipnirMatrixLike auto wrt) - : m_variables{detail::GradientExpressionGraph{variable} - .generate_tree(wrt)}, + : m_variables{ + detail::GradientExpressionGraph{variable}.generate_tree( + wrt)}, m_wrt{wrt} { slp_assert(m_wrt.cols() == 1); @@ -71,7 +72,7 @@ class Hessian { m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt); } 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(). + // rows to be recomputed in value(). m_nonlinear_rows.emplace_back(row); } } @@ -149,7 +150,7 @@ class Hessian { gch::small_vector> m_cached_triplets; // List of row indices for nonlinear rows whose graients will be computed in - // Value() + // value() gch::small_vector m_nonlinear_rows; }; 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 082a6cf678..edd949eefd 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 @@ -80,7 +80,7 @@ class Jacobian { m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt); } 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(). + // rows to be recomputed in value(). m_nonlinear_rows.emplace_back(row); } } @@ -148,11 +148,11 @@ class Jacobian { Eigen::SparseMatrix m_J{m_variables.rows(), m_wrt.rows()}; - // Cached triplets for gradients of linear rows + /// Cached triplets for gradients of linear rows gch::small_vector> m_cached_triplets; - // List of row indices for nonlinear rows whose graients will be computed in - // Value() + /// List of row indices for nonlinear rows whose graients will be computed in + /// value() gch::small_vector m_nonlinear_rows; }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp index 7616c02b45..130a4f057e 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp @@ -311,6 +311,24 @@ class Variable : public SleipnirBase { template friend Variable log10(const Variable& x); template + friend Variable max(const ScalarLike auto& a, + const Variable& b); + template + friend Variable max(const Variable& a, + const ScalarLike auto& b); + template + friend Variable max(const Variable& a, + const Variable& b); + template + friend Variable min(const ScalarLike auto& a, + const Variable& b); + template + friend Variable min(const Variable& a, + const ScalarLike auto& b); + template + friend Variable min(const Variable& a, + const Variable& b); + template friend Variable pow(const ScalarLike auto& base, const Variable& power); template @@ -513,6 +531,78 @@ Variable log10(const Variable& x) { return Variable{detail::log10(x.expr)}; } +/// max() for Variables. +/// +/// Returns the greater of a and b. If the values are equivalent, returns a. +/// +/// @tparam Scalar Scalar type. +/// @param a The a argument. +/// @param b The b argument. +template +Variable max(const ScalarLike auto& a, const Variable& b) { + return Variable{detail::max(Variable(a).expr, b.expr)}; +} + +/// max() for Variables. +/// +/// Returns the greater of a and b. If the values are equivalent, returns a. +/// +/// @tparam Scalar Scalar type. +/// @param a The a argument. +/// @param b The b argument. +template +Variable max(const Variable& a, const ScalarLike auto& b) { + return Variable{detail::max(a.expr, Variable(b).expr)}; +} + +/// max() for Variables. +/// +/// Returns the greater of a and b. If the values are equivalent, returns a. +/// +/// @tparam Scalar Scalar type. +/// @param a The a argument. +/// @param b The b argument. +template +Variable max(const Variable& a, const Variable& b) { + return Variable{detail::max(a.expr, b.expr)}; +} + +/// min() for Variables. +/// +/// Returns the lesser of a and b. If the values are equivalent, returns a. +/// +/// @tparam Scalar Scalar type. +/// @param a The a argument. +/// @param b The b argument. +template +Variable min(const ScalarLike auto& a, const Variable& b) { + return Variable{detail::min(Variable(a).expr, b.expr)}; +} + +/// min() for Variables. +/// +/// Returns the lesser of a and b. If the values are equivalent, returns a. +/// +/// @tparam Scalar Scalar type. +/// @param a The a argument. +/// @param b The b argument. +template +Variable min(const Variable& a, const ScalarLike auto& b) { + return Variable{detail::min(a.expr, Variable(b).expr)}; +} + +/// min() for Variables. +/// +/// Returns the lesser of a and b. If the values are equivalent, returns a. +/// +/// @tparam Scalar Scalar type. +/// @param a The a argument. +/// @param b The b argument. +template +Variable min(const Variable& a, const Variable& b) { + return Variable{detail::min(a.expr, b.expr)}; +} + /// pow() for Variables. /// /// @tparam Scalar Scalar type. 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 d90645910d..e9f90fb12d 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 @@ -1211,7 +1211,7 @@ class VariableMatrix : public SleipnirBase { /// @param rows The number of matrix rows. /// @param cols The number of matrix columns. /// @return A variable matrix filled with ones. - static VariableMatrix ones(int rows, int cols) { + static VariableMatrix one(int rows, int cols) { VariableMatrix result{detail::empty, rows, cols}; for (auto& elem : result) { @@ -1221,6 +1221,22 @@ class VariableMatrix : public SleipnirBase { return result; } + /// Returns a variable matrix filled with a constant. + /// + /// @param rows The number of matrix rows. + /// @param cols The number of matrix columns. + /// @param constant The constant. + /// @return A variable matrix filled with a constant. + static VariableMatrix constant(int rows, int cols, Scalar constant) { + VariableMatrix result{detail::empty, rows, cols}; + + for (auto& elem : result) { + elem = constant; + } + + return result; + } + private: gch::small_vector> m_storage; int m_rows = 0; @@ -1260,7 +1276,7 @@ VariableMatrix cwise_reduce( return result; } -/// Assemble a VariableMatrix from a nested list of blocks. +/// Assembles a VariableMatrix from a nested list of blocks. /// /// Each row's blocks must have the same height, and the assembled block rows /// must have the same width. For example, for the block matrix [[A, B], [C]] to @@ -1315,7 +1331,7 @@ VariableMatrix block( return result; } -/// Assemble a VariableMatrix from a nested list of blocks. +/// Assembles a VariableMatrix from a nested list of blocks. /// /// Each row's blocks must have the same height, and the assembled block rows /// must have the same width. For example, for the block matrix [[A, B], [C]] to diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp index e906c194e9..e5163f9c2d 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp @@ -48,7 +48,7 @@ namespace slp { template class OCP : public Problem { public: - /// Build an optimization problem using a system evolution function (explicit + /// Builds an optimization problem using a system evolution function (explicit /// ODE or discrete state transition function). /// /// @param num_states The number of system states. @@ -87,7 +87,7 @@ class OCP : public Problem { timestep_method, transcription_method} {} - /// Build an optimization problem using a system evolution function (explicit + /// Builds an optimization problem using a system evolution function (explicit /// ODE or discrete state transition function). /// /// @param num_states The number of system states. @@ -156,7 +156,7 @@ class OCP : public Problem { } } - /// Utility function to constrain the initial state. + /// Constrains the initial state. /// /// @param initial_state the initial state to constrain to. template @@ -165,7 +165,7 @@ class OCP : public Problem { this->subject_to(this->initial_state() == initial_state); } - /// Utility function to constrain the final state. + /// Constrains the final state. /// /// @param final_state the final state to constrain to. template @@ -174,7 +174,7 @@ class OCP : public Problem { this->subject_to(this->final_state() == final_state); } - /// Set the constraint evaluation function. This function is called + /// Sets the constraint evaluation function. This function is called /// `num_steps+1` times, with the corresponding state and input /// VariableMatrices. /// @@ -190,7 +190,7 @@ class OCP : public Problem { } } - /// Set the constraint evaluation function. This function is called + /// Sets the constraint evaluation function. This function is called /// `num_steps+1` times, with the corresponding state and input /// VariableMatrices. /// @@ -213,7 +213,7 @@ class OCP : public Problem { } } - /// Convenience function to set a lower bound on the input. + /// Sets a lower bound on the input. /// /// @param lower_bound The lower bound that inputs must always be above. Must /// be shaped (num_inputs)x1. @@ -225,7 +225,7 @@ class OCP : public Problem { } } - /// Convenience function to set an upper bound on the input. + /// Sets an upper bound on the input. /// /// @param upper_bound The upper bound that inputs must always be below. Must /// be shaped (num_inputs)x1. @@ -237,21 +237,21 @@ class OCP : public Problem { } } - /// Convenience function to set a lower bound on the timestep. + /// Sets a lower bound on the timestep. /// /// @param min_timestep The minimum timestep. void set_min_timestep(std::chrono::duration min_timestep) { this->subject_to(dt() >= min_timestep.count()); } - /// Convenience function to set an upper bound on the timestep. + /// Sets an upper bound on the timestep. /// /// @param max_timestep The maximum timestep. void set_max_timestep(std::chrono::duration max_timestep) { this->subject_to(dt() <= max_timestep.count()); } - /// Get the state variables. After the problem is solved, this will contain + /// Gets the state variables. After the problem is solved, this will contain /// the optimized trajectory. /// /// Shaped (num_states)x(num_steps+1). @@ -259,7 +259,7 @@ class OCP : public Problem { /// @return The state variable matrix. VariableMatrix& X() { return m_X; } - /// Get the input variables. After the problem is solved, this will contain + /// Gets the input variables. After the problem is solved, this will contain /// the inputs corresponding to the optimized trajectory. /// /// Shaped (num_inputs)x(num_steps+1), although the last input step is unused @@ -268,8 +268,8 @@ class OCP : public Problem { /// @return The input variable matrix. VariableMatrix& U() { return m_U; } - /// Get the timestep variables. After the problem is solved, this will contain - /// the timesteps corresponding to the optimized trajectory. + /// Gets the timestep variables. After the problem is solved, this will + /// contain the timesteps corresponding to the optimized trajectory. /// /// Shaped 1x(num_steps+1), although the last timestep is unused in the /// trajectory. @@ -277,12 +277,12 @@ class OCP : public Problem { /// @return The timestep variable matrix. VariableMatrix& dt() { return m_DT; } - /// Convenience function to get the initial state in the trajectory. + /// Gets the initial state in the trajectory. /// /// @return The initial state of the trajectory. VariableMatrix initial_state() { return m_X.col(0); } - /// Convenience function to get the final state in the trajectory. + /// Gets the final state in the trajectory. /// /// @return The final state of the trajectory. VariableMatrix final_state() { return m_X.col(m_num_steps); } @@ -318,7 +318,7 @@ class OCP : public Problem { return x + (k1 + k2 * Scalar(2) + k3 * Scalar(2) + k4) * (dt / Scalar(6)); } - /// Apply direct collocation dynamics constraints. + /// Applies direct collocation dynamics constraints. void constrain_direct_collocation() { slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE); @@ -355,7 +355,7 @@ class OCP : public Problem { } } - /// Apply direct transcription dynamics constraints. + /// Applies direct transcription dynamics constraints. void constrain_direct_transcription() { Variable time{0}; @@ -378,7 +378,7 @@ class OCP : public Problem { } } - /// Apply single shooting dynamics constraints. + /// Applies single shooting dynamics constraints. void constrain_single_shooting() { Variable time{0}; 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 4243d13c9d..e05ef004e9 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 @@ -34,9 +34,10 @@ #include "sleipnir/util/empty.hpp" #include "sleipnir/util/print.hpp" #include "sleipnir/util/print_diagnostics.hpp" -#include "sleipnir/util/setup_profiler.hpp" +#include "sleipnir/util/profiler.hpp" #include "sleipnir/util/spy.hpp" #include "sleipnir/util/symbol_exports.hpp" +#include "sleipnir/util/to_underlying.hpp" namespace slp { @@ -66,10 +67,12 @@ namespace slp { template class Problem { public: - /// Construct the optimization problem. + /// Constructs the optimization problem. Problem() noexcept = default; - /// Create a decision variable in the optimization problem. + /// Creates a decision variable in the optimization problem. + /// + /// Decision variables have an initial value of zero. /// /// @return A decision variable in the optimization problem. [[nodiscard]] @@ -78,7 +81,9 @@ class Problem { return m_decision_variables.back(); } - /// Create a matrix of decision variables in the optimization problem. + /// Creates a matrix of decision variables in the optimization problem. + /// + /// Decision variables have an initial value of zero. /// /// @param rows Number of matrix rows. /// @param cols Number of matrix columns. @@ -99,12 +104,14 @@ class Problem { return vars; } - /// Create a symmetric matrix of decision variables in the optimization + /// Creates a symmetric matrix of decision variables in the optimization /// problem. /// /// Variable instances are reused across the diagonal, which helps reduce /// problem dimensionality. /// + /// Decision variables have an initial value of zero. + /// /// @param rows Number of matrix rows. /// @return A symmetric matrix of decision varaibles in the optimization /// problem. @@ -139,7 +146,9 @@ class Problem { /// will find the closest solution to the initial conditions that's in the /// feasible set. /// - /// @param cost The cost function to minimize. + /// @param cost The cost function to minimize. A 1x1 VariableMatrix will + /// implicitly convert to a Variable, and a non-1x1 VariableMatrix will + /// raise an assertion. void minimize(const Variable& cost) { m_f = cost; } /// Tells the solver to minimize the output of the given cost function. @@ -148,7 +157,9 @@ class Problem { /// will find the closest solution to the initial conditions that's in the /// feasible set. /// - /// @param cost The cost function to minimize. + /// @param cost The cost function to minimize. A 1x1 VariableMatrix will + /// implicitly convert to a Variable, and a non-1x1 VariableMatrix will + /// raise an assertion. void minimize(Variable&& cost) { m_f = std::move(cost); } /// Tells the solver to maximize the output of the given objective function. @@ -157,7 +168,9 @@ class Problem { /// will find the closest solution to the initial conditions that's in the /// feasible set. /// - /// @param objective The objective function to maximize. + /// @param objective The objective function to maximize. A 1x1 VariableMatrix + /// will implicitly convert to a Variable, and a non-1x1 VariableMatrix + /// will raise an assertion. void maximize(const Variable& objective) { // Maximizing a cost function is the same as minimizing its negative m_f = -objective; @@ -169,7 +182,9 @@ class Problem { /// will find the closest solution to the initial conditions that's in the /// feasible set. /// - /// @param objective The objective function to maximize. + /// @param objective The objective function to maximize. A 1x1 VariableMatrix + /// will implicitly convert to a Variable, and a non-1x1 VariableMatrix + /// will raise an assertion. void maximize(Variable&& objective) { // Maximizing a cost function is the same as minimizing its negative m_f = -std::move(objective); @@ -256,7 +271,7 @@ class Problem { } } - /// Solve the optimization problem. The solution will be stored in the + /// Solves the optimization problem. The solution will be stored in the /// original variables used to construct the problem. /// /// @param options Solver options. @@ -311,20 +326,17 @@ class Problem { Gradient g{f, x_ad}; ad_setup_profilers.back().stop(); - [[maybe_unused]] int num_decision_variables = m_decision_variables.size(); - [[maybe_unused]] int num_equality_constraints = m_equality_constraints.size(); - [[maybe_unused]] int num_inequality_constraints = m_inequality_constraints.size(); gch::small_vector& info)>> - callbacks; + iteration_callbacks; for (const auto& callback : m_iteration_callbacks) { - callbacks.emplace_back(callback); + iteration_callbacks.emplace_back(callback); } for (const auto& callback : m_persistent_iteration_callbacks) { - callbacks.emplace_back(callback); + iteration_callbacks.emplace_back(callback); } // Solve the optimization problem @@ -349,28 +361,32 @@ class Problem { H_spy = std::make_unique>( "H.spy", "Hessian", "Decision variables", "Decision variables", num_decision_variables, num_decision_variables); - callbacks.push_back([&](const IterationInfo& info) -> bool { - H_spy->add(info.H); - return false; - }); + iteration_callbacks.push_back( + [&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + return false; + }); } #endif + NewtonMatrixCallbacks matrix_callbacks{ + num_decision_variables, + [&](const DenseVector& x) -> Scalar { + x_ad.set_value(x); + return f.value(); + }, + [&](const DenseVector& x) -> SparseVector { + x_ad.set_value(x); + return g.value(); + }, + [&](const DenseVector& x) -> SparseMatrix { + x_ad.set_value(x); + return H.value(); + }}; + // Invoke Newton solver - status = newton(NewtonMatrixCallbacks{ - [&](const DenseVector& x) -> Scalar { - x_ad.set_value(x); - return f.value(); - }, - [&](const DenseVector& x) -> SparseVector { - x_ad.set_value(x); - return g.value(); - }, - [&](const DenseVector& x) -> SparseMatrix { - x_ad.set_value(x); - return H.value(); - }}, - callbacks, options, x); + status = + newton(matrix_callbacks, iteration_callbacks, options, x); } else if (m_inequality_constraints.empty()) { if (options.diagnostics) { slp::println("\nInvoking SQP solver\n"); @@ -390,6 +406,7 @@ class Problem { // Set up Lagrangian Hessian autodiff ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); Hessian H{L, x_ad}; + Hessian H_c{-y_ad.T() * c_e_ad, x_ad}; ad_setup_profilers.back().stop(); ad_setup_profilers[0].stop(); @@ -407,39 +424,47 @@ class Problem { "A_e.spy", "Equality constraint Jacobian", "Constraints", "Decision variables", num_equality_constraints, num_decision_variables); - callbacks.push_back([&](const IterationInfo& info) -> bool { - H_spy->add(info.H); - A_e_spy->add(info.A_e); - return false; - }); + iteration_callbacks.push_back( + [&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + A_e_spy->add(info.A_e); + return false; + }); } #endif + SQPMatrixCallbacks matrix_callbacks{ + num_decision_variables, + num_equality_constraints, + [&](const DenseVector& x) -> Scalar { + x_ad.set_value(x); + return f.value(); + }, + [&](const DenseVector& x) -> SparseVector { + x_ad.set_value(x); + return g.value(); + }, + [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { + x_ad.set_value(x); + y_ad.set_value(y); + return H.value(); + }, + [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { + x_ad.set_value(x); + y_ad.set_value(y); + return H_c.value(); + }, + [&](const DenseVector& x) -> DenseVector { + x_ad.set_value(x); + return c_e_ad.value(); + }, + [&](const DenseVector& x) -> SparseMatrix { + x_ad.set_value(x); + return A_e.value(); + }}; + // Invoke SQP solver - status = sqp( - SQPMatrixCallbacks{ - [&](const DenseVector& x) -> Scalar { - x_ad.set_value(x); - return f.value(); - }, - [&](const DenseVector& x) -> SparseVector { - x_ad.set_value(x); - return g.value(); - }, - [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { - x_ad.set_value(x); - y_ad.set_value(y); - return H.value(); - }, - [&](const DenseVector& x) -> DenseVector { - x_ad.set_value(x); - return c_e_ad.value(); - }, - [&](const DenseVector& x) -> SparseMatrix { - x_ad.set_value(x); - return A_e.value(); - }}, - callbacks, options, x); + status = sqp(matrix_callbacks, iteration_callbacks, options, x); } else { if (options.diagnostics) { slp::println("\nInvoking IPM solver...\n"); @@ -466,6 +491,8 @@ class Problem { // Set up Lagrangian Hessian autodiff ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); Hessian H{L, x_ad}; + Hessian H_c{-y_ad.T() * c_e_ad - z_ad.T() * c_i_ad, + x_ad}; ad_setup_profilers.back().stop(); ad_setup_profilers[0].stop(); @@ -488,12 +515,13 @@ class Problem { "A_i.spy", "Inequality constraint Jacobian", "Constraints", "Decision variables", num_inequality_constraints, num_decision_variables); - callbacks.push_back([&](const IterationInfo& info) -> bool { - H_spy->add(info.H); - A_e_spy->add(info.A_e); - A_i_spy->add(info.A_i); - return false; - }); + iteration_callbacks.push_back( + [&](const IterationInfo& info) -> bool { + H_spy->add(info.H); + A_e_spy->add(info.A_e); + A_i_spy->add(info.A_i); + return false; + }); } #endif @@ -511,45 +539,57 @@ class Problem { #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION project_onto_bounds(x, bounds); #endif + + InteriorPointMatrixCallbacks matrix_callbacks{ + num_decision_variables, + num_equality_constraints, + num_inequality_constraints, + [&](const DenseVector& x) -> Scalar { + x_ad.set_value(x); + return f.value(); + }, + [&](const DenseVector& x) -> SparseVector { + x_ad.set_value(x); + return 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.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_c.value(); + }, + [&](const DenseVector& x) -> DenseVector { + x_ad.set_value(x); + return c_e_ad.value(); + }, + [&](const DenseVector& x) -> SparseMatrix { + x_ad.set_value(x); + return A_e.value(); + }, + [&](const DenseVector& x) -> DenseVector { + x_ad.set_value(x); + return c_i_ad.value(); + }, + [&](const DenseVector& x) -> SparseMatrix { + x_ad.set_value(x); + return A_i.value(); + }}; + // Invoke interior-point method solver - status = interior_point( - InteriorPointMatrixCallbacks{ - [&](const DenseVector& x) -> Scalar { - x_ad.set_value(x); - return f.value(); - }, - [&](const DenseVector& x) -> SparseVector { - x_ad.set_value(x); - return 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.value(); - }, - [&](const DenseVector& x) -> DenseVector { - x_ad.set_value(x); - return c_e_ad.value(); - }, - [&](const DenseVector& x) -> SparseMatrix { - x_ad.set_value(x); - return A_e.value(); - }, - [&](const DenseVector& x) -> DenseVector { - x_ad.set_value(x); - return c_i_ad.value(); - }, - [&](const DenseVector& x) -> SparseMatrix { - x_ad.set_value(x); - return A_i.value(); - }}, - callbacks, options, + status = + interior_point(matrix_callbacks, iteration_callbacks, options, #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION - bound_constraint_mask, + bound_constraint_mask, #endif - x); + x); } if (options.diagnostics) { @@ -657,11 +697,11 @@ class Problem { // Print problem structure slp::println("\nProblem structure:"); slp::println(" ↳ {} cost function", - types[static_cast(cost_function_type())]); + types[slp::to_underlying(cost_function_type())]); slp::println(" ↳ {} equality constraints", - types[static_cast(equality_constraint_type())]); + types[slp::to_underlying(equality_constraint_type())]); slp::println(" ↳ {} inequality constraints", - types[static_cast(inequality_constraint_type())]); + types[slp::to_underlying(inequality_constraint_type())]); if (m_decision_variables.size() == 1) { slp::print("\n1 decision variable\n"); @@ -673,7 +713,7 @@ class Problem { [](const gch::small_vector>& constraints) { std::array counts{}; for (const auto& constraint : constraints) { - ++counts[static_cast(constraint.type())]; + ++counts[slp::to_underlying(constraint.type())]; } for (size_t i = 0; i < counts.size(); ++i) { constexpr std::array names{"empty", "constant", "linear", diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp index 4d43cea244..ee9bfbc763 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp @@ -25,11 +25,12 @@ enum class ExitStatus : int8_t { GLOBALLY_INFEASIBLE = -3, /// The linear system factorization failed. FACTORIZATION_FAILED = -4, - /// The backtracking line search failed, and the problem isn't locally - /// infeasible. - LINE_SEARCH_FAILED = -5, - /// The solver encountered nonfinite initial cost or constraints and gave up. - NONFINITE_INITIAL_COST_OR_CONSTRAINTS = -6, + /// The solver failed to reach the desired tolerance, and feasibility + /// restoration failed to converge. + FEASIBILITY_RESTORATION_FAILED = -5, + /// The solver encountered nonfinite initial cost, constraints, or derivatives + /// and gave up. + NONFINITE_INITIAL_GUESS = -6, /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up. DIVERGING_ITERATES = -7, /// The solver returned its solution so far after exceeding the maximum number @@ -42,10 +43,12 @@ enum class ExitStatus : int8_t { } // namespace slp +// @cond Suppress Doxygen + /// Formatter for ExitStatus. template <> struct fmt::formatter { - /// Parse format string. + /// Parses format string. /// /// @param ctx Format parse context. /// @return Format parse context iterator. @@ -53,14 +56,15 @@ struct fmt::formatter { return m_underlying.parse(ctx); } - /// Format ExitStatus. + /// Formats ExitStatus. /// /// @tparam FmtContext Format context type. /// @param exit_status Exit status. /// @param ctx Format context. /// @return Format context iterator. template - auto format(const slp::ExitStatus& exit_status, FmtContext& ctx) const { + constexpr auto format(const slp::ExitStatus& exit_status, + FmtContext& ctx) const { using enum slp::ExitStatus; switch (exit_status) { @@ -76,11 +80,11 @@ struct fmt::formatter { return m_underlying.format("globally infeasible", ctx); case FACTORIZATION_FAILED: return m_underlying.format("factorization failed", ctx); - case LINE_SEARCH_FAILED: - return m_underlying.format("line search failed", ctx); - case NONFINITE_INITIAL_COST_OR_CONSTRAINTS: - return m_underlying.format("nonfinite initial cost or constraints", - ctx); + case FEASIBILITY_RESTORATION_FAILED: + return m_underlying.format("feasibility restoration failed", ctx); + case NONFINITE_INITIAL_GUESS: + return m_underlying.format( + "nonfinite initial cost, constraints, or derivatives", ctx); case DIVERGING_ITERATES: return m_underlying.format("diverging iterates", ctx); case MAX_ITERATIONS_EXCEEDED: @@ -95,3 +99,5 @@ struct fmt::formatter { private: fmt::formatter m_underlying; }; + +// @endcond 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 57cc004129..9f9cdc2560 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 @@ -17,7 +17,9 @@ #include "sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp" #include "sleipnir/optimization/solver/iteration_info.hpp" #include "sleipnir/optimization/solver/options.hpp" -#include "sleipnir/optimization/solver/util/error_estimate.hpp" +#include "sleipnir/optimization/solver/util/all_finite.hpp" +#include "sleipnir/optimization/solver/util/append_as_triplets.hpp" +#include "sleipnir/optimization/solver/util/feasibility_restoration.hpp" #include "sleipnir/optimization/solver/util/filter.hpp" #include "sleipnir/optimization/solver/util/fraction_to_the_boundary_rule.hpp" #include "sleipnir/optimization/solver/util/is_locally_infeasible.hpp" @@ -25,9 +27,8 @@ #include "sleipnir/optimization/solver/util/regularized_ldlt.hpp" #include "sleipnir/util/assert.hpp" #include "sleipnir/util/print_diagnostics.hpp" +#include "sleipnir/util/profiler.hpp" #include "sleipnir/util/scope_exit.hpp" -#include "sleipnir/util/scoped_profiler.hpp" -#include "sleipnir/util/solve_profiler.hpp" #include "sleipnir/util/symbol_exports.hpp" // See docs/algorithms.md#Works_cited for citation definitions. @@ -70,17 +71,78 @@ ExitStatus interior_point( #endif Eigen::Vector& x) { using DenseVector = Eigen::Vector; + + DenseVector s = + DenseVector::Ones(matrix_callbacks.num_inequality_constraints); + DenseVector y = DenseVector::Zero(matrix_callbacks.num_equality_constraints); + DenseVector z = + DenseVector::Ones(matrix_callbacks.num_inequality_constraints); + Scalar μ(0.1); + + return interior_point(matrix_callbacks, iteration_callbacks, options, false, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + bound_constraint_mask, +#endif + x, s, y, z, μ); +} + +/// Finds the optimal solution to a nonlinear program using the interior-point +/// method. +/// +/// A nonlinear program has the form: +/// +/// ``` +/// min_x f(x) +/// subject to cₑ(x) = 0 +/// cᵢ(x) ≥ 0 +/// ``` +/// +/// where f(x) is the cost function, cₑ(x) are the equality constraints, and +/// cᵢ(x) are the inequality constraints. +/// +/// @tparam Scalar Scalar type. +/// @param[in] matrix_callbacks Matrix callbacks. +/// @param[in] iteration_callbacks The list of callbacks to call at the +/// beginning of each iteration. +/// @param[in] options Solver options. +/// @param[in] in_feasibility_restoration Whether solver is in feasibility +/// restoration mode. +/// @param[in,out] x The initial guess and output location for the decision +/// variables. +/// @param[in,out] s The initial guess and output location for the inequality +/// constraint slack variables. +/// @param[in,out] y The initial guess and output location for the equality +/// constraint dual variables. +/// @param[in,out] z The initial guess and output location for the inequality +/// constraint dual variables. +/// @param[in,out] μ The initial guess and output location for the barrier +/// parameter. +/// @return The exit status. +template +ExitStatus interior_point( + const InteriorPointMatrixCallbacks& matrix_callbacks, + std::span& info)>> + iteration_callbacks, + const Options& options, bool in_feasibility_restoration, +#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& μ) { + using DenseVector = Eigen::Vector; using SparseMatrix = Eigen::SparseMatrix; using SparseVector = Eigen::SparseVector; /// Interior-point method step direction. struct Step { - /// Primal step. + /// Decision variable primal step. DenseVector p_x; + /// Inequality constraint slack variable primal step. + DenseVector p_s; /// Equality constraint dual step. DenseVector p_y; - /// Inequality constraint slack variable step. - DenseVector p_s; /// Inequality constraint dual step. DenseVector p_z; }; @@ -104,6 +166,7 @@ ExitStatus interior_point( solve_profilers.emplace_back(" ↳ f(x)"); solve_profilers.emplace_back(" ↳ ∇f(x)"); solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓ(yᵀcₑ + zᵀcᵢ)"); solve_profilers.emplace_back(" ↳ cₑ(x)"); solve_profilers.emplace_back(" ↳ ∂cₑ/∂x"); solve_profilers.emplace_back(" ↳ cᵢ(x)"); @@ -126,12 +189,16 @@ ExitStatus interior_point( auto& f_prof = solve_profilers[11]; auto& g_prof = solve_profilers[12]; auto& H_prof = solve_profilers[13]; - auto& c_e_prof = solve_profilers[14]; - auto& A_e_prof = solve_profilers[15]; - auto& c_i_prof = solve_profilers[16]; - auto& A_i_prof = solve_profilers[17]; + auto& H_c_prof = solve_profilers[14]; + auto& c_e_prof = solve_profilers[15]; + auto& A_e_prof = solve_profilers[16]; + auto& c_i_prof = solve_profilers[17]; + auto& A_i_prof = solve_profilers[18]; InteriorPointMatrixCallbacks matrices{ + matrix_callbacks.num_decision_variables, + matrix_callbacks.num_equality_constraints, + matrix_callbacks.num_inequality_constraints, [&](const DenseVector& x) -> Scalar { ScopedProfiler prof{f_prof}; return matrix_callbacks.f(x); @@ -145,6 +212,11 @@ ExitStatus interior_point( ScopedProfiler prof{H_prof}; return matrix_callbacks.H(x, y, z); }, + [&](const DenseVector& x, const DenseVector& y, + const DenseVector& z) -> SparseMatrix { + ScopedProfiler prof{H_c_prof}; + return matrix_callbacks.H_c(x, y, z); + }, [&](const DenseVector& x) -> DenseVector { ScopedProfiler prof{c_e_prof}; return matrix_callbacks.c_e(x); @@ -169,15 +241,26 @@ ExitStatus interior_point( setup_prof.start(); Scalar f = matrices.f(x); + SparseVector g = matrices.g(x); + SparseMatrix H = matrices.H(x, y, z); DenseVector c_e = matrices.c_e(x); + SparseMatrix A_e = matrices.A_e(x); DenseVector c_i = matrices.c_i(x); + SparseMatrix A_i = matrices.A_i(x); - int num_decision_variables = x.rows(); - int num_equality_constraints = c_e.rows(); - int num_inequality_constraints = c_i.rows(); + // Ensure matrix callback dimensions are consistent + slp_assert(g.rows() == matrices.num_decision_variables); + slp_assert(H.rows() == matrices.num_decision_variables); + slp_assert(H.cols() == matrices.num_decision_variables); + slp_assert(c_e.rows() == matrices.num_equality_constraints); + slp_assert(A_e.rows() == matrices.num_equality_constraints); + slp_assert(A_e.cols() == matrices.num_decision_variables); + slp_assert(c_i.rows() == matrices.num_inequality_constraints); + slp_assert(A_i.rows() == matrices.num_inequality_constraints); + slp_assert(A_i.cols() == matrices.num_decision_variables); // Check for overconstrained problem - if (num_equality_constraints > num_decision_variables) { + if (matrices.num_equality_constraints > matrices.num_decision_variables) { if (options.diagnostics) { print_too_few_dofs_error(c_e); } @@ -185,52 +268,33 @@ ExitStatus interior_point( return ExitStatus::TOO_FEW_DOFS; } - SparseVector g = matrices.g(x); - SparseMatrix A_e = matrices.A_e(x); - SparseMatrix A_i = matrices.A_i(x); + // Check whether initial guess has finite cost, constraints, and derivatives + if (!isfinite(f) || !all_finite(g) || !all_finite(H) || !c_e.allFinite() || + !all_finite(A_e) || !c_i.allFinite() || !all_finite(A_i)) { + return ExitStatus::NONFINITE_INITIAL_GUESS; + } - DenseVector s = DenseVector::Ones(num_inequality_constraints); #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION // We set sʲ = cᵢʲ(x) for each bound inequality constraint index j s = bound_constraint_mask.select(c_i, s); #endif - DenseVector y = DenseVector::Zero(num_equality_constraints); - DenseVector z = DenseVector::Ones(num_inequality_constraints); - - SparseMatrix H = matrices.H(x, y, z); - - // Ensure matrix callback dimensions are consistent - slp_assert(g.rows() == num_decision_variables); - slp_assert(A_e.rows() == num_equality_constraints); - slp_assert(A_e.cols() == num_decision_variables); - slp_assert(A_i.rows() == num_inequality_constraints); - slp_assert(A_i.cols() == num_decision_variables); - slp_assert(H.rows() == num_decision_variables); - slp_assert(H.cols() == num_decision_variables); - - // Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ) - if (!isfinite(f) || !c_e.allFinite() || !c_i.allFinite()) { - return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; - } int iterations = 0; // Barrier parameter minimum const Scalar μ_min = Scalar(options.tolerance) / Scalar(10); - // Barrier parameter μ - Scalar μ(0.1); - // Fraction-to-the-boundary rule scale factor minimum constexpr Scalar τ_min(0.99); // Fraction-to-the-boundary rule scale factor τ Scalar τ = τ_min; - Filter filter; + Filter filter{c_e.template lpNorm<1>() + + (c_i - s).template lpNorm<1>()}; - // This should be run when the error estimate is below a desired threshold for - // the current barrier parameter + // This should be run when the error is below a desired threshold for the + // current barrier parameter auto update_barrier_parameter_and_reset_filter = [&] { // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1). constexpr Scalar κ_μ(0.2); @@ -261,8 +325,11 @@ ExitStatus interior_point( // Kept outside the loop so its storage can be reused gch::small_vector> triplets; - RegularizedLDLT solver{num_decision_variables, - num_equality_constraints}; + // Constraint regularization is forced to zero in feasibility restoration + // because the equality constraint Jacobian cannot be rank-deficient + RegularizedLDLT solver{ + matrices.num_decision_variables, matrices.num_equality_constraints, + in_feasibility_restoration ? Scalar(0) : Scalar(1e-10)}; // Variables for determining when a step is acceptable constexpr Scalar α_reduction_factor(0.5); @@ -270,7 +337,7 @@ ExitStatus interior_point( int full_step_rejected_counter = 0; - // Error estimate + // Error Scalar E_0 = std::numeric_limits::infinity(); setup_prof.stop(); @@ -279,6 +346,11 @@ ExitStatus interior_point( scope_exit exit{[&] { if (options.diagnostics) { solver_prof.stop(); + + if (in_feasibility_restoration) { + return; + } + if (iterations > 0) { print_bottom_iteration_diagnostics(); } @@ -319,7 +391,7 @@ ExitStatus interior_point( // Call iteration callbacks for (const auto& callback : iteration_callbacks) { - if (callback({iterations, x, g, H, A_e, A_i})) { + if (callback({iterations, x, s, y, z, g, H, A_e, A_i})) { return ExitStatus::CALLBACK_REQUESTED_STOP; } } @@ -340,18 +412,10 @@ ExitStatus interior_point( H + (A_i.transpose() * Σ * A_i).template triangularView(); triplets.clear(); triplets.reserve(top_left.nonZeros() + A_e.nonZeros()); - for (int col = 0; col < H.cols(); ++col) { - // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant - for (typename SparseMatrix::InnerIterator it{top_left, col}; it; ++it) { - triplets.emplace_back(it.row(), it.col(), it.value()); - } - // Append column of Aₑ in bottom-left quadrant - for (typename SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { - triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); - } - } - SparseMatrix lhs(num_decision_variables + num_equality_constraints, - num_decision_variables + num_equality_constraints); + append_as_triplets(triplets, 0, 0, {top_left, A_e}); + SparseMatrix lhs( + matrices.num_decision_variables + matrices.num_equality_constraints, + matrices.num_decision_variables + matrices.num_equality_constraints); lhs.setFromSortedTriplets(triplets.begin(), triplets.end()); // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)] @@ -369,6 +433,7 @@ ExitStatus interior_point( Scalar α_max(1); Scalar α(1); Scalar α_z(1); + bool call_feasibility_restoration = false; // Solve the Newton-KKT system // @@ -402,14 +467,16 @@ ExitStatus interior_point( α_max = fraction_to_the_boundary_rule(s, step.p_s, τ); α = α_max; - // If maximum step size is below minimum, report line search failure + // If maximum step size is below minimum, invoke feasibility restoration if (α < α_min) { - return ExitStatus::LINE_SEARCH_FAILED; + call_feasibility_restoration = true; } // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ) α_z = fraction_to_the_boundary_rule(z, step.p_z, τ); + const FilterEntry current_entry{f, s, c_e, c_i, μ}; + // Loop until a step is accepted while (1) { DenseVector trial_x = x + α * step.p_x; @@ -428,7 +495,8 @@ ExitStatus interior_point( α *= α_reduction_factor; if (α < α_min) { - return ExitStatus::LINE_SEARCH_FAILED; + call_feasibility_restoration = true; + break; } continue; } @@ -445,8 +513,8 @@ ExitStatus interior_point( } // Check whether filter accepts trial iterate - if (filter.try_add(FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ}, - α)) { + 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, α)) { // Accept step break; } @@ -484,7 +552,10 @@ ExitStatus interior_point( step_acceptable ? IterationType::ACCEPTED_SOC : IterationType::REJECTED_SOC, soc_profiler.current_duration(), - error_estimate(g, A_e, trial_c_e, trial_y), trial_f, + kkt_error( + 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(), @@ -531,8 +602,8 @@ ExitStatus interior_point( } // Check whether filter accepts trial iterate - if (filter.try_add( - FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ}, α)) { + 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; @@ -559,7 +630,8 @@ ExitStatus interior_point( // See section 3.2 case I of [2]. if (full_step_rejected_counter >= 4 && filter.max_constraint_violation > - filter.back().constraint_violation / Scalar(10)) { + current_entry.constraint_violation / Scalar(10) && + filter.last_rejection_due_to_filter()) { filter.max_constraint_violation *= Scalar(0.1); filter.reset(); continue; @@ -569,10 +641,10 @@ ExitStatus interior_point( α *= α_reduction_factor; // If step size hit a minimum, check if the KKT error was reduced. If it - // wasn't, report line search failure. + // wasn't, invoke feasibility restoration. if (α < α_min) { - Scalar current_kkt_error = - kkt_error(g, A_e, c_e, A_i, c_i, s, y, z, μ); + Scalar current_kkt_error = kkt_error( + g, A_e, c_e, A_i, c_i, s, y, z, μ); trial_x = x + α_max * step.p_x; trial_s = s + α_max * step.p_s; @@ -583,7 +655,7 @@ ExitStatus interior_point( trial_c_e = matrices.c_e(trial_x); trial_c_i = matrices.c_i(trial_x); - Scalar next_kkt_error = kkt_error( + Scalar next_kkt_error = kkt_error( matrices.g(trial_x), matrices.A_e(trial_x), matrices.c_e(trial_x), matrices.A_i(trial_x), trial_c_i, trial_s, trial_y, trial_z, μ); @@ -595,43 +667,84 @@ ExitStatus interior_point( break; } - return ExitStatus::LINE_SEARCH_FAILED; + call_feasibility_restoration = true; + break; } } line_search_profiler.stop(); - // If full step was accepted, reset full-step rejected counter - if (α == α_max) { - full_step_rejected_counter = 0; - } + if (call_feasibility_restoration) { + // If already in feasibility restoration mode, running it again won't help + if (in_feasibility_restoration) { + return ExitStatus::FEASIBILITY_RESTORATION_FAILED; + } - // 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; + FilterEntry initial_entry{matrices.f(x), s, c_e, c_i, μ}; - // A requirement for the convergence proof is that the primal-dual barrier - // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal - // barrier term Hessian μSₖ₊₁⁻². - // - // Σₖ₊₁ = μSₖ₊₁⁻² - // Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻² - // Zₖ₊₁ = μSₖ₊₁⁻¹ - // - // We ensure this by resetting - // - // zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁) - // - // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. - for (int row = 0; row < z.rows(); ++row) { - constexpr Scalar κ_Σ(1e10); - z[row] = - std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]); + // Feasibility restoration phase + gch::small_vector& info)>> + callbacks; + for (auto& callback : iteration_callbacks) { + callbacks.emplace_back(callback); + } + callbacks.emplace_back([&](const IterationInfo& info) { + DenseVector trial_x = + info.x.segment(0, matrices.num_decision_variables); + DenseVector trial_s = + info.s.segment(0, matrices.num_inequality_constraints); + + DenseVector trial_c_e = matrices.c_e(trial_x); + DenseVector trial_c_i = matrices.c_i(trial_x); + + // If the current iterate sufficiently reduces constraint violation and + // is accepted by the normal filter, stop feasibility restoration + FilterEntry trial_entry{matrices.f(trial_x), trial_s, trial_c_e, + trial_c_i, μ}; + return trial_entry.constraint_violation < + 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, μ); + + if (status != ExitStatus::SUCCESS) { + // Report failure + return status; + } + } 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; + + // A requirement for the convergence proof is that the primal-dual barrier + // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal + // barrier term Hessian μSₖ₊₁⁻². + // + // Σₖ₊₁ = μSₖ₊₁⁻² + // Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻² + // Zₖ₊₁ = μSₖ₊₁⁻¹ + // + // We ensure this by resetting + // + // zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁) + // + // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. + for (int row = 0; row < z.rows(); ++row) { + constexpr Scalar κ_Σ(1e10); + z[row] = + std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]); + } } // Update autodiff for Jacobians and Hessian @@ -646,20 +759,23 @@ ExitStatus interior_point( c_e = matrices.c_e(x); c_i = matrices.c_i(x); - // Update the error estimate - E_0 = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0)); + // Update the error + E_0 = kkt_error( + 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)) { // Barrier parameter scale factor for tolerance checks constexpr Scalar κ_ε(10); - // While the error estimate is below the desired threshold for this - // barrier parameter value, decrease the barrier parameter further - Scalar E_μ = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); + // While the error is below the desired threshold for this barrier + // parameter value, decrease the barrier parameter further + Scalar E_μ = kkt_error( + g, A_e, c_e, A_i, c_i, s, y, z, μ); while (μ > μ_min && E_μ <= κ_ε * μ) { update_barrier_parameter_and_reset_filter(); - E_μ = error_estimate(g, A_e, c_e, A_i, c_i, s, y, z, μ); + E_μ = kkt_error(g, A_e, c_e, A_i, + c_i, s, y, z, μ); } } @@ -668,7 +784,9 @@ ExitStatus interior_point( if (options.diagnostics) { print_iteration_diagnostics( - iterations, IterationType::NORMAL, + iterations, + in_feasibility_restoration ? IterationType::FEASIBILITY_RESTORATION + : 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, 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 b0855cf622..94895dc848 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 @@ -21,6 +21,15 @@ struct InteriorPointMatrixCallbacks { /// Type alias for sparse vector. using SparseVector = Eigen::SparseVector; + /// Number of decision variables. + int num_decision_variables = 0; + + /// Number of equality constraints. + int num_equality_constraints = 0; + + /// Number of inequality constraints. + int num_inequality_constraints = 0; + /// Cost function value f(x) getter. /// /// @@ -65,7 +74,7 @@ struct InteriorPointMatrixCallbacks { /// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter. /// - /// L(xₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀcᵢ(xₖ) + /// L(x, y, z) = f(x) − yᵀcₑ(x) − zᵀcᵢ(x) /// ///
/// @@ -98,6 +107,39 @@ struct InteriorPointMatrixCallbacks { const DenseVector& z)> H; + /// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x) − zᵀcᵢ(x)) getter. + /// + ///
+ /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
ynum_equality_constraints1
znum_inequality_constraints1
∇ₓₓ²(−yᵀcₑ(x) − zᵀcᵢ(x))num_decision_variablesnum_decision_variables
+ std::function + H_c; + /// Equality constraint value cₑ(x) getter. /// /// @@ -122,10 +164,10 @@ struct InteriorPointMatrixCallbacks { /// Equality constraint Jacobian ∂cₑ/∂x getter. /// /// ``` - /// [∇ᵀcₑ₁(xₖ)] - /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)] - /// [ ⋮ ] - /// [∇ᵀcₑₘ(xₖ)] + /// [∇ᵀcₑ₁(x)] + /// Aₑ(x) = [∇ᵀcₑ₂(x)] + /// [ ⋮ ] + /// [∇ᵀcₑₘ(x)] /// ``` /// ///
@@ -171,10 +213,10 @@ struct InteriorPointMatrixCallbacks { /// Inequality constraint Jacobian ∂cᵢ/∂x getter. /// /// ``` - /// [∇ᵀcᵢ₁(xₖ)] - /// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)] - /// [ ⋮ ] - /// [∇ᵀcᵢₘ(xₖ)] + /// [∇ᵀcᵢ₁(x)] + /// Aᵢ(x) = [∇ᵀcᵢ₂(x)] + /// [ ⋮ ] + /// [∇ᵀcᵢₘ(x)] /// ``` /// ///
diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp index 60e9d65d79..8cc6c29397 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp @@ -18,6 +18,15 @@ struct IterationInfo { /// The decision variables. const Eigen::Vector& x; + /// The inequality constraint slack variables. + const Eigen::Vector& s; + + /// The equality constraint dual variables. + const Eigen::Vector& y; + + /// The inequality constraint dual variables. + const Eigen::Vector& z; + /// The gradient of the cost function. const Eigen::SparseVector& g; 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 4a1b4c4d08..875a46ce9e 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 @@ -16,15 +16,14 @@ #include "sleipnir/optimization/solver/iteration_info.hpp" #include "sleipnir/optimization/solver/newton_matrix_callbacks.hpp" #include "sleipnir/optimization/solver/options.hpp" -#include "sleipnir/optimization/solver/util/error_estimate.hpp" +#include "sleipnir/optimization/solver/util/all_finite.hpp" #include "sleipnir/optimization/solver/util/filter.hpp" #include "sleipnir/optimization/solver/util/kkt_error.hpp" #include "sleipnir/optimization/solver/util/regularized_ldlt.hpp" #include "sleipnir/util/assert.hpp" #include "sleipnir/util/print_diagnostics.hpp" +#include "sleipnir/util/profiler.hpp" #include "sleipnir/util/scope_exit.hpp" -#include "sleipnir/util/scoped_profiler.hpp" -#include "sleipnir/util/solve_profiler.hpp" #include "sleipnir/util/symbol_exports.hpp" // See docs/algorithms.md#Works_cited for citation definitions. @@ -94,6 +93,7 @@ ExitStatus newton( auto& H_prof = solve_profilers[11]; NewtonMatrixCallbacks matrices{ + matrix_callbacks.num_decision_variables, [&](const DenseVector& x) -> Scalar { ScopedProfiler prof{f_prof}; return matrix_callbacks.f(x); @@ -114,33 +114,30 @@ ExitStatus newton( setup_prof.start(); Scalar f = matrices.f(x); - - int num_decision_variables = x.rows(); - SparseVector g = matrices.g(x); SparseMatrix H = matrices.H(x); // Ensure matrix callback dimensions are consistent - slp_assert(g.rows() == num_decision_variables); - slp_assert(H.rows() == num_decision_variables); - slp_assert(H.cols() == num_decision_variables); + slp_assert(g.rows() == matrices.num_decision_variables); + slp_assert(H.rows() == matrices.num_decision_variables); + slp_assert(H.cols() == matrices.num_decision_variables); - // Check whether initial guess has finite f(xₖ) - if (!isfinite(f)) { - return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; + // Check whether initial guess has finite cost and derivatives + if (!isfinite(f) || !all_finite(g) || !all_finite(H)) { + return ExitStatus::NONFINITE_INITIAL_GUESS; } int iterations = 0; Filter filter; - RegularizedLDLT solver{num_decision_variables, 0}; + RegularizedLDLT solver{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 estimate + // Error Scalar E_0 = std::numeric_limits::infinity(); setup_prof.stop(); @@ -170,7 +167,7 @@ ExitStatus newton( // Call iteration callbacks for (const auto& callback : iteration_callbacks) { - if (callback({iterations, x, g, H, SparseMatrix{}, SparseMatrix{}})) { + if (callback({iterations, x, {}, {}, {}, g, H, {}, {}})) { return ExitStatus::CALLBACK_REQUESTED_STOP; } } @@ -207,13 +204,13 @@ ExitStatus newton( α *= α_reduction_factor; if (α < α_min) { - return ExitStatus::LINE_SEARCH_FAILED; + return ExitStatus::LOCALLY_INFEASIBLE; } continue; } // Check whether filter accepts trial iterate - if (filter.try_add(FilterEntry{trial_f}, α)) { + if (filter.try_add(FilterEntry{f}, FilterEntry{trial_f}, p_x, g, α)) { // Accept step break; } @@ -224,11 +221,12 @@ ExitStatus newton( // If step size hit a minimum, check if the KKT error was reduced. If it // wasn't, report bad line search. if (α < α_min) { - Scalar current_kkt_error = kkt_error(g); + Scalar current_kkt_error = kkt_error(g); DenseVector trial_x = x + α_max * p_x; - Scalar next_kkt_error = kkt_error(matrices.g(trial_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) { @@ -238,7 +236,7 @@ ExitStatus newton( break; } - return ExitStatus::LINE_SEARCH_FAILED; + return ExitStatus::LOCALLY_INFEASIBLE; } } @@ -254,8 +252,8 @@ ExitStatus newton( ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof}; - // Update the error estimate - E_0 = error_estimate(g); + // Update the error + E_0 = kkt_error(g); next_iter_prep_profiler.stop(); inner_iter_profiler.stop(); 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 85bc698a97..81bd721f32 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 @@ -21,6 +21,9 @@ struct NewtonMatrixCallbacks { /// Type alias for sparse vector. using SparseVector = Eigen::SparseVector; + /// Number of decision variables. + int num_decision_variables = 0; + /// Cost function value f(x) getter. /// ///
@@ -65,7 +68,7 @@ struct NewtonMatrixCallbacks { /// Lagrangian Hessian ∇ₓₓ²L(x) getter. /// - /// L(xₖ) = f(xₖ) + /// L(x) = f(x) /// ///
/// diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp index 5603569011..8029975282 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp @@ -21,71 +21,19 @@ struct SLEIPNIR_DLLEXPORT Options { std::chrono::duration timeout{ std::numeric_limits::infinity()}; - /// Enables the feasible interior-point method. When the inequality - /// constraints are all feasible, step sizes are reduced when necessary to - /// prevent them becoming infeasible again. This is useful when parts of the - /// problem are ill-conditioned in infeasible regions (e.g., square root of a - /// negative value). This can slow or prevent progress toward a solution - /// though, so only enable it if necessary. + /// Enables the feasible interior-point method. + /// + /// When the inequality constraints are all feasible, step sizes are reduced + /// when necessary to prevent them becoming infeasible again. This is useful + /// when parts of the problem are ill-conditioned in infeasible regions (e.g., + /// square root of a negative value). This can slow or prevent progress toward + /// a solution though, so only enable it if necessary. bool feasible_ipm = false; - /// Enables diagnostic prints. + /// Enables diagnostic output. /// - ///
- /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - /// - ///
HeadingDescription
iterIteration number
typeIteration type (normal, accepted second-order correction, rejected - /// second-order correction)
time (ms)Duration of iteration in milliseconds
errorError estimate
costCost function value at current iterate
infeas.Constraint infeasibility at current iterate
complement.Complementary slackness at current iterate (sᵀz)
μBarrier parameter
regIteration matrix regularization
primal αPrimal step size
dual αDual step size
↩Number of line search backtracks
+ /// See https://sleipnirgroup.github.io/Sleipnir/md_usage.html#output for more + /// information. bool diagnostics = false; }; 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 a4738668e6..335245136b 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 @@ -16,16 +16,17 @@ #include "sleipnir/optimization/solver/iteration_info.hpp" #include "sleipnir/optimization/solver/options.hpp" #include "sleipnir/optimization/solver/sqp_matrix_callbacks.hpp" -#include "sleipnir/optimization/solver/util/error_estimate.hpp" +#include "sleipnir/optimization/solver/util/all_finite.hpp" +#include "sleipnir/optimization/solver/util/append_as_triplets.hpp" +#include "sleipnir/optimization/solver/util/feasibility_restoration.hpp" #include "sleipnir/optimization/solver/util/filter.hpp" #include "sleipnir/optimization/solver/util/is_locally_infeasible.hpp" #include "sleipnir/optimization/solver/util/kkt_error.hpp" #include "sleipnir/optimization/solver/util/regularized_ldlt.hpp" #include "sleipnir/util/assert.hpp" #include "sleipnir/util/print_diagnostics.hpp" +#include "sleipnir/util/profiler.hpp" #include "sleipnir/util/scope_exit.hpp" -#include "sleipnir/util/scoped_profiler.hpp" -#include "sleipnir/util/solve_profiler.hpp" #include "sleipnir/util/symbol_exports.hpp" // See docs/algorithms.md#Works_cited for citation definitions. @@ -59,14 +60,49 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, const Options& options, Eigen::Vector& x) { using DenseVector = Eigen::Vector; + + DenseVector y = DenseVector::Zero(matrix_callbacks.num_equality_constraints); + + return sqp(matrix_callbacks, iteration_callbacks, options, x, y); +} + +/// Finds the optimal solution to a nonlinear program using Sequential Quadratic +/// Programming (SQP). +/// +/// A nonlinear program has the form: +/// +/// ``` +/// min_x f(x) +/// subject to cₑ(x) = 0 +/// ``` +/// +/// where f(x) is the cost function and cₑ(x) are the equality constraints. +/// +/// @tparam Scalar Scalar type. +/// @param[in] matrix_callbacks Matrix callbacks. +/// @param[in] iteration_callbacks The list of callbacks to call at the +/// beginning of each iteration. +/// @param[in] options Solver options. +/// @param[in,out] x The initial guess and output location for the decision +/// variables. +/// @param[in,out] y The initial guess and output location for the equality +/// constraint dual variables. +/// @return The exit status. +template +ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, + std::span& info)>> + iteration_callbacks, + const Options& options, Eigen::Vector& x, + Eigen::Vector& y) { + using DenseVector = Eigen::Vector; using SparseMatrix = Eigen::SparseMatrix; using SparseVector = Eigen::SparseVector; /// SQP step direction. struct Step { - /// Primal step. + /// Decision variable primal step. DenseVector p_x; - /// Dual step. + /// Equality constraint dual step. DenseVector p_y; }; @@ -89,6 +125,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, solve_profilers.emplace_back(" ↳ f(x)"); solve_profilers.emplace_back(" ↳ ∇f(x)"); solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓyᵀcₑ"); solve_profilers.emplace_back(" ↳ cₑ(x)"); solve_profilers.emplace_back(" ↳ ∂cₑ/∂x"); @@ -109,10 +146,13 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, auto& f_prof = solve_profilers[11]; auto& g_prof = solve_profilers[12]; auto& H_prof = solve_profilers[13]; - auto& c_e_prof = solve_profilers[14]; - auto& A_e_prof = solve_profilers[15]; + auto& H_c_prof = solve_profilers[14]; + auto& c_e_prof = solve_profilers[15]; + auto& A_e_prof = solve_profilers[16]; SQPMatrixCallbacks matrices{ + matrix_callbacks.num_decision_variables, + matrix_callbacks.num_equality_constraints, [&](const DenseVector& x) -> Scalar { ScopedProfiler prof{f_prof}; return matrix_callbacks.f(x); @@ -125,6 +165,10 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, ScopedProfiler prof{H_prof}; return matrix_callbacks.H(x, y); }, + [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { + ScopedProfiler prof{H_c_prof}; + return matrix_callbacks.H_c(x, y); + }, [&](const DenseVector& x) -> DenseVector { ScopedProfiler prof{c_e_prof}; return matrix_callbacks.c_e(x); @@ -141,13 +185,21 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, setup_prof.start(); Scalar f = matrices.f(x); + SparseVector g = matrices.g(x); + SparseMatrix H = matrices.H(x, y); DenseVector c_e = matrices.c_e(x); + SparseMatrix A_e = matrices.A_e(x); - int num_decision_variables = x.rows(); - int num_equality_constraints = c_e.rows(); + // Ensure matrix callback dimensions are consistent + slp_assert(g.rows() == matrices.num_decision_variables); + slp_assert(H.rows() == matrices.num_decision_variables); + slp_assert(H.cols() == matrices.num_decision_variables); + slp_assert(c_e.rows() == matrices.num_equality_constraints); + slp_assert(A_e.rows() == matrices.num_equality_constraints); + slp_assert(A_e.cols() == matrices.num_decision_variables); // Check for overconstrained problem - if (num_equality_constraints > num_decision_variables) { + if (matrices.num_equality_constraints > matrices.num_decision_variables) { if (options.diagnostics) { print_too_few_dofs_error(c_e); } @@ -155,34 +207,21 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, return ExitStatus::TOO_FEW_DOFS; } - SparseVector g = matrices.g(x); - SparseMatrix A_e = matrices.A_e(x); - - DenseVector y = DenseVector::Zero(num_equality_constraints); - - SparseMatrix H = matrices.H(x, y); - - // Ensure matrix callback dimensions are consistent - slp_assert(g.rows() == num_decision_variables); - slp_assert(A_e.rows() == num_equality_constraints); - slp_assert(A_e.cols() == num_decision_variables); - slp_assert(H.rows() == num_decision_variables); - slp_assert(H.cols() == num_decision_variables); - - // Check whether initial guess has finite f(xₖ) and cₑ(xₖ) - if (!isfinite(f) || !c_e.allFinite()) { - return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS; + // Check whether initial guess has finite cost, constraints, and derivatives + if (!isfinite(f) || !all_finite(g) || !all_finite(H) || !c_e.allFinite() || + !all_finite(A_e)) { + return ExitStatus::NONFINITE_INITIAL_GUESS; } int iterations = 0; - Filter filter; + Filter filter{c_e.template lpNorm<1>()}; // Kept outside the loop so its storage can be reused gch::small_vector> triplets; - RegularizedLDLT solver{num_decision_variables, - num_equality_constraints}; + RegularizedLDLT solver{matrices.num_decision_variables, + matrices.num_equality_constraints}; // Variables for determining when a step is acceptable constexpr Scalar α_reduction_factor(0.5); @@ -190,7 +229,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, int full_step_rejected_counter = 0; - // Error estimate + // Error Scalar E_0 = std::numeric_limits::infinity(); setup_prof.stop(); @@ -229,7 +268,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, // Call iteration callbacks for (const auto& callback : iteration_callbacks) { - if (callback({iterations, x, g, H, A_e, SparseMatrix{}})) { + if (callback({iterations, x, {}, y, {}, g, H, A_e, {}})) { return ExitStatus::CALLBACK_REQUESTED_STOP; } } @@ -243,18 +282,10 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, // Don't assign upper triangle because solver only uses lower triangle. triplets.clear(); triplets.reserve(H.nonZeros() + A_e.nonZeros()); - for (int col = 0; col < H.cols(); ++col) { - // Append column of H lower triangle in top-left quadrant - for (typename SparseMatrix::InnerIterator it{H, col}; it; ++it) { - triplets.emplace_back(it.row(), it.col(), it.value()); - } - // Append column of Aₑ in bottom-left quadrant - for (typename SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { - triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); - } - } - SparseMatrix lhs(num_decision_variables + num_equality_constraints, - num_decision_variables + num_equality_constraints); + append_as_triplets(triplets, 0, 0, {H, A_e}); + SparseMatrix lhs( + matrices.num_decision_variables + matrices.num_equality_constraints, + matrices.num_decision_variables + matrices.num_equality_constraints); lhs.setFromSortedTriplets(triplets.begin(), triplets.end()); // rhs = −[∇f − Aₑᵀy] @@ -269,6 +300,7 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, Step step; constexpr Scalar α_max(1); Scalar α(1); + bool call_feasibility_restoration = false; // Solve the Newton-KKT system // @@ -295,6 +327,8 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, α = α_max; + const FilterEntry current_entry{f, c_e}; + // Loop until a step is accepted while (1) { DenseVector trial_x = x + α * step.p_x; @@ -310,13 +344,15 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, α *= α_reduction_factor; if (α < α_min) { - return ExitStatus::LINE_SEARCH_FAILED; + call_feasibility_restoration = true; + break; } continue; } // Check whether filter accepts trial iterate - if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) { + FilterEntry trial_entry{trial_f, trial_c_e}; + if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) { // Accept step break; } @@ -350,8 +386,9 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, step_acceptable ? IterationType::ACCEPTED_SOC : IterationType::REJECTED_SOC, soc_profiler.current_duration(), - error_estimate(g, A_e, trial_c_e, trial_y), trial_f, - trial_c_e.template lpNorm<1>(), Scalar(0), Scalar(0), + 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)); } @@ -386,7 +423,8 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, } // Check whether filter accepts trial iterate - if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) { + 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; @@ -412,7 +450,8 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, // See section 3.2 case I of [2]. if (full_step_rejected_counter >= 4 && filter.max_constraint_violation > - filter.back().constraint_violation / Scalar(10)) { + current_entry.constraint_violation / Scalar(10) && + filter.last_rejection_due_to_filter()) { filter.max_constraint_violation *= Scalar(0.1); filter.reset(); continue; @@ -422,16 +461,17 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, α *= α_reduction_factor; // If step size hit a minimum, check if the KKT error was reduced. If it - // wasn't, report line search failure. + // wasn't, invoke feasibility restoration. if (α < α_min) { - Scalar current_kkt_error = kkt_error(g, A_e, c_e, y); + Scalar current_kkt_error = + kkt_error(g, A_e, c_e, y); trial_x = x + α_max * step.p_x; trial_y = y + α_max * step.p_y; trial_c_e = matrices.c_e(trial_x); - Scalar next_kkt_error = kkt_error( + Scalar next_kkt_error = kkt_error( matrices.g(trial_x), matrices.A_e(trial_x), trial_c_e, trial_y); // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway @@ -442,21 +482,54 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, break; } - return ExitStatus::LINE_SEARCH_FAILED; + call_feasibility_restoration = true; + break; } } line_search_profiler.stop(); - // If full step was accepted, reset full-step rejected counter - if (α == α_max) { - full_step_rejected_counter = 0; - } + if (call_feasibility_restoration) { + FilterEntry initial_entry{matrices.f(x), c_e}; - // xₖ₊₁ = xₖ + αₖpₖˣ - // yₖ₊₁ = yₖ + αₖpₖʸ - x += α * step.p_x; - y += α * step.p_y; + // Feasibility restoration phase + gch::small_vector& info)>> + callbacks; + for (auto& callback : iteration_callbacks) { + callbacks.emplace_back(callback); + } + callbacks.emplace_back([&](const IterationInfo& info) { + DenseVector trial_x = + info.x.segment(0, matrices.num_decision_variables); + + DenseVector trial_c_e = matrices.c_e(trial_x); + + FilterEntry trial_entry{matrices.f(trial_x), trial_c_e}; + + // If the current iterate sufficiently reduces constraint violation and + // is accepted by the normal filter, stop feasibility restoration + return trial_entry.constraint_violation < + 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); + + if (status != ExitStatus::SUCCESS) { + // Report failure + return status; + } + } 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 autodiff for Jacobians and Hessian f = matrices.f(x); @@ -468,8 +541,8 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, c_e = matrices.c_e(x); - // Update the error estimate - E_0 = error_estimate(g, A_e, c_e, y); + // Update the error + E_0 = kkt_error(g, A_e, c_e, y); next_iter_prep_profiler.stop(); inner_iter_profiler.stop(); 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 55aabcc6e8..ee44c36d67 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 @@ -21,6 +21,12 @@ struct SQPMatrixCallbacks { /// Type alias for sparse vector. using SparseVector = Eigen::SparseVector; + /// Number of decision variables. + int num_decision_variables = 0; + + /// Number of equality constraints. + int num_equality_constraints = 0; + /// Cost function value f(x) getter. /// /// @@ -65,7 +71,7 @@ struct SQPMatrixCallbacks { /// Lagrangian Hessian ∇ₓₓ²L(x, y) getter. /// - /// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ) + /// L(x, y) = f(x) − yᵀcₑ(x) /// ///
/// @@ -91,6 +97,32 @@ struct SQPMatrixCallbacks { ///
std::function H; + /// Constraint part of Lagrangian Hessian ∇ₓₓ² −yᵀcₑ(x) getter. + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// + ///
VariableRowsColumns
xnum_decision_variables1
ynum_equality_constraints1
∇ₓₓ² −yᵀcₑ(x)num_decision_variablesnum_decision_variables
+ std::function H_c; + /// Equality constraint value cₑ(x) getter. /// /// @@ -115,10 +147,10 @@ struct SQPMatrixCallbacks { /// Equality constraint Jacobian ∂cₑ/∂x getter. /// /// ``` - /// [∇ᵀcₑ₁(xₖ)] - /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)] - /// [ ⋮ ] - /// [∇ᵀcₑₘ(xₖ)] + /// [∇ᵀcₑ₁(x)] + /// Aₑ(x) = [∇ᵀcₑ₂(x)] + /// [ ⋮ ] + /// [∇ᵀcₑₘ(x)] /// ``` /// ///
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 new file mode 100644 index 0000000000..57b47fcd99 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp @@ -0,0 +1,26 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +/// Returns true if elements of sparse matrix are all finite. +/// +/// @param mat Sparse matrix. +/// @return True if elements of sparse matrix are all finite. +template +bool all_finite(const Eigen::SparseCompressedBase& mat) { + using std::isfinite; + + for (int k = 0; k < mat.outerSize(); ++k) { + for (typename Derived::InnerIterator it{mat, k}; it; ++it) { + if (!isfinite(it.value())) { + return false; + } + } + } + + return true; +} 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 new file mode 100644 index 0000000000..7e96216277 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp @@ -0,0 +1,60 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include +#include + +/// 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, +/// first column of mat2 underneath first column of mat1, second column of mat1, +/// second column of mat2 underneath second column of mat1). +/// +/// @tparam Scalar Scalar type. +/// @param triplets List of triplets. +/// @param row_offset Row offset for first matrix. +/// @param col_offset Column offset for first matrix. +/// @param mats Sparse matrices to append. +template +void append_as_triplets( + gch::small_vector>& triplets, int row_offset, + int col_offset, std::initializer_list> mats) { + // Compute row offset for each matrix + gch::small_vector mat_row_offsets; + int mat_row_offset = 0; + for (const auto& mat : mats) { + mat_row_offsets.emplace_back(mat_row_offset); + mat_row_offset += mat.rows(); + } + + // Append elements in column-major order + for (int col = 0; col < mats.begin()[0].cols(); ++col) { + for (size_t i = 0; i < mats.size(); ++i) { + for (typename Eigen::SparseMatrix::InnerIterator it{ + mats.begin()[i], col}; + it; ++it) { + triplets.emplace_back(row_offset + mat_row_offsets[i] + it.row(), + col_offset + it.col(), it.value()); + } + } + } +} + +/// Append diagonal matrix to list of triplets at the given offset. +/// +/// @tparam Scalar Scalar type. +/// @param triplets List of triplets. +/// @param row_offset Row offset for first matrix. +/// @param col_offset Column offset for first matrix. +/// @param diag Diagonal of matrix. +template +void append_diagonal_as_triplets( + gch::small_vector>& triplets, int row_offset, + int col_offset, const Eigen::Vector& diag) { + for (int row = 0; row < diag.rows(); ++row) { + triplets.emplace_back(row_offset + row, col_offset + row, diag[row]); + } +} diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp deleted file mode 100644 index d82d5bf85a..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp +++ /dev/null @@ -1,130 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include - -#include -#include - -// See docs/algorithms.md#Works_cited for citation definitions - -namespace slp { - -/// Returns the error estimate using the KKT conditions for Newton's method. -/// -/// @tparam Scalar Scalar type. -/// @param g Gradient of the cost function ∇f. -template -Scalar error_estimate(const Eigen::Vector& g) { - // Update the error estimate using the KKT conditions from equations (19.5a) - // through (19.5d) of [1]. - // - // ∇f = 0 - - return g.template lpNorm(); -} - -/// Returns the error estimate using the KKT conditions 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 c_e The problem's equality constraints cₑ(x) evaluated at the current -/// iterate. -/// @param y Equality constraint dual variables. -template -Scalar error_estimate(const Eigen::Vector& g, - const Eigen::SparseMatrix& A_e, - const Eigen::Vector& c_e, - const Eigen::Vector& y) { - // Update the error estimate using the KKT conditions from equations (19.5a) - // through (19.5d) of [1]. - // - // ∇f − Aₑᵀy = 0 - // cₑ = 0 - // - // The error tolerance is the max of the following infinity norms scaled by - // s_d (see equation (5) of [2]). - // - // ‖∇f − Aₑᵀy‖_∞ / s_d - // ‖cₑ‖_∞ - - // s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ - constexpr Scalar s_max(100); - Scalar s_d = - std::max(s_max, y.template lpNorm<1>() / Scalar(y.rows())) / s_max; - - return std::max( - {(g - A_e.transpose() * y).template lpNorm() / s_d, - c_e.template lpNorm()}); -} - -/// Returns the error estimate using the KKT conditions for the 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 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. -/// @param s Inequality constraint slack variables. -/// @param y Equality constraint dual variables. -/// @param z Inequality constraint dual variables. -/// @param μ Barrier parameter. -template -Scalar error_estimate(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 μ) { - // Update the error estimate using the KKT conditions from equations (19.5a) - // through (19.5d) of [1]. - // - // ∇f − Aₑᵀy − Aᵢᵀz = 0 - // Sz − μe = 0 - // cₑ = 0 - // cᵢ − s = 0 - // - // The error tolerance is the max of the following infinity norms scaled by - // s_d and s_c (see equation (5) of [2]). - // - // ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d - // ‖Sz − μe‖_∞ / s_c - // ‖cₑ‖_∞ - // ‖cᵢ − s‖_∞ - - // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ - constexpr Scalar s_max(100); - Scalar s_d = - std::max(s_max, (y.template lpNorm<1>() + z.template lpNorm<1>()) / - Scalar(y.rows() + z.rows())) / - s_max; - - // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ - Scalar s_c = - std::max(s_max, z.template lpNorm<1>() / Scalar(z.rows())) / s_max; - - const auto S = s.asDiagonal(); - const Eigen::Vector μe = - Eigen::Vector::Constant(s.rows(), μ); - - return std::max({(g - A_e.transpose() * y - A_i.transpose() * z) - .template lpNorm() / - s_d, - (S * z - μe).template lpNorm() / s_c, - c_e.template lpNorm(), - (c_i - s).template lpNorm()}); -} - -} // 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 new file mode 100644 index 0000000000..1c042f8599 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp @@ -0,0 +1,601 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include + +#include "sleipnir/optimization/solver/exit_status.hpp" +#include "sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp" +#include "sleipnir/optimization/solver/iteration_info.hpp" +#include "sleipnir/optimization/solver/options.hpp" +#include "sleipnir/optimization/solver/sqp_matrix_callbacks.hpp" +#include "sleipnir/optimization/solver/util/append_as_triplets.hpp" +#include "sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp" + +namespace slp { + +template +ExitStatus interior_point( + const InteriorPointMatrixCallbacks& matrix_callbacks, + std::span& info)>> + iteration_callbacks, + const Options& options, bool in_feasibility_restoration, +#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& μ); + +/// Computes initial values for p and n in feasibility restoration. +/// +/// @tparam Scalar Scalar type. +/// @param[in] c The constraint violation. +/// @param[in] ρ Scaling parameter. +/// @param[in] μ Barrier parameter. +/// @return Tuple of positive and negative constraint relaxation slack variables +/// respectively. +template +std::tuple, + Eigen::Vector> +compute_p_n(const Eigen::Vector& c, Scalar ρ, + Scalar μ) { + // From equation (33) of [2]: + // ______________________ + // μ − ρ c(x) /(μ − ρ c(x))² μ c(x) + // n = −−−−−−−−−− + / (−−−−−−−−−−) + −−−−−− (1) + // 2ρ √ ( 2ρ ) 2ρ + // + // The quadratic formula: + // ________ + // -b + √b² - 4ac + // x = −−−−−−−−−−−−−− (2) + // 2a + // + // Rearrange (1) to fit the quadratic formula better: + // _________________________ + // μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x) + // n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−− + // 2ρ + // + // Solve for coefficients: + // + // a = ρ (3) + // b = ρ c(x) - μ (4) + // + // -4ac = 2ρ μ c(x) + // -4(ρ)c = 2ρ μ c(x) + // -4c = 2μ c(x) + // c = -μ c(x)/2 (5) + // + // p = c(x) + n (6) + + using DenseVector = Eigen::Vector; + + using std::sqrt; + + DenseVector p{c.rows()}; + DenseVector n{c.rows()}; + for (int row = 0; row < p.rows(); ++row) { + Scalar _a = ρ; + Scalar _b = ρ * c[row] - μ; + Scalar _c = -μ * c[row] / Scalar(2); + + n[row] = (-_b + sqrt(_b * _b - Scalar(4) * _a * _c)) / (Scalar(2) * _a); + p[row] = c[row] + n[row]; + } + + return {std::move(p), std::move(n)}; +} + +// @cond Suppress Doxygen + +/// 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 +/// feasible point. +/// +/// @tparam Scalar Scalar type. +/// @param[in] matrix_callbacks Matrix callbacks. +/// @param[in] iteration_callbacks The list of callbacks to call at the +/// beginning of each iteration. +/// @param[in] options Solver options. +/// @param[in,out] x The decision variables from the normal solve. +/// @param[in,out] y The equality constraint dual variables from the normal +/// solve. +/// @return The exit status. +template +ExitStatus feasibility_restoration( + const SQPMatrixCallbacks& matrix_callbacks, + std::span& info)>> + iteration_callbacks, + const Options& options, Eigen::Vector& x, + Eigen::Vector& y) { + // Feasibility restoration + // + // min ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ) + // x + // pₑ,nₑ + // + // s.t. cₑ(x) - pₑ + nₑ = 0 + // pₑ ≥ 0 + // nₑ ≥ 0 + // + // where ρ = 1000, ζ = √μ where μ is the barrier parameter, xᵣ is original + // iterate before feasibility restoration, and Dᵣ is a scaling matrix defined + // by + // + // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows()) + + using DenseVector = Eigen::Vector; + using DiagonalMatrix = Eigen::DiagonalMatrix; + using SparseMatrix = Eigen::SparseMatrix; + using SparseVector = Eigen::SparseVector; + + using std::sqrt; + + const auto& matrices = matrix_callbacks; + const auto& num_vars = matrices.num_decision_variables; + const auto& num_eq = matrices.num_equality_constraints; + + constexpr Scalar ρ(1e3); + const Scalar μ(options.tolerance / 10.0); + const Scalar ζ = sqrt(μ); + + const DenseVector c_e = matrices.c_e(x); + + const auto& x_r = x; + const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ); + + // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows()) + const DiagonalMatrix D_r = + x.cwiseSquare().cwiseInverse().cwiseMin(Scalar(1)).asDiagonal(); + + DenseVector fr_x{num_vars + 2 * num_eq}; + fr_x << x, p_e_0, n_e_0; + + DenseVector fr_s = DenseVector::Ones(2 * num_eq); + + DenseVector fr_y = DenseVector::Zero(num_eq); + + DenseVector fr_z{2 * num_eq}; + fr_z << μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse(); + + Scalar fr_μ = std::max(μ, c_e.template lpNorm()); + + InteriorPointMatrixCallbacks fr_matrix_callbacks{ + static_cast(fr_x.rows()), + static_cast(fr_y.rows()), + static_cast(fr_z.rows()), + [&](const DenseVector& x_p) -> Scalar { + auto x = x_p.segment(0, num_vars); + + // Cost function + // + // ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ) + + auto diff = x - x_r; + return ρ * x_p.segment(num_vars, 2 * num_eq).array().sum() + + ζ / Scalar(2) * diff.transpose() * D_r * diff; + }, + [&](const DenseVector& x_p) -> SparseVector { + auto x = x_p.segment(0, num_vars); + + // Cost function gradient + // + // [ζDᵣ(x − xᵣ)] + // [ ρ ] + // [ ρ ] + DenseVector g{x_p.rows()}; + g.segment(0, num_vars) = ζ * D_r * (x - x_r); + g.segment(num_vars, 2 * num_eq).setConstant(ρ); + return g.sparseView(); + }, + [&](const DenseVector& x_p, const DenseVector& y_p, + [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix { + auto x = x_p.segment(0, num_vars); + const auto& y = y_p; + + // Cost function Hessian + // + // [ζDᵣ 0 0] + // [ 0 0 0] + // [ 0 0 0] + gch::small_vector> triplets; + triplets.reserve(x_p.rows()); + append_as_triplets(triplets, 0, 0, {SparseMatrix{ζ * D_r}}); + SparseMatrix d2f_dx2{x_p.rows(), x_p.rows()}; + d2f_dx2.setFromSortedTriplets(triplets.begin(), triplets.end()); + + // Constraint part of original problem's Lagrangian Hessian + // + // −∇ₓₓ²yᵀcₑ(x) + auto H_c = matrices.H_c(x, y); + H_c.resize(x_p.rows(), x_p.rows()); + + // Lagrangian Hessian + // + // [ζDᵣ 0 0] + // [ 0 0 0] − ∇ₓₓ²yᵀcₑ(x) + // [ 0 0 0] + return d2f_dx2 + H_c; + }, + [&](const DenseVector& x_p, [[maybe_unused]] const DenseVector& y_p, + [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix { + return SparseMatrix{x_p.rows(), x_p.rows()}; + }, + [&](const DenseVector& x_p) -> DenseVector { + auto x = x_p.segment(0, num_vars); + auto p_e = x_p.segment(num_vars, num_eq); + auto n_e = x_p.segment(num_vars + num_eq, num_eq); + + // Equality constraints + // + // cₑ(x) - pₑ + nₑ = 0 + return matrices.c_e(x) - p_e + n_e; + }, + [&](const DenseVector& x_p) -> SparseMatrix { + auto x = x_p.segment(0, num_vars); + + // Equality constraint Jacobian + // + // [Aₑ −I I] + + SparseMatrix A_e = matrices.A_e(x); + + gch::small_vector> triplets; + triplets.reserve(A_e.nonZeros() + 2 * num_eq); + + append_as_triplets(triplets, 0, 0, {A_e}); + append_diagonal_as_triplets( + triplets, 0, num_vars, + DenseVector::Constant(num_eq, Scalar(-1)).eval()); + append_diagonal_as_triplets( + triplets, 0, num_vars + num_eq, + DenseVector::Constant(num_eq, Scalar(1)).eval()); + + SparseMatrix A_e_p{A_e.rows(), x_p.rows()}; + A_e_p.setFromSortedTriplets(triplets.begin(), triplets.end()); + return A_e_p; + }, + [&](const DenseVector& x_p) -> DenseVector { + // Inequality constraints + // + // pₑ ≥ 0 + // nₑ ≥ 0 + return x_p.segment(num_vars, 2 * num_eq); + }, + [&](const DenseVector& x_p) -> SparseMatrix { + // Inequality constraint Jacobian + // + // [0 I 0] + // [0 0 I] + + gch::small_vector> triplets; + triplets.reserve(2 * num_eq); + + append_diagonal_as_triplets( + triplets, 0, num_vars, + DenseVector::Constant(2 * num_eq, Scalar(1)).eval()); + + SparseMatrix A_i_p{2 * num_eq, x_p.rows()}; + A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end()); + return A_i_p; + }}; + + auto status = interior_point(fr_matrix_callbacks, iteration_callbacks, + options, true, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + {}, +#endif + fr_x, fr_s, fr_y, fr_z, fr_μ); + + x = fr_x.segment(0, x.rows()); + + if (status == ExitStatus::CALLBACK_REQUESTED_STOP) { + auto g = matrices.g(x); + auto A_e = matrices.A_e(x); + + y = lagrange_multiplier_estimate(g, A_e); + + return ExitStatus::SUCCESS; + } else if (status == ExitStatus::SUCCESS) { + return ExitStatus::LOCALLY_INFEASIBLE; + } else { + return ExitStatus::FEASIBILITY_RESTORATION_FAILED; + } +} + +/// 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 interior-point method fails to converge to a feasible point. +/// +/// @tparam Scalar Scalar type. +/// @param[in] matrix_callbacks Matrix callbacks. +/// @param[in] iteration_callbacks The list of callbacks to call at the +/// beginning of each iteration. +/// @param[in] options Solver options. +/// @param[in,out] x The current decision variables from the normal solve. +/// @param[in,out] s The current inequality constraint slack variables from the +/// normal solve. +/// @param[in,out] y The current equality constraint duals from the normal +/// solve. +/// @param[in,out] z The current inequality constraint duals from the normal +/// solve. +/// @param[in] μ Barrier parameter. +/// @return The exit status. +template +ExitStatus feasibility_restoration( + const InteriorPointMatrixCallbacks& matrix_callbacks, + std::span& info)>> + iteration_callbacks, + const Options& options, Eigen::Vector& x, + Eigen::Vector& s, + Eigen::Vector& y, + Eigen::Vector& z, Scalar μ) { + // Feasibility restoration + // + // min ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ) + // x + // pₑ,nₑ + // pᵢ,nᵢ + // + // s.t. cₑ(x) - pₑ + nₑ = 0 + // cᵢ(x) - pᵢ + nᵢ ≥ 0 + // pₑ ≥ 0 + // nₑ ≥ 0 + // pᵢ ≥ 0 + // nᵢ ≥ 0 + // + // where ρ = 1000, ζ = √μ where μ is the barrier parameter, xᵣ is original + // iterate before feasibility restoration, and Dᵣ is a scaling matrix defined + // by + // + // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows()) + + using DenseVector = Eigen::Vector; + using DiagonalMatrix = Eigen::DiagonalMatrix; + using SparseMatrix = Eigen::SparseMatrix; + using SparseVector = Eigen::SparseVector; + + using std::sqrt; + + const auto& matrices = matrix_callbacks; + const auto& num_vars = matrices.num_decision_variables; + const auto& num_eq = matrices.num_equality_constraints; + 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); + + 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(), ρ, μ); + + // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows()) + const DiagonalMatrix D_r = + x.cwiseSquare().cwiseInverse().cwiseMin(Scalar(1)).asDiagonal(); + + DenseVector fr_x{num_vars + 2 * num_eq + 2 * num_ineq}; + fr_x << x, p_e_0, n_e_0, p_i_0, n_i_0; + + DenseVector fr_s{s.rows() + 2 * num_eq + 2 * num_ineq}; + fr_s.segment(0, s.rows()) = s; + fr_s.segment(s.rows(), 2 * num_eq + 2 * num_ineq).setOnes(); + + DenseVector fr_y = DenseVector::Zero(c_e.rows()); + + 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(); + + Scalar fr_μ = std::max({μ, c_e.template lpNorm(), + (c_i - s).template lpNorm()}); + + InteriorPointMatrixCallbacks fr_matrix_callbacks{ + static_cast(fr_x.rows()), + static_cast(fr_y.rows()), + static_cast(fr_z.rows()), + [&](const DenseVector& x_p) -> Scalar { + auto x = x_p.segment(0, num_vars); + + // Cost function + // + // ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ) + auto diff = x - x_r; + return ρ * x_p.segment(num_vars, 2 * num_eq + 2 * num_ineq) + .array() + .sum() + + ζ / Scalar(2) * diff.transpose() * D_r * diff; + }, + [&](const DenseVector& x_p) -> SparseVector { + auto x = x_p.segment(0, num_vars); + + // Cost function gradient + // + // [ζDᵣ(x − xᵣ)] + // [ ρ ] + // [ ρ ] + // [ ρ ] + // [ ρ ] + DenseVector g{x_p.rows()}; + g.segment(0, num_vars) = ζ * D_r * (x - x_r); + g.segment(num_vars, 2 * num_eq + 2 * num_ineq).setConstant(ρ); + return g.sparseView(); + }, + [&](const DenseVector& x_p, const DenseVector& y_p, + const DenseVector& z_p) -> SparseMatrix { + auto x = x_p.segment(0, num_vars); + const auto& y = y_p; + auto z = z_p.segment(0, num_ineq); + + // Cost function Hessian + // + // [ζDᵣ 0 0 0 0] + // [ 0 0 0 0 0] + // [ 0 0 0 0 0] + // [ 0 0 0 0 0] + // [ 0 0 0 0 0] + gch::small_vector> triplets; + triplets.reserve(x_p.rows()); + append_as_triplets(triplets, 0, 0, {SparseMatrix{ζ * D_r}}); + SparseMatrix d2f_dx2{x_p.rows(), x_p.rows()}; + d2f_dx2.setFromSortedTriplets(triplets.begin(), triplets.end()); + + // Constraint part of original problem's Lagrangian Hessian + // + // −∇ₓₓ²yᵀcₑ(x) − ∇ₓₓ²zᵀcᵢ(x) + auto H_c = matrices.H_c(x, y, z); + H_c.resize(x_p.rows(), x_p.rows()); + + // Lagrangian Hessian + // + // [ζDᵣ 0 0 0 0] + // [ 0 0 0 0 0] + // [ 0 0 0 0 0] − ∇ₓₓ²yᵀcₑ(x) − ∇ₓₓ²zᵀcᵢ(x) + // [ 0 0 0 0 0] + // [ 0 0 0 0 0] + return d2f_dx2 + H_c; + }, + [&](const DenseVector& x_p, [[maybe_unused]] const DenseVector& y_p, + [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix { + return SparseMatrix{x_p.rows(), x_p.rows()}; + }, + [&](const DenseVector& x_p) -> DenseVector { + auto x = x_p.segment(0, num_vars); + auto p_e = x_p.segment(num_vars, num_eq); + auto n_e = x_p.segment(num_vars + num_eq, num_eq); + + // Equality constraints + // + // cₑ(x) - pₑ + nₑ = 0 + return matrices.c_e(x) - p_e + n_e; + }, + [&](const DenseVector& x_p) -> SparseMatrix { + auto x = x_p.segment(0, num_vars); + + // Equality constraint Jacobian + // + // [Aₑ −I I 0 0] + + SparseMatrix A_e = matrices.A_e(x); + + gch::small_vector> triplets; + triplets.reserve(A_e.nonZeros() + 2 * num_eq); + + append_as_triplets(triplets, 0, 0, {A_e}); + append_diagonal_as_triplets( + triplets, 0, num_vars, + DenseVector::Constant(num_eq, Scalar(-1)).eval()); + append_diagonal_as_triplets( + triplets, 0, num_vars + num_eq, + DenseVector::Constant(num_eq, Scalar(1)).eval()); + + SparseMatrix A_e_p{A_e.rows(), x_p.rows()}; + A_e_p.setFromSortedTriplets(triplets.begin(), triplets.end()); + return A_e_p; + }, + [&](const DenseVector& x_p) -> DenseVector { + auto x = x_p.segment(0, num_vars); + auto p_i = x_p.segment(num_vars + 2 * num_eq, num_ineq); + auto n_i = x_p.segment(num_vars + 2 * num_eq + num_ineq, num_ineq); + + // Inequality constraints + // + // cᵢ(x) - pᵢ + nᵢ ≥ 0 + // pₑ ≥ 0 + // nₑ ≥ 0 + // pᵢ ≥ 0 + // nᵢ ≥ 0 + DenseVector c_i_p{c_i.rows() + 2 * num_eq + 2 * num_ineq}; + c_i_p.segment(0, num_ineq) = matrices.c_i(x) - p_i + n_i; + c_i_p.segment(p_i.rows(), 2 * num_eq + 2 * num_ineq) = + x_p.segment(num_vars, 2 * num_eq + 2 * num_ineq); + return c_i_p; + }, + [&](const DenseVector& x_p) -> SparseMatrix { + auto x = x_p.segment(0, num_vars); + + // Inequality constraint Jacobian + // + // [Aᵢ 0 0 −I I] + // [0 I 0 0 0] + // [0 0 I 0 0] + // [0 0 0 I 0] + // [0 0 0 0 I] + + SparseMatrix A_i = matrices.A_i(x); + + gch::small_vector> triplets; + triplets.reserve(A_i.nonZeros() + 2 * num_eq + 4 * num_ineq); + + // Column 0 + append_as_triplets(triplets, 0, 0, {A_i}); + + // Columns 1 and 2 + append_diagonal_as_triplets( + triplets, num_ineq, num_vars, + DenseVector::Constant(2 * num_eq, Scalar(1)).eval()); + + SparseMatrix I_ineq{ + DenseVector::Constant(num_ineq, Scalar(1)).asDiagonal()}; + + // Column 3 + SparseMatrix Z_col3{2 * num_eq, num_ineq}; + append_as_triplets(triplets, 0, num_vars + 2 * num_eq, + {(-I_ineq).eval(), Z_col3, I_ineq}); + + // Column 4 + SparseMatrix Z_col4{2 * num_eq + num_ineq, num_ineq}; + append_as_triplets(triplets, 0, num_vars + 2 * num_eq + num_ineq, + {I_ineq, Z_col4, I_ineq}); + + 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; + }}; + + auto status = interior_point(fr_matrix_callbacks, iteration_callbacks, + options, true, +#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION + {}, +#endif + fr_x, fr_s, fr_y, fr_z, fr_μ); + + x = fr_x.segment(0, x.rows()); + s = fr_s.segment(0, s.rows()); + + if (status == ExitStatus::CALLBACK_REQUESTED_STOP) { + auto g = matrices.g(x); + auto A_e = matrices.A_e(x); + auto A_i = matrices.A_i(x); + + auto [y_estimate, z_estimate] = + lagrange_multiplier_estimate(g, A_e, A_i, s, μ); + y = y_estimate; + z = z_estimate; + + return ExitStatus::SUCCESS; + } else if (status == ExitStatus::SUCCESS) { + return ExitStatus::LOCALLY_INFEASIBLE; + } else { + return ExitStatus::FEASIBILITY_RESTORATION_FAILED; + } +} + +// @endcond + +} // namespace slp + +#include "sleipnir/optimization/solver/interior_point.hpp" 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 c96ffb5b65..8d98cbee4c 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 @@ -5,9 +5,9 @@ #include #include #include -#include #include +#include #include // See docs/algorithms.md#Works_cited for citation definitions. @@ -34,14 +34,10 @@ struct FilterEntry { /// /// @param cost The cost function's value. /// @param constraint_violation The constraint violation. - constexpr FilterEntry(Scalar cost, Scalar constraint_violation) + explicit constexpr FilterEntry(Scalar cost, + Scalar constraint_violation = Scalar(0)) : cost{cost}, constraint_violation{constraint_violation} {} - /// Constructs a Newton's method filter entry. - /// - /// @param f The cost function value. - explicit FilterEntry(Scalar f) : FilterEntry{f, Scalar(0)} {} - /// Constructs a Sequential Quadratic Programming filter entry. /// /// @param f The cost function value. @@ -61,6 +57,15 @@ struct FilterEntry { : FilterEntry{f - μ * s.array().log().sum(), c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>()} { } + + /// Returns true if this filter entry is dominated by another. + /// + /// @param entry The other entry. + /// @return True if this filter entry is dominated by another. + constexpr bool dominated_by(const FilterEntry& entry) const { + return entry.cost <= cost && + entry.constraint_violation <= constraint_violation; + } }; /// Step filter. @@ -71,115 +76,151 @@ struct FilterEntry { template class Filter { public: + /// Type alias for dense vector. + using DenseVector = Eigen::Vector; + + /// Type alias for sparse vector. + using SparseVector = Eigen::SparseVector; + + /// The minimum constraint violation + Scalar min_constraint_violation; + /// The maximum constraint violation - Scalar max_constraint_violation{1e4}; + Scalar max_constraint_violation; /// Constructs an empty filter. - Filter() { - // Initial filter entry rejects constraint violations above max - m_filter.emplace_back(std::numeric_limits::infinity(), - max_constraint_violation); + /// + /// @param initial_constraint_violation The optimization problem's initial + /// constraint violation. + explicit constexpr Filter(Scalar initial_constraint_violation = Scalar(0)) { + using std::max; + + min_constraint_violation = + Scalar(1e-4) * max(Scalar(1), initial_constraint_violation); + max_constraint_violation = + Scalar(1e4) * max(Scalar(1), initial_constraint_violation); } /// Resets the filter. void reset() { m_filter.clear(); - - // Initial filter entry rejects constraint violations above max - m_filter.emplace_back(std::numeric_limits::infinity(), - max_constraint_violation); + m_last_rejection_due_to_filter = false; } - /// Adds a new entry to the filter. + /// Returns true if the given trial entry is acceptable to the filter. /// - /// @param entry The entry to add to the filter. - void add(const FilterEntry& entry) { - // Remove dominated entries - erase_if(m_filter, [&](const auto& elem) { - return entry.cost <= elem.cost && - entry.constraint_violation <= elem.constraint_violation; - }); - - m_filter.push_back(entry); - } - - /// Adds a new entry to the filter. - /// - /// @param entry The entry to add to the filter. - void add(FilterEntry&& entry) { - // Remove dominated entries - erase_if(m_filter, [&](const auto& elem) { - return entry.cost <= elem.cost && - entry.constraint_violation <= elem.constraint_violation; - }); - - m_filter.push_back(entry); - } - - /// Returns true if the given iterate is accepted by the filter. - /// - /// @param entry The entry to attempt adding to the filter. - /// @param α The step size (0, 1]. - /// @return True if the given iterate is accepted by the filter. - bool try_add(const FilterEntry& entry, Scalar α) { - if (is_acceptable(entry, α)) { - add(entry); - return true; - } else { - return false; - } - } - - /// Returns true if the given iterate is accepted by the filter. - /// - /// @param entry The entry to attempt adding to the filter. - /// @param α The step size (0, 1]. - /// @return True if the given iterate is accepted by the filter. - bool try_add(FilterEntry&& entry, Scalar α) { - if (is_acceptable(entry, α)) { - add(std::move(entry)); - return true; - } else { - return false; - } - } - - /// Returns true if the given entry is acceptable to the filter. - /// - /// @param entry The entry to check. + /// @param current_entry The entry corresponding to the current iterate. + /// @param trial_entry The entry corresponding to the trial iterate. + /// @param p_x Decision variable primal step. + /// @param g Cost function gradient. /// @param α The step size (0, 1]. /// @return True if the given entry is acceptable to the filter. - bool is_acceptable(const FilterEntry& entry, Scalar α) { + bool try_add(const FilterEntry& current_entry, + const FilterEntry& trial_entry, const DenseVector& p_x, + const SparseVector& g, Scalar α) { using std::isfinite; using std::pow; - if (!isfinite(entry.cost) || !isfinite(entry.constraint_violation)) { + // Reject steps with nonfinite cost or constraint violation above maximum + if (!isfinite(trial_entry.cost) || + trial_entry.constraint_violation > max_constraint_violation) { return false; } - Scalar ϕ = pow(α, Scalar(1.5)); + Scalar g_p_x = g.transpose() * p_x; - // If current filter entry is better than all prior ones in some respect, - // accept it. + // Switching condition + constexpr Scalar s_ϕ(2.3); + constexpr Scalar s_θ(1.1); + bool switching_condition = + g_p_x < Scalar(0) && + α * pow(-g_p_x, s_ϕ) > pow(current_entry.constraint_violation, s_θ); + + // Armijo condition + constexpr Scalar η_ϕ(1e-8); + bool armijo_condition = + trial_entry.cost <= current_entry.cost + η_ϕ * α * g_p_x; + + // Sufficient decrease condition // // See equation (2.13) of [4]. - return std::ranges::all_of(m_filter, [&](const auto& elem) { - return entry.cost <= elem.cost - ϕ * γ_cost * elem.constraint_violation || - entry.constraint_violation <= - (Scalar(1) - ϕ * γ_constraint) * elem.constraint_violation; - }); + Scalar ϕ = pow(α, Scalar(1.5)); + bool sufficient_decrease = + trial_entry.cost <= + current_entry.cost - + ϕ * γ_cost * current_entry.constraint_violation || + trial_entry.constraint_violation <= + (Scalar(1) - ϕ * γ_constraint) * current_entry.constraint_violation; + + // If constraint violation is below threshold and switching condition is + // true, check Armijo condition for step rejection. Otherwise, check + // sufficient decrease condition. + if (current_entry.constraint_violation <= min_constraint_violation && + switching_condition) { + if (!armijo_condition) { + m_last_rejection_due_to_filter = false; + return false; + } + } else if (!sufficient_decrease) { + m_last_rejection_due_to_filter = false; + return false; + } + + // Reject steps in filter (i.e., dominated by any filter entry) + if (in_filter(trial_entry)) { + m_last_rejection_due_to_filter = true; + return false; + } + + // Augment filter with accepted iterate if switching condition or Armijo + // condition are false + if (!switching_condition || !armijo_condition) { + add(FilterEntry{ + current_entry.cost - ϕ * γ_cost * current_entry.constraint_violation, + (Scalar(1) - ϕ * γ_constraint) * current_entry.constraint_violation}); + } + + return true; } - /// Returns the most recently added filter entry. + /// Returns true if the most recent trial entry rejection was due to the + /// filter. /// - /// @return The most recently added filter entry. - const FilterEntry& back() const { return m_filter.back(); } + /// @return True if the most recent trial entry rejection was due to the + /// filter. + bool last_rejection_due_to_filter() const { + return m_last_rejection_due_to_filter; + } private: static constexpr Scalar γ_cost{1e-8}; static constexpr Scalar γ_constraint{1e-5}; gch::small_vector> m_filter; + + bool m_last_rejection_due_to_filter = false; + + /// Adds a new entry to the filter. + /// + /// @param entry The entry to add to the filter. + void add(const FilterEntry& entry) { + // Remove dominated entries + erase_if(m_filter, + [&](const auto& elem) { return elem.dominated_by(entry); }); + + m_filter.push_back(entry); + } + + /// Returns true if the given entry is in the filter. + /// + /// @param entry The entry. + /// @return True if the given entry is in the filter. + bool in_filter(const FilterEntry& entry) const { + // An entry is in the filter if it's dominated by any filter entry + return std::any_of(m_filter.begin(), m_filter.end(), [&](const auto& elem) { + return entry.dominated_by(elem); + }); + } }; } // namespace slp 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 7feea13236..8f429be5f3 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 @@ -2,6 +2,8 @@ #pragma once +#include + #include #include @@ -9,47 +11,75 @@ namespace slp { +/// Type of KKT error to compute. +enum class KKTErrorType { + /// ∞-norm of scaled KKT condition errors. + INF_NORM_SCALED, + /// 1-norm of KKT condition errors. + ONE_NORM +}; + /// 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. -template +template Scalar kkt_error(const Eigen::Vector& g) { - // Compute the KKT error as the 1-norm of the KKT conditions from equations - // (19.5a) through (19.5d) of [1]. + // The KKT conditions from docs/algorithms.md: // // ∇f = 0 - return g.template lpNorm<1>(); + if constexpr (T == KKTErrorType::INF_NORM_SCALED) { + return g.template lpNorm(); + } else if constexpr (T == KKTErrorType::ONE_NORM) { + return g.template lpNorm<1>(); + } } /// 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. /// @param y Equality constraint dual variables. -template +template Scalar kkt_error(const Eigen::Vector& g, const Eigen::SparseMatrix& A_e, const Eigen::Vector& c_e, const Eigen::Vector& y) { - // Compute the KKT error as the 1-norm of the KKT conditions from equations - // (19.5a) through (19.5d) of [1]. + // The KKT conditions from docs/algorithms.md: // // ∇f − Aₑᵀy = 0 // cₑ = 0 - return (g - A_e.transpose() * y).template lpNorm<1>() + - c_e.template lpNorm<1>(); + if constexpr (T == KKTErrorType::INF_NORM_SCALED) { + // See equation (5) of [2]. + + // s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ + constexpr Scalar s_max(100); + Scalar s_d = + std::max(s_max, y.template lpNorm<1>() / Scalar(y.rows())) / s_max; + + // ‖∇f − Aₑᵀy‖_∞ / s_d + // ‖cₑ‖_∞ + return std::max( + {(g - A_e.transpose() * y).template lpNorm() / s_d, + c_e.template lpNorm()}); + } else if constexpr (T == KKTErrorType::ONE_NORM) { + return (g - A_e.transpose() * y).template lpNorm<1>() + + c_e.template lpNorm<1>(); + } } /// 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. @@ -63,7 +93,7 @@ Scalar kkt_error(const Eigen::Vector& g, /// @param y Equality constraint dual variables. /// @param z Inequality constraint dual variables. /// @param μ Barrier parameter. -template +template Scalar kkt_error(const Eigen::Vector& g, const Eigen::SparseMatrix& A_e, const Eigen::Vector& c_e, @@ -72,21 +102,51 @@ Scalar kkt_error(const Eigen::Vector& g, const Eigen::Vector& s, const Eigen::Vector& y, const Eigen::Vector& z, Scalar μ) { - // Compute the KKT error as the 1-norm of the KKT conditions from equations - // (19.5a) through (19.5d) of [1]. + // The KKT conditions from docs/algorithms.md: // // ∇f − Aₑᵀy − Aᵢᵀz = 0 // Sz − μe = 0 // cₑ = 0 // cᵢ − s = 0 - const auto S = s.asDiagonal(); - const Eigen::Vector μe = - Eigen::Vector::Constant(s.rows(), μ); + if constexpr (T == KKTErrorType::INF_NORM_SCALED) { + // See equation (5) of [2]. - return (g - A_e.transpose() * y - A_i.transpose() * z).template lpNorm<1>() + - (S * z - μe).template lpNorm<1>() + c_e.template lpNorm<1>() + - (c_i - s).template lpNorm<1>(); + // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ + constexpr Scalar s_max(100); + Scalar s_d = + std::max(s_max, (y.template lpNorm<1>() + z.template lpNorm<1>()) / + Scalar(y.rows() + z.rows())) / + s_max; + + // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ + Scalar s_c = + std::max(s_max, z.template lpNorm<1>() / Scalar(z.rows())) / s_max; + + const auto S = s.asDiagonal(); + const Eigen::Vector μe = + Eigen::Vector::Constant(s.rows(), μ); + + // ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d + // ‖Sz − μe‖_∞ / s_c + // ‖cₑ‖_∞ + // ‖cᵢ − s‖_∞ + return std::max({(g - A_e.transpose() * y - A_i.transpose() * z) + .template lpNorm() / + s_d, + (S * z - μe).template lpNorm() / s_c, + c_e.template lpNorm(), + (c_i - s).template lpNorm()}); + } else if constexpr (T == KKTErrorType::ONE_NORM) { + const auto S = s.asDiagonal(); + const Eigen::Vector μe = + Eigen::Vector::Constant(s.rows(), μ); + + return (g - A_e.transpose() * y - A_i.transpose() * z) + .template lpNorm<1>() + + (S * z - μe).template lpNorm<1>() + c_e.template lpNorm<1>() + + (c_i - s).template lpNorm<1>(); + } } } // 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 new file mode 100644 index 0000000000..55ad3d28b8 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp @@ -0,0 +1,135 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include + +#include +#include +#include + +#include "sleipnir/optimization/solver/util/append_as_triplets.hpp" + +namespace slp { + +/// Lagrange multiplier estimates. +/// +/// @tparam Scalar Scalar type. +template +struct LagrangeMultiplierEstimate { + /// Equality constraint dual estimate. + Eigen::Vector y; + /// Inequality constraint dual estimate. + Eigen::Vector z; +}; + +/// 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. +template +Eigen::Vector lagrange_multiplier_estimate( + const Eigen::SparseVector& g, + const Eigen::SparseMatrix& A_e) { + // Lagrange multiplier estimates + // + // ∇f − Aₑᵀy = 0 + // Aₑᵀy = ∇f + // y = (AₑAₑᵀ)⁻¹Aₑ∇f + return Eigen::SimplicialLDLT>{A_e * + A_e.transpose()} + .solve(A_e * g); +} + +/// 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 s Inequality constraint slack variables. +/// @param μ Barrier parameter. +template +LagrangeMultiplierEstimate lagrange_multiplier_estimate( + const Eigen::SparseVector& g, + const Eigen::SparseMatrix& A_e, + const Eigen::SparseMatrix& A_i, + const Eigen::Vector& s, Scalar μ) { + using DenseVector = Eigen::Vector; + using SparseMatrix = Eigen::SparseMatrix; + + // Lagrange multiplier estimates + // + // ∇f − Aₑᵀy − Aᵢᵀz = 0 + // Sz − μe = 0 + // + // Aₑᵀy + Aᵢᵀz = ∇f + // −Sz = −μe + // + // [Aₑᵀ Aᵢᵀ][y] = [ ∇f] + // [ 0 −S ][z] [−μe] + // + // [Aₑ 0]ᵀ[y] = [ ∇f] + // [Aᵢ −S] [z] [−μe] + // + // Let  = [Aₑ 0] + // [Aᵢ −S] + // + // Âᵀ[y] = [ ∇f] + // [z] [−μe] + // + // [y] = (ÂÂᵀ)⁻¹Â[ ∇f] + // [z] [−μe] + + gch::small_vector> triplets; + + //  = [Aₑ 0] + // [Aᵢ −S] + triplets.reserve(A_e.nonZeros() + A_i.nonZeros() + s.rows()); + append_as_triplets(triplets, 0, 0, {A_e, A_i}); + append_diagonal_as_triplets(triplets, A_e.rows(), A_i.cols(), (-s).eval()); + SparseMatrix A_hat{A_e.rows() + A_i.rows(), A_e.cols() + s.rows()}; + A_hat.setFromSortedTriplets(triplets.begin(), triplets.end()); + + // lhs = ÂÂᵀ + SparseMatrix lhs = A_hat * A_hat.transpose(); + + // rhs = Â[ ∇f] + // [−μe] + DenseVector rhs_temp{g.rows() + s.rows()}; + rhs_temp.segment(0, g.rows()) = g; + rhs_temp.segment(g.rows(), s.rows()).setConstant(-μ); + DenseVector rhs = A_hat * rhs_temp; + + Eigen::SimplicialLDLT yz_estimator{lhs}; + DenseVector sol = yz_estimator.solve(rhs); + DenseVector y = sol.segment(0, A_e.rows()); + DenseVector z = sol.segment(A_e.rows(), s.rows()); + + // A requirement for the convergence proof is that the primal-dual barrier + // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal barrier + // term Hessian μSₖ₊₁⁻². + // + // Σₖ₊₁ = μSₖ₊₁⁻² + // Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻² + // Zₖ₊₁ = μSₖ₊₁⁻¹ + // + // We ensure this by resetting + // + // zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁) + // + // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2]. + for (int row = 0; row < z.rows(); ++row) { + constexpr Scalar κ_Σ(1e10); + z[row] = std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]); + } + + return {std::move(y), std::move(z)}; +} + +} // 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 43a29285ee..4f9e7a7180 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 @@ -36,6 +36,19 @@ class RegularizedLDLT { : m_num_decision_variables{num_decision_variables}, m_num_equality_constraints{num_equality_constraints} {} + /// Constructs a RegularizedLDLT 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. + 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} {} + /// Reports whether previous computation was successful. /// /// @return Whether previous computation was successful. @@ -55,14 +68,14 @@ class RegularizedLDLT { m_info = m_is_sparse ? compute_sparse(lhs).info() : m_dense_solver.compute(lhs).info(); - Inertia inertia; - if (m_info == Eigen::Success) { - inertia = m_is_sparse ? Inertia{m_sparse_solver.vectorD()} - : Inertia{m_dense_solver.vectorD()}; + auto D = + m_is_sparse ? m_sparse_solver.vectorD() : m_dense_solver.vectorD(); - // If the inertia is ideal, don't regularize the system - if (inertia == ideal_inertia) { + // 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; } @@ -73,9 +86,11 @@ class RegularizedLDLT { // 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 γ(1e-10); + Scalar γ = m_γ_min; while (true) { + Inertia inertia; + // Regularize lhs by adding a multiple of the identity matrix // // lhs = [H + AᵢᵀΣAᵢ + δI Aₑᵀ] @@ -109,7 +124,7 @@ class RegularizedLDLT { } else if (inertia.positive > ideal_inertia.positive) { // If there's too many positive eigenvalues, increase γ by an order of // magnitude and try again - γ *= Scalar(10); + γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10); } } else { // If the decomposition failed, increase δ and γ by an order of @@ -132,7 +147,7 @@ class RegularizedLDLT { /// @param rhs Right-hand side of the system. /// @return The solution. template - DenseVector solve(const Eigen::MatrixBase& rhs) { + DenseVector solve(const Eigen::MatrixBase& rhs) const { if (m_is_sparse) { return m_sparse_solver.solve(rhs); } else { @@ -145,7 +160,7 @@ class RegularizedLDLT { /// @param rhs Right-hand side of the system. /// @return The solution. template - DenseVector solve(const Eigen::SparseMatrixBase& rhs) { + DenseVector solve(const Eigen::SparseMatrixBase& rhs) const { if (m_is_sparse) { return m_sparse_solver.solve(rhs); } else { @@ -174,6 +189,9 @@ class RegularizedLDLT { /// 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}; @@ -206,7 +224,7 @@ class RegularizedLDLT { /// @param δ The Hessian regularization factor. /// @param γ The equality constraint Jacobian regularization factor. /// @return Regularization matrix. - SparseMatrix regularization(Scalar δ, Scalar γ) { + 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) diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp index 4f0203a4b8..783a0e71a1 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp @@ -8,7 +8,7 @@ #include -/// Throw an exception in Python. +/// Throws an exception in Python. #define slp_assert(condition) \ do { \ if (!(condition)) { \ @@ -21,6 +21,6 @@ #else #include -/// Abort in C++. +/// Aborts in C++. #define slp_assert(condition) assert(condition) #endif diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp index f42866030f..55cd08acec 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp @@ -75,7 +75,7 @@ class function_ref { std::swap(callback_, rhs.callback_); } - /// Call the stored callable with the given arguments. + /// Calls the stored callable with the given arguments. /// /// @param args The arguments. /// @return The return value of the callable. 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 c50d7e171b..698a8df43d 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 @@ -16,8 +16,8 @@ #include #include "sleipnir/util/print.hpp" -#include "sleipnir/util/setup_profiler.hpp" -#include "sleipnir/util/solve_profiler.hpp" +#include "sleipnir/util/profiler.hpp" +#include "sleipnir/util/to_underlying.hpp" namespace slp { @@ -28,7 +28,9 @@ enum class IterationType : uint8_t { /// Accepted second-order correction iteration. ACCEPTED_SOC, /// Rejected second-order correction iteration. - REJECTED_SOC + REJECTED_SOC, + /// Feasibility restoration iteration. + FEASIBILITY_RESTORATION }; /// Converts std::chrono::duration to a number of milliseconds rounded to three @@ -234,13 +236,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"}; + constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC", "rest"}; slp::println( "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} " "{:.2e} {:2d}│", - iterations, ITERATION_TYPES[static_cast(type)], to_ms(time), - error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, - dual_α, backtracks); + iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error, + cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α, + backtracks); } #else #define print_iteration_diagnostics(...) diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/profiler.hpp new file mode 100644 index 0000000000..68bdd1ae9b --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/profiler.hpp @@ -0,0 +1,187 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include +#include +#include +#include +#include + +namespace slp { + +/// Records the number of profiler measurements (start/stop pairs) and the +/// average duration between each start and stop call. +class SetupProfiler { + public: + /// Constructs a SetupProfiler. + /// + /// @param name Name of measurement to show in diagnostics. + explicit SetupProfiler(std::string_view name) : m_name{name} {} + + /// Starts setup time measurement. + void start() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_setup_start_time = std::chrono::steady_clock::now(); +#endif + } + + /// Stops setup time measurement. + void stop() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_setup_stop_time = std::chrono::steady_clock::now(); + m_setup_duration = m_setup_stop_time - m_setup_start_time; +#endif + } + + /// Returns name of measurement to show in diagnostics. + /// + /// @return Name of measurement to show in diagnostics. + std::string_view name() const { return m_name; } + + /// Returns the setup duration in milliseconds as a double. + /// + /// @return The setup duration in milliseconds as a double. + const std::chrono::duration& duration() const { + return m_setup_duration; + } + + private: + /// Name of measurement to show in diagnostics. + std::string m_name; + + std::chrono::steady_clock::time_point m_setup_start_time; + std::chrono::steady_clock::time_point m_setup_stop_time; + std::chrono::duration m_setup_duration{0.0}; +}; + +/// Records the number of profiler measurements (start/stop pairs) and the +/// average duration between each start and stop call. +class SolveProfiler { + public: + /// Constructs a SolveProfiler. + /// + /// @param name Name of measurement to show in diagnostics. + explicit SolveProfiler(std::string_view name) : m_name{name} {} + + /// Starts solve time measurement. + void start() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_current_solve_start_time = std::chrono::steady_clock::now(); +#endif + } + + /// Stops solve time measurement, increments the number of averages, and + /// incorporates the latest measurement into the average. + void stop() { +#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS + m_current_solve_stop_time = std::chrono::steady_clock::now(); + m_current_solve_duration = + m_current_solve_stop_time - m_current_solve_start_time; + m_total_solve_duration += m_current_solve_duration; + + ++m_num_solves; + m_average_solve_duration = + (m_num_solves - 1.0) / m_num_solves * m_average_solve_duration + + 1.0 / m_num_solves * m_current_solve_duration; +#endif + } + + /// Returns name of measurement to show in diagnostics. + /// + /// @return Name of measurement to show in diagnostics. + std::string_view name() const { return m_name; } + + /// Returns the number of solves. + /// + /// @return The number of solves. + int num_solves() const { return m_num_solves; } + + /// Returns the most recent solve duration in seconds. + /// + /// @return The most recent solve duration in seconds. + const std::chrono::duration& current_duration() const { + return m_current_solve_duration; + } + + /// Returns the average solve duration in seconds. + /// + /// @return The average solve duration in seconds. + const std::chrono::duration& average_duration() const { + return m_average_solve_duration; + } + + /// Returns the sum of all solve durations in seconds. + /// + /// @return The sum of all solve durations in seconds. + const std::chrono::duration& total_duration() const { + return m_total_solve_duration; + } + + private: + /// Name of measurement to show in diagnostics. + std::string m_name; + + std::chrono::steady_clock::time_point m_current_solve_start_time; + std::chrono::steady_clock::time_point m_current_solve_stop_time; + std::chrono::duration m_current_solve_duration{0.0}; + std::chrono::duration m_total_solve_duration{0.0}; + + int m_num_solves = 0; + std::chrono::duration m_average_solve_duration{0.0}; +}; + +/// Starts a profiler in the constructor and stops it in the destructor. +template + requires std::same_as || + std::same_as +class ScopedProfiler { + public: + /// Starts a profiler. + /// + /// @param profiler The profiler. + explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} { + m_profiler->start(); + } + + /// Stops a profiler. + ~ScopedProfiler() { + if (m_active) { + m_profiler->stop(); + } + } + + /// Move constructor. + /// + /// @param rhs The other ScopedProfiler. + ScopedProfiler(ScopedProfiler&& rhs) noexcept + : m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} { + rhs.m_active = false; + } + + ScopedProfiler(const ScopedProfiler&) = delete; + ScopedProfiler& operator=(const ScopedProfiler&) = delete; + + /// Stops the profiler. + /// + /// If this is called, the destructor is a no-op. + void stop() { + if (m_active) { + m_profiler->stop(); + m_active = false; + } + } + + /// Returns the most recent solve duration in milliseconds as a double. + /// + /// @return The most recent solve duration in milliseconds as a double. + const std::chrono::duration& current_duration() const { + return m_profiler->current_duration(); + } + + private: + Profiler* m_profiler; + bool m_active = true; +}; + +} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp deleted file mode 100644 index f84e0f38ef..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include - -#include "sleipnir/util/setup_profiler.hpp" -#include "sleipnir/util/solve_profiler.hpp" - -namespace slp { - -/// Starts a profiler in the constructor and stops it in the destructor. -template - requires std::same_as || - std::same_as -class ScopedProfiler { - public: - /// Starts a profiler. - /// - /// @param profiler The profiler. - explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} { - m_profiler->start(); - } - - /// Stops a profiler. - ~ScopedProfiler() { - if (m_active) { - m_profiler->stop(); - } - } - - /// Move constructor. - /// - /// @param rhs The other ScopedProfiler. - ScopedProfiler(ScopedProfiler&& rhs) noexcept - : m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} { - rhs.m_active = false; - } - - ScopedProfiler(const ScopedProfiler&) = delete; - ScopedProfiler& operator=(const ScopedProfiler&) = delete; - - /// Stops the profiler. - /// - /// If this is called, the destructor is a no-op. - void stop() { - if (m_active) { - m_profiler->stop(); - m_active = false; - } - } - - /// Returns the most recent solve duration in milliseconds as a double. - /// - /// @return The most recent solve duration in milliseconds as a double. - const std::chrono::duration& current_duration() const { - return m_profiler->current_duration(); - } - - private: - Profiler* m_profiler; - bool m_active = true; -}; - -} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp deleted file mode 100644 index f5a0e75259..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include - -namespace slp { - -/// Records the number of profiler measurements (start/stop pairs) and the -/// average duration between each start and stop call. -class SetupProfiler { - public: - /// Constructs a SetupProfiler. - /// - /// @param name Name of measurement to show in diagnostics. - explicit SetupProfiler(std::string_view name) : m_name{name} {} - - /// Tell the profiler to start measuring setup time. - void start() { -#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - m_setup_start_time = std::chrono::steady_clock::now(); -#endif - } - - /// Tell the profiler to stop measuring setup time. - void stop() { -#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - m_setup_stop_time = std::chrono::steady_clock::now(); - m_setup_duration = m_setup_stop_time - m_setup_start_time; -#endif - } - - /// Returns name of measurement to show in diagnostics. - /// - /// @return Name of measurement to show in diagnostics. - std::string_view name() const { return m_name; } - - /// Returns the setup duration in milliseconds as a double. - /// - /// @return The setup duration in milliseconds as a double. - const std::chrono::duration& duration() const { - return m_setup_duration; - } - - private: - /// Name of measurement to show in diagnostics. - std::string m_name; - - std::chrono::steady_clock::time_point m_setup_start_time; - std::chrono::steady_clock::time_point m_setup_stop_time; - std::chrono::duration m_setup_duration{0.0}; -}; - -} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp deleted file mode 100644 index d6f6beeb9a..0000000000 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright (c) Sleipnir contributors - -#pragma once - -#include -#include -#include - -namespace slp { - -/// Records the number of profiler measurements (start/stop pairs) and the -/// average duration between each start and stop call. -class SolveProfiler { - public: - /// Constructs a SolveProfiler. - /// - /// @param name Name of measurement to show in diagnostics. - explicit SolveProfiler(std::string_view name) : m_name{name} {} - - /// Tell the profiler to start measuring solve time. - void start() { -#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - m_current_solve_start_time = std::chrono::steady_clock::now(); -#endif - } - - /// Tell the profiler to stop measuring solve time, increment the number of - /// averages, and incorporate the latest measurement into the average. - void stop() { -#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS - m_current_solve_stop_time = std::chrono::steady_clock::now(); - m_current_solve_duration = - m_current_solve_stop_time - m_current_solve_start_time; - m_total_solve_duration += m_current_solve_duration; - - ++m_num_solves; - m_average_solve_duration = - (m_num_solves - 1.0) / m_num_solves * m_average_solve_duration + - 1.0 / m_num_solves * m_current_solve_duration; -#endif - } - - /// Returns name of measurement to show in diagnostics. - /// - /// @return Name of measurement to show in diagnostics. - std::string_view name() const { return m_name; } - - /// Returns the number of solves. - /// - /// @return The number of solves. - int num_solves() const { return m_num_solves; } - - /// Returns the most recent solve duration in seconds. - /// - /// @return The most recent solve duration in seconds. - const std::chrono::duration& current_duration() const { - return m_current_solve_duration; - } - - /// Returns the average solve duration in seconds. - /// - /// @return The average solve duration in seconds. - const std::chrono::duration& average_duration() const { - return m_average_solve_duration; - } - - /// Returns the sum of all solve durations in seconds. - /// - /// @return The sum of all solve durations in seconds. - const std::chrono::duration& total_duration() const { - return m_total_solve_duration; - } - - private: - /// Name of measurement to show in diagnostics. - std::string m_name; - - std::chrono::steady_clock::time_point m_current_solve_start_time; - std::chrono::steady_clock::time_point m_current_solve_stop_time; - std::chrono::duration m_current_solve_duration{0.0}; - std::chrono::duration m_total_solve_duration{0.0}; - - int m_num_solves = 0; - std::chrono::duration m_average_solve_duration{0.0}; -}; - -} // namespace slp diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp new file mode 100644 index 0000000000..3f9b4835b9 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp @@ -0,0 +1,14 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +namespace slp { + +template +constexpr std::underlying_type_t to_underlying(Enum e) noexcept { + return static_cast>(e); +} + +} // namespace slp diff --git a/wpimath/src/test/native/cpp/optimization/NonlinearProblemTest.cpp b/wpimath/src/test/native/cpp/optimization/NonlinearProblemTest.cpp index 0a23a6a1c3..28b1171c69 100644 --- a/wpimath/src/test/native/cpp/optimization/NonlinearProblemTest.cpp +++ b/wpimath/src/test/native/cpp/optimization/NonlinearProblemTest.cpp @@ -135,14 +135,7 @@ TEST(ProblemTest, Minimum2DDistanceWithLinearConstraint) { EXPECT_EQ(problem.equality_constraint_type(), slp::ExpressionType::LINEAR); EXPECT_EQ(problem.inequality_constraint_type(), slp::ExpressionType::NONE); -#if defined(__linux__) && defined(__aarch64__) - // FIXME: Fails on Linux aarch64 with "line search failed" - EXPECT_EQ(problem.solve({.diagnostics = true}), - slp::ExitStatus::LINE_SEARCH_FAILED); - return; -#else EXPECT_EQ(problem.solve({.diagnostics = true}), slp::ExitStatus::SUCCESS); -#endif EXPECT_NEAR(x.value(), 2.5, 1e-2); EXPECT_NEAR(y.value(), 2.5, 1e-2); @@ -208,13 +201,11 @@ TEST(ProblemTest, WachterAndBieglerLineSearchFailure) { EXPECT_EQ(problem.equality_constraint_type(), slp::ExpressionType::QUADRATIC); EXPECT_EQ(problem.inequality_constraint_type(), slp::ExpressionType::LINEAR); - // FIXME: Fails with "line search failed" - EXPECT_EQ(problem.solve({.diagnostics = true}), - slp::ExitStatus::LINE_SEARCH_FAILED); + EXPECT_EQ(problem.solve({.diagnostics = true}), slp::ExitStatus::SUCCESS); - // EXPECT_EQ(x.value(), 1.0); - // EXPECT_EQ(s1.value(), 0.0); - // EXPECT_EQ(s2.value(), 0.5); + EXPECT_NEAR(x.value(), 1.0, 1e-6); + EXPECT_NEAR(s1.value(), 0.0, 1e-6); + EXPECT_NEAR(s2.value(), 0.5, 1e-6); if (auto output = testing::internal::GetCapturedStdout(); HasFailure()) { fmt::println("{}", output);