From ebbf90308d46fd8412f514cc5855129ff85f9aae Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 16:34:48 -0800 Subject: [PATCH] [upstream_utils] Upgrade Sleipnir It now uses SQP for problems without inequality constraints, which is faster. main: ``` [ RUN ] Ellipse2dTest.DistanceToPoint 0.203 ms [ OK ] Ellipse2dTest.DistanceToPoint (0 ms) [ RUN ] Ellipse2dTest.FindNearestPoint 0.019 ms [ OK ] Ellipse2dTest.FindNearestPoint (0 ms) ``` upgrade: ``` [ RUN ] Ellipse2dTest.DistanceToPoint 0.197 ms [ OK ] Ellipse2dTest.DistanceToPoint (0 ms) [ RUN ] Ellipse2dTest.FindNearestPoint 0.015 ms [ OK ] Ellipse2dTest.FindNearestPoint (0 ms) ``` --- upstream_utils/sleipnir.py | 4 +- .../0002-Use-wpi-SmallVector.patch | 126 +++- .../optimization/OptimizationProblem.hpp | 14 +- .../sleipnir/optimization/solver/SQP.hpp | 46 ++ .../src/optimization/solver/InteriorPoint.cpp | 17 +- .../sleipnir/src/optimization/solver/SQP.cpp | 559 ++++++++++++++++++ .../solver/util/ErrorEstimate.hpp | 36 ++ .../solver/util/FeasibilityRestoration.hpp | 162 +++++ .../src/optimization/solver/util/Filter.hpp | 49 +- .../src/optimization/solver/util/KKTError.hpp | 22 + 10 files changed, 965 insertions(+), 70 deletions(-) create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp create mode 100644 wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index 9b5d3e6aae0..ac25370ff8d 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -48,8 +48,8 @@ def copy_upstream_src(wpilib_root): def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - # main on 2024-09-18 - tag = "8bbce85252bc351c5aacb0de9f50fa31b8b9e1ae" + # main on 2024-12-07 + tag = "01206ab17d741f4c45a7faeb56b8a5442df1681c" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch index 61e887791c8..6b816784a7a 100644 --- a/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch +++ b/upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch @@ -4,22 +4,23 @@ Date: Sun, 16 Jun 2024 12:08:49 -0700 Subject: [PATCH 2/3] Use wpi::SmallVector --- - include/.styleguide | 1 + - include/sleipnir/autodiff/Expression.hpp | 13 +++++++------ - include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++------- - include/sleipnir/autodiff/Hessian.hpp | 4 ++-- - include/sleipnir/autodiff/Jacobian.hpp | 10 +++++----- - include/sleipnir/autodiff/Variable.hpp | 10 +++++----- - include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++-- - include/sleipnir/optimization/Multistart.hpp | 7 ++++--- - .../sleipnir/optimization/OptimizationProblem.hpp | 8 ++++---- - include/sleipnir/util/Pool.hpp | 7 ++++--- - include/sleipnir/util/Spy.hpp | 4 ++-- - src/.styleguide | 1 + - src/optimization/solver/InteriorPoint.cpp | 4 ++-- - .../solver/util/FeasibilityRestoration.hpp | 10 +++++----- - src/optimization/solver/util/Filter.hpp | 4 ++-- - 15 files changed, 54 insertions(+), 48 deletions(-) + include/.styleguide | 1 + + include/sleipnir/autodiff/Expression.hpp | 13 +++++++------ + include/sleipnir/autodiff/ExpressionGraph.hpp | 15 ++++++++------- + include/sleipnir/autodiff/Hessian.hpp | 4 ++-- + include/sleipnir/autodiff/Jacobian.hpp | 10 +++++----- + include/sleipnir/autodiff/Variable.hpp | 10 +++++----- + include/sleipnir/autodiff/VariableMatrix.hpp | 4 ++-- + include/sleipnir/optimization/Multistart.hpp | 7 ++++--- + .../optimization/OptimizationProblem.hpp | 8 ++++---- + include/sleipnir/util/Pool.hpp | 7 ++++--- + include/sleipnir/util/Spy.hpp | 4 ++-- + src/.styleguide | 1 + + src/optimization/solver/InteriorPoint.cpp | 4 ++-- + src/optimization/solver/SQP.cpp | 4 ++-- + .../solver/util/FeasibilityRestoration.hpp | 18 +++++++++--------- + src/optimization/solver/util/Filter.hpp | 4 ++-- + 16 files changed, 60 insertions(+), 54 deletions(-) diff --git a/include/.styleguide b/include/.styleguide index 6a7f8ed28f9cb037c9746a7e0ef5e110481d9825..efa36cee1fb593ae810032340c64f1854fbbc523 100644 @@ -334,7 +335,7 @@ index 8055713a2492a9c8473f047a2fb9fe7ca57753c3..09b1b2f3bf33c35ae0aeddb9b5d47c0d for (auto& future : futures) { diff --git a/include/sleipnir/optimization/OptimizationProblem.hpp b/include/sleipnir/optimization/OptimizationProblem.hpp -index 28b2943f5842229335e79eae14abbda6ff5b7000..e812fa5e3454903d4dfb8572539ebf1b506bdf11 100644 +index 569dcdee507881ceb412585ca811927072552c15..66883fed98ad087010fb153bd91effce6e047928 100644 --- a/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/include/sleipnir/optimization/OptimizationProblem.hpp @@ -11,6 +11,7 @@ @@ -345,15 +346,15 @@ index 28b2943f5842229335e79eae14abbda6ff5b7000..e812fa5e3454903d4dfb8572539ebf1b #include "sleipnir/autodiff/Variable.hpp" #include "sleipnir/autodiff/VariableMatrix.hpp" -@@ -21,7 +22,6 @@ - #include "sleipnir/optimization/solver/InteriorPoint.hpp" +@@ -22,7 +23,6 @@ + #include "sleipnir/optimization/solver/SQP.hpp" #include "sleipnir/util/Print.hpp" #include "sleipnir/util/SymbolExports.hpp" -#include "sleipnir/util/small_vector.hpp" namespace sleipnir { -@@ -358,16 +358,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { +@@ -364,16 +364,16 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { private: // The list of decision variables, which are the root of the problem's // expression tree @@ -434,7 +435,7 @@ index f3b2f0cf9e60b3a86b9654ff2b381f9c48734ff6..ad739cea6dce6f6cb586f538d1d30b92 + ^wpi/ } diff --git a/src/optimization/solver/InteriorPoint.cpp b/src/optimization/solver/InteriorPoint.cpp -index dcd8b56dc08516b80f89550c43cb7002745b93d8..892d2dd20f7fe92c2c91518a4ecb487425643585 100644 +index a09d9866d05731c8ce53122b3d1a850803883df4..d3981c59d163927e3e5ba602c3323f6e1429c475 100644 --- a/src/optimization/solver/InteriorPoint.cpp +++ b/src/optimization/solver/InteriorPoint.cpp @@ -9,6 +9,7 @@ @@ -462,8 +463,37 @@ index dcd8b56dc08516b80f89550c43cb7002745b93d8..892d2dd20f7fe92c2c91518a4ecb4874 RegularizedLDLT solver; +diff --git a/src/optimization/solver/SQP.cpp b/src/optimization/solver/SQP.cpp +index 77b9632e1da37361c995a8579d1d83a2756d6d88..662abc2fb6e311767b0fbb3a61121ce78549a3f6 100644 +--- a/src/optimization/solver/SQP.cpp ++++ b/src/optimization/solver/SQP.cpp +@@ -9,6 +9,7 @@ + #include + + #include ++#include + + #include "optimization/RegularizedLDLT.hpp" + #include "optimization/solver/util/ErrorEstimate.hpp" +@@ -22,7 +23,6 @@ + #include "sleipnir/optimization/SolverExitCondition.hpp" + #include "sleipnir/util/Print.hpp" + #include "sleipnir/util/Spy.hpp" +-#include "sleipnir/util/small_vector.hpp" + #include "util/ScopeExit.hpp" + #include "util/ToMilliseconds.hpp" + +@@ -155,7 +155,7 @@ void SQP(std::span decisionVariables, + Filter filter{f}; + + // Kept outside the loop so its storage can be reused +- small_vector> triplets; ++ wpi::SmallVector> triplets; + + RegularizedLDLT solver; + diff --git a/src/optimization/solver/util/FeasibilityRestoration.hpp b/src/optimization/solver/util/FeasibilityRestoration.hpp -index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d5498acaa18 100644 +index feefe137adf0832b094a36d61201b15962138ded..79b5d99ae27de6049ba098888a965291e6b677fa 100644 --- a/src/optimization/solver/util/FeasibilityRestoration.hpp +++ b/src/optimization/solver/util/FeasibilityRestoration.hpp @@ -8,6 +8,7 @@ @@ -482,7 +512,43 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 namespace sleipnir { -@@ -65,7 +65,7 @@ inline void FeasibilityRestoration( +@@ -57,7 +57,7 @@ inline void FeasibilityRestoration( + constexpr double ρ = 1000.0; + double μ = config.tolerance / 10.0; + +- small_vector fr_decisionVariables; ++ wpi::SmallVector fr_decisionVariables; + fr_decisionVariables.reserve(decisionVariables.size() + + 2 * equalityConstraints.size()); + +@@ -70,7 +70,7 @@ inline void FeasibilityRestoration( + fr_decisionVariables.emplace_back(); + } + +- auto it = fr_decisionVariables.cbegin(); ++ auto it = fr_decisionVariables.begin(); + + VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; + it += decisionVariables.size(); +@@ -128,7 +128,7 @@ inline void FeasibilityRestoration( + } + + // cₑ(x) - pₑ + nₑ = 0 +- small_vector fr_equalityConstraints; ++ wpi::SmallVector fr_equalityConstraints; + fr_equalityConstraints.assign(equalityConstraints.begin(), + equalityConstraints.end()); + for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { +@@ -137,7 +137,7 @@ inline void FeasibilityRestoration( + } + + // cᵢ(x) - s - pᵢ + nᵢ = 0 +- small_vector fr_inequalityConstraints; ++ wpi::SmallVector fr_inequalityConstraints; + + // pₑ ≥ 0 + std::copy(p_e.begin(), p_e.end(), +@@ -227,7 +227,7 @@ inline void FeasibilityRestoration( constexpr double ρ = 1000.0; @@ -491,7 +557,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 fr_decisionVariables.reserve(decisionVariables.size() + 2 * equalityConstraints.size() + 2 * inequalityConstraints.size()); -@@ -81,7 +81,7 @@ inline void FeasibilityRestoration( +@@ -243,7 +243,7 @@ inline void FeasibilityRestoration( fr_decisionVariables.emplace_back(); } @@ -500,7 +566,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; it += decisionVariables.size(); -@@ -157,7 +157,7 @@ inline void FeasibilityRestoration( +@@ -319,7 +319,7 @@ inline void FeasibilityRestoration( } // cₑ(x) - pₑ + nₑ = 0 @@ -509,7 +575,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 fr_equalityConstraints.assign(equalityConstraints.begin(), equalityConstraints.end()); for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { -@@ -166,7 +166,7 @@ inline void FeasibilityRestoration( +@@ -328,7 +328,7 @@ inline void FeasibilityRestoration( } // cᵢ(x) - s - pᵢ + nᵢ = 0 @@ -519,7 +585,7 @@ index c324ef093cc7dc8ce93af2cba337042a65b28475..0f13676aea0e80549ef1ef43e4972d54 inequalityConstraints.end()); for (size_t row = 0; row < fr_inequalityConstraints.size(); ++row) { diff --git a/src/optimization/solver/util/Filter.hpp b/src/optimization/solver/util/Filter.hpp -index 3fbb849edc4a6b3336f94b5af9e018a37b07b123..02bdb5a8db5c80dd86d17ea4421ec564d7e0a2c7 100644 +index f19236928c59623bc0a3ce87b659f0756997f821..0c14efd7b8afa6cef398f5a7d383c54dbf64ec68 100644 --- a/src/optimization/solver/util/Filter.hpp +++ b/src/optimization/solver/util/Filter.hpp @@ -8,9 +8,9 @@ @@ -531,12 +597,12 @@ index 3fbb849edc4a6b3336f94b5af9e018a37b07b123..02bdb5a8db5c80dd86d17ea4421ec564 #include "sleipnir/autodiff/Variable.hpp" -#include "sleipnir/util/small_vector.hpp" - namespace sleipnir { + // See docs/algorithms.md#Works_cited for citation definitions. + +@@ -177,7 +177,7 @@ class Filter { -@@ -182,7 +182,7 @@ class Filter { private: Variable* m_f = nullptr; - double m_μ = 0.0; - small_vector m_filter; + wpi::SmallVector m_filter; }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp index e812fa5e345..66883fed98a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/OptimizationProblem.hpp @@ -20,6 +20,7 @@ #include "sleipnir/optimization/SolverIterationInfo.hpp" #include "sleipnir/optimization/SolverStatus.hpp" #include "sleipnir/optimization/solver/InteriorPoint.hpp" +#include "sleipnir/optimization/solver/SQP.hpp" #include "sleipnir/util/Print.hpp" #include "sleipnir/util/SymbolExports.hpp" @@ -305,10 +306,15 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem { } // Solve the optimization problem - Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size()); - InteriorPoint(m_decisionVariables, m_equalityConstraints, - m_inequalityConstraints, m_f.value(), m_callback, config, - false, x, s, &status); + if (m_inequalityConstraints.empty()) { + SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback, + config, x, &status); + } else { + Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size()); + InteriorPoint(m_decisionVariables, m_equalityConstraints, + m_inequalityConstraints, m_f.value(), m_callback, config, + false, x, s, &status); + } if (config.diagnostics) { sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition)); 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 new file mode 100644 index 00000000000..ed10ffedff2 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/SQP.hpp @@ -0,0 +1,46 @@ +// Copyright (c) Sleipnir contributors + +#pragma once + +#include + +#include + +#include "sleipnir/autodiff/Variable.hpp" +#include "sleipnir/optimization/SolverConfig.hpp" +#include "sleipnir/optimization/SolverIterationInfo.hpp" +#include "sleipnir/optimization/SolverStatus.hpp" +#include "sleipnir/util/FunctionRef.hpp" +#include "sleipnir/util/SymbolExports.hpp" + +namespace sleipnir { + +/** +Finds the optimal solution to a nonlinear program using Sequential Quadratic +Programming (SQP). + +A nonlinear program has the form: + +@verbatim + min_x f(x) +subject to cₑ(x) = 0 +@endverbatim + +where f(x) is the cost function and cₑ(x) are the equality constraints. + +@param[in] decisionVariables The list of decision variables. +@param[in] equalityConstraints The list of equality constraints. +@param[in] f The cost function. +@param[in] callback The user callback. +@param[in] config Configuration options for the solver. +@param[in,out] x The initial guess and output location for the decision + variables. +@param[out] status The solver status. +*/ +SLEIPNIR_DLLEXPORT void SQP( + std::span decisionVariables, + std::span equalityConstraints, Variable& f, + function_ref callback, + const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status); + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp index 892d2dd20f7..d3981c59d16 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/InteriorPoint.cpp @@ -195,7 +195,7 @@ void InteriorPoint(std::span decisionVariables, // Fraction-to-the-boundary rule scale factor τ double τ = τ_min; - Filter filter{f, μ}; + Filter filter{f}; // This should be run when the error estimate is below a desired threshold for // the current barrier parameter @@ -222,7 +222,7 @@ void InteriorPoint(std::span decisionVariables, τ = std::max(τ_min, 1.0 - μ); // Reset the filter when the barrier parameter is updated - filter.Reset(μ); + filter.Reset(); }; // Kept outside the loop so its storage can be reused @@ -372,6 +372,9 @@ void InteriorPoint(std::span decisionVariables, rhs.segment(x.rows(), y.rows()) = -c_e; // Solve the Newton-KKT system + // + // [H + AᵢᵀΣAᵢ Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy + Aᵢᵀ(S⁻¹(Zcᵢ − μe) − z)] + // [ Aₑ 0 ][−pₖʸ] [ cₑ ] solver.Compute(lhs, equalityConstraints.size(), μ); Eigen::VectorXd step{x.rows() + y.rows()}; if (solver.Info() == Eigen::Success) { @@ -435,7 +438,7 @@ void InteriorPoint(std::span decisionVariables, } // Check whether filter accepts trial iterate - auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i); + auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); if (filter.TryAdd(entry)) { // Accept step break; @@ -499,7 +502,7 @@ void InteriorPoint(std::span decisionVariables, trial_c_i = c_iAD.Value(); // Check whether filter accepts trial iterate - entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i); + entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); if (filter.TryAdd(entry)) { p_x = p_x_cor; p_y = p_y_soc; @@ -531,7 +534,7 @@ void InteriorPoint(std::span decisionVariables, if (fullStepRejectedCounter >= 4 && filter.maxConstraintViolation > entry.constraintViolation / 10.0) { filter.maxConstraintViolation *= 0.1; - filter.Reset(μ); + filter.Reset(); continue; } @@ -580,7 +583,7 @@ void InteriorPoint(std::span decisionVariables, return; } - auto initialEntry = filter.MakeEntry(s, c_e, c_i); + auto initialEntry = filter.MakeEntry(s, c_e, c_i, μ); // Feasibility restoration phase Eigen::VectorXd fr_x = x; @@ -603,7 +606,7 @@ void InteriorPoint(std::span decisionVariables, // If current iterate is acceptable to normal filter and // constraint violation has sufficiently reduced, stop // feasibility restoration - auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i); + auto entry = filter.MakeEntry(trial_s, trial_c_e, trial_c_i, μ); if (filter.IsAcceptable(entry) && entry.constraintViolation < 0.9 * initialEntry.constraintViolation) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp new file mode 100644 index 00000000000..662abc2fb6e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/SQP.cpp @@ -0,0 +1,559 @@ +// Copyright (c) Sleipnir contributors + +#include "sleipnir/optimization/solver/SQP.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include "optimization/RegularizedLDLT.hpp" +#include "optimization/solver/util/ErrorEstimate.hpp" +#include "optimization/solver/util/FeasibilityRestoration.hpp" +#include "optimization/solver/util/Filter.hpp" +#include "optimization/solver/util/IsLocallyInfeasible.hpp" +#include "optimization/solver/util/KKTError.hpp" +#include "sleipnir/autodiff/Gradient.hpp" +#include "sleipnir/autodiff/Hessian.hpp" +#include "sleipnir/autodiff/Jacobian.hpp" +#include "sleipnir/optimization/SolverExitCondition.hpp" +#include "sleipnir/util/Print.hpp" +#include "sleipnir/util/Spy.hpp" +#include "util/ScopeExit.hpp" +#include "util/ToMilliseconds.hpp" + +// See docs/algorithms.md#Works_cited for citation definitions. + +namespace sleipnir { + +void SQP(std::span decisionVariables, + std::span equalityConstraints, Variable& f, + function_ref callback, + const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) { + const auto solveStartTime = std::chrono::system_clock::now(); + + // Map decision variables and constraints to VariableMatrices for Lagrangian + VariableMatrix xAD{decisionVariables}; + xAD.SetValue(x); + VariableMatrix c_eAD{equalityConstraints}; + + // Create autodiff variables for y for Lagrangian + VariableMatrix yAD(equalityConstraints.size()); + for (auto& y : yAD) { + y.SetValue(0.0); + } + + // Lagrangian L + // + // L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ) + auto L = f - (yAD.T() * c_eAD)(0); + + // Equality constraint Jacobian Aₑ + // + // [∇ᵀcₑ₁(xₖ)] + // Aₑ(x) = [∇ᵀcₑ₂(xₖ)] + // [ ⋮ ] + // [∇ᵀcₑₘ(xₖ)] + Jacobian jacobianCe{c_eAD, xAD}; + Eigen::SparseMatrix A_e = jacobianCe.Value(); + + // Gradient of f ∇f + Gradient gradientF{f, xAD}; + Eigen::SparseVector g = gradientF.Value(); + + // Hessian of the Lagrangian H + // + // Hₖ = ∇²ₓₓL(xₖ, yₖ) + Hessian hessianL{L, xAD}; + Eigen::SparseMatrix H = hessianL.Value(); + + Eigen::VectorXd y = yAD.Value(); + Eigen::VectorXd c_e = c_eAD.Value(); + + // Check for overconstrained problem + if (equalityConstraints.size() > decisionVariables.size()) { + if (config.diagnostics) { + sleipnir::println("The problem has too few degrees of freedom."); + sleipnir::println( + "Violated constraints (cₑ(x) = 0) in order of declaration:"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e(row) < 0.0) { + sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); + } + } + } + + status->exitCondition = SolverExitCondition::kTooFewDOFs; + return; + } + + // Check whether initial guess has finite f(xₖ) and cₑ(xₖ) + if (!std::isfinite(f.Value()) || !c_e.allFinite()) { + status->exitCondition = + SolverExitCondition::kNonfiniteInitialCostOrConstraints; + return; + } + + // Sparsity pattern files written when spy flag is set in SolverConfig + std::ofstream H_spy; + std::ofstream A_e_spy; + if (config.spy) { + A_e_spy.open("A_e.spy"); + H_spy.open("H.spy"); + } + + if (config.diagnostics) { + sleipnir::println("Error tolerance: {}\n", config.tolerance); + } + + std::chrono::system_clock::time_point iterationsStartTime; + + int iterations = 0; + + // Prints final diagnostics when the solver exits + scope_exit exit{[&] { + status->cost = f.Value(); + + if (config.diagnostics) { + auto solveEndTime = std::chrono::system_clock::now(); + + sleipnir::println("\nSolve time: {:.3f} ms", + ToMilliseconds(solveEndTime - solveStartTime)); + sleipnir::println(" ↳ {:.3f} ms (solver setup)", + ToMilliseconds(iterationsStartTime - solveStartTime)); + if (iterations > 0) { + sleipnir::println( + " ↳ {:.3f} ms ({} solver iterations; {:.3f} ms average)", + ToMilliseconds(solveEndTime - iterationsStartTime), iterations, + ToMilliseconds((solveEndTime - iterationsStartTime) / iterations)); + } + sleipnir::println(""); + + sleipnir::println("{:^8} {:^10} {:^14} {:^6}", "autodiff", + "setup (ms)", "avg solve (ms)", "solves"); + sleipnir::println("{:=^47}", ""); + constexpr auto format = "{:^8} {:10.3f} {:14.3f} {:6}"; + sleipnir::println(format, "∇f(x)", + gradientF.GetProfiler().SetupDuration(), + gradientF.GetProfiler().AverageSolveDuration(), + gradientF.GetProfiler().SolveMeasurements()); + sleipnir::println(format, "∇²ₓₓL", hessianL.GetProfiler().SetupDuration(), + hessianL.GetProfiler().AverageSolveDuration(), + hessianL.GetProfiler().SolveMeasurements()); + sleipnir::println(format, "∂cₑ/∂x", + jacobianCe.GetProfiler().SetupDuration(), + jacobianCe.GetProfiler().AverageSolveDuration(), + jacobianCe.GetProfiler().SolveMeasurements()); + sleipnir::println(""); + } + }}; + + Filter filter{f}; + + // Kept outside the loop so its storage can be reused + wpi::SmallVector> triplets; + + RegularizedLDLT solver; + + // Variables for determining when a step is acceptable + constexpr double α_red_factor = 0.5; + int acceptableIterCounter = 0; + + int fullStepRejectedCounter = 0; + + // Error estimate + double E_0 = std::numeric_limits::infinity(); + + if (config.diagnostics) { + iterationsStartTime = std::chrono::system_clock::now(); + } + + while (E_0 > config.tolerance && + acceptableIterCounter < config.maxAcceptableIterations) { + std::chrono::system_clock::time_point innerIterStartTime; + if (config.diagnostics) { + innerIterStartTime = std::chrono::system_clock::now(); + } + + // Check for local equality constraint infeasibility + if (IsEqualityLocallyInfeasible(A_e, c_e)) { + if (config.diagnostics) { + sleipnir::println( + "The problem is locally infeasible due to violated equality " + "constraints."); + sleipnir::println( + "Violated constraints (cₑ(x) = 0) in order of declaration:"); + for (int row = 0; row < c_e.rows(); ++row) { + if (c_e(row) < 0.0) { + sleipnir::println(" {}/{}: {} = 0", row + 1, c_e.rows(), c_e(row)); + } + } + } + + status->exitCondition = SolverExitCondition::kLocallyInfeasible; + return; + } + + // Check for diverging iterates + if (x.lpNorm() > 1e20 || !x.allFinite()) { + status->exitCondition = SolverExitCondition::kDivergingIterates; + return; + } + + // Write out spy file contents if that's enabled + if (config.spy) { + // Gap between sparsity patterns + if (iterations > 0) { + A_e_spy << "\n"; + H_spy << "\n"; + } + + Spy(H_spy, H); + Spy(A_e_spy, A_e); + } + + // Call user callback + if (callback({iterations, x, Eigen::VectorXd::Zero(0), g, H, A_e, + Eigen::SparseMatrix{}})) { + status->exitCondition = SolverExitCondition::kCallbackRequestedStop; + return; + } + + // lhs = [H Aₑᵀ] + // [Aₑ 0 ] + // + // Don't assign upper triangle because solver only uses lower triangle. + const Eigen::SparseMatrix topLeft = + H.triangularView(); + triplets.clear(); + triplets.reserve(topLeft.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 (Eigen::SparseMatrix::InnerIterator it{topLeft, col}; it; + ++it) { + triplets.emplace_back(it.row(), it.col(), it.value()); + } + // Append column of Aₑ in bottom-left quadrant + for (Eigen::SparseMatrix::InnerIterator it{A_e, col}; it; ++it) { + triplets.emplace_back(H.rows() + it.row(), it.col(), it.value()); + } + } + Eigen::SparseMatrix lhs( + decisionVariables.size() + equalityConstraints.size(), + decisionVariables.size() + equalityConstraints.size()); + lhs.setFromSortedTriplets(triplets.begin(), triplets.end(), + [](const auto&, const auto& b) { return b; }); + + // rhs = −[∇f − Aₑᵀy] + // [ cₑ ] + Eigen::VectorXd rhs{x.rows() + y.rows()}; + rhs.segment(0, x.rows()) = -(g - A_e.transpose() * y); + rhs.segment(x.rows(), y.rows()) = -c_e; + + // Solve the Newton-KKT system + // + // [H Aₑᵀ][ pₖˣ] = −[∇f − Aₑᵀy] + // [Aₑ 0 ][−pₖʸ] [ cₑ ] + solver.Compute(lhs, equalityConstraints.size(), config.tolerance / 10.0); + Eigen::VectorXd step{x.rows() + y.rows()}; + if (solver.Info() == Eigen::Success) { + step = solver.Solve(rhs); + } else { + // The regularization procedure failed due to a rank-deficient equality + // constraint Jacobian with linearly dependent constraints + status->exitCondition = SolverExitCondition::kLocallyInfeasible; + return; + } + + // step = [ pₖˣ] + // [−pₖʸ] + Eigen::VectorXd p_x = step.segment(0, x.rows()); + Eigen::VectorXd p_y = -step.segment(x.rows(), y.rows()); + + constexpr double α_max = 1.0; + double α = α_max; + + // Loop until a step is accepted. If a step becomes acceptable, the loop + // will exit early. + while (1) { + Eigen::VectorXd trial_x = x + α * p_x; + Eigen::VectorXd trial_y = y + α * p_y; + + xAD.SetValue(trial_x); + + Eigen::VectorXd trial_c_e = c_eAD.Value(); + + // If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size + // immediately + if (!std::isfinite(f.Value()) || !trial_c_e.allFinite()) { + // Reduce step size + α *= α_red_factor; + continue; + } + + // Check whether filter accepts trial iterate + auto entry = filter.MakeEntry(trial_c_e); + if (filter.TryAdd(entry)) { + // Accept step + break; + } + + double prevConstraintViolation = c_e.lpNorm<1>(); + double nextConstraintViolation = trial_c_e.lpNorm<1>(); + + // Second-order corrections + // + // If first trial point was rejected and constraint violation stayed the + // same or went up, apply second-order corrections + if (nextConstraintViolation >= prevConstraintViolation) { + // Apply second-order corrections. See section 2.4 of [2]. + Eigen::VectorXd p_x_cor = p_x; + Eigen::VectorXd p_y_soc = p_y; + + double α_soc = α; + Eigen::VectorXd c_e_soc = c_e; + + bool stepAcceptable = false; + for (int soc_iteration = 0; soc_iteration < 5 && !stepAcceptable; + ++soc_iteration) { + // Rebuild Newton-KKT rhs with updated constraint values. + // + // rhs = −[∇f − Aₑᵀy] + // [ cₑˢᵒᶜ ] + // + // where cₑˢᵒᶜ = αc(xₖ) + c(xₖ + αpₖˣ) + c_e_soc = α_soc * c_e_soc + trial_c_e; + rhs.bottomRows(y.rows()) = -c_e_soc; + + // Solve the Newton-KKT system + step = solver.Solve(rhs); + + p_x_cor = step.segment(0, x.rows()); + p_y_soc = -step.segment(x.rows(), y.rows()); + + trial_x = x + α_soc * p_x_cor; + trial_y = y + α_soc * p_y_soc; + + xAD.SetValue(trial_x); + + trial_c_e = c_eAD.Value(); + + // Check whether filter accepts trial iterate + entry = filter.MakeEntry(trial_c_e); + if (filter.TryAdd(entry)) { + p_x = p_x_cor; + p_y = p_y_soc; + α = α_soc; + stepAcceptable = true; + } + } + + if (stepAcceptable) { + // Accept step + break; + } + } + + // If we got here and α is the full step, the full step was rejected. + // Increment the full-step rejected counter to keep track of how many full + // steps have been rejected in a row. + if (α == α_max) { + ++fullStepRejectedCounter; + } + + // If the full step was rejected enough times in a row, reset the filter + // because it may be impeding progress. + // + // See section 3.2 case I of [2]. + if (fullStepRejectedCounter >= 4 && + filter.maxConstraintViolation > entry.constraintViolation / 10.0) { + filter.maxConstraintViolation *= 0.1; + filter.Reset(); + continue; + } + + // Reduce step size + α *= α_red_factor; + + // Safety factor for the minimal step size + constexpr double α_min_frac = 0.05; + + // If step size hit a minimum, check if the KKT error was reduced. If it + // wasn't, report infeasible. + if (α < α_min_frac * Filter::γConstraint) { + double currentKKTError = KKTError(g, A_e, c_e, y); + + Eigen::VectorXd trial_x = x + α_max * p_x; + Eigen::VectorXd trial_y = y + α_max * p_y; + + // Upate autodiff + xAD.SetValue(trial_x); + yAD.SetValue(trial_y); + + Eigen::VectorXd trial_c_e = c_eAD.Value(); + + double nextKKTError = + KKTError(gradientF.Value(), jacobianCe.Value(), trial_c_e, trial_y); + + // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway + if (nextKKTError <= 0.999 * currentKKTError) { + α = α_max; + + // Accept step + break; + } + + auto initialEntry = filter.MakeEntry(c_e); + + // Feasibility restoration phase + Eigen::VectorXd fr_x = x; + SolverStatus fr_status; + FeasibilityRestoration( + decisionVariables, equalityConstraints, + [&](const SolverIterationInfo& info) { + trial_x = info.x.segment(0, decisionVariables.size()); + xAD.SetValue(trial_x); + + trial_c_e = c_eAD.Value(); + + // If current iterate is acceptable to normal filter and + // constraint violation has sufficiently reduced, stop + // feasibility restoration + entry = filter.MakeEntry(trial_c_e); + if (filter.IsAcceptable(entry) && + entry.constraintViolation < + 0.9 * initialEntry.constraintViolation) { + return true; + } + + return false; + }, + config, fr_x, &fr_status); + + if (fr_status.exitCondition == + SolverExitCondition::kCallbackRequestedStop) { + p_x = fr_x - x; + + // Lagrange mutliplier estimates + // + // y = (AₑAₑᵀ)⁻¹Aₑ∇f + // + // See equation (19.37) of [1]. + { + xAD.SetValue(fr_x); + + A_e = jacobianCe.Value(); + g = gradientF.Value(); + + // lhs = AₑAₑᵀ + Eigen::SparseMatrix lhs = A_e * A_e.transpose(); + + // rhs = Aₑ∇f + Eigen::VectorXd rhs = A_e * g; + + Eigen::SimplicialLDLT> yEstimator{lhs}; + Eigen::VectorXd sol = yEstimator.solve(rhs); + + p_y = y - sol.block(0, 0, y.rows(), 1); + } + + α = 1.0; + + // Accept step + break; + } else if (fr_status.exitCondition == SolverExitCondition::kSuccess) { + status->exitCondition = SolverExitCondition::kLocallyInfeasible; + x = fr_x; + return; + } else { + status->exitCondition = + SolverExitCondition::kFeasibilityRestorationFailed; + x = fr_x; + return; + } + } + } + + // If full step was accepted, reset full-step rejected counter + if (α == α_max) { + fullStepRejectedCounter = 0; + } + + // Handle very small search directions by letting αₖ = αₖᵐᵃˣ when + // max(|pₖˣ(i)|/(1 + |xₖ(i)|)) < 10ε_mach. + // + // See section 3.9 of [2]. + double maxStepScaled = 0.0; + for (int row = 0; row < x.rows(); ++row) { + maxStepScaled = std::max(maxStepScaled, + std::abs(p_x(row)) / (1.0 + std::abs(x(row)))); + } + if (maxStepScaled < 10.0 * std::numeric_limits::epsilon()) { + α = α_max; + } + + // xₖ₊₁ = xₖ + αₖpₖˣ + // yₖ₊₁ = yₖ + αₖpₖʸ + x += α * p_x; + y += α * p_y; + + // Update autodiff for Jacobians and Hessian + xAD.SetValue(x); + yAD.SetValue(y); + A_e = jacobianCe.Value(); + g = gradientF.Value(); + H = hessianL.Value(); + + c_e = c_eAD.Value(); + + // Update the error estimate + E_0 = ErrorEstimate(g, A_e, c_e, y); + if (E_0 < config.acceptableTolerance) { + ++acceptableIterCounter; + } else { + acceptableIterCounter = 0; + } + + const auto innerIterEndTime = std::chrono::system_clock::now(); + + // Diagnostics for current iteration + if (config.diagnostics) { + if (iterations % 20 == 0) { + sleipnir::println("{:^4} {:^9} {:^13} {:^13} {:^13}", "iter", + "time (ms)", "error", "cost", "infeasibility"); + sleipnir::println("{:=^61}", ""); + } + + sleipnir::println("{:4} {:9.3f} {:13e} {:13e} {:13e}", iterations, + ToMilliseconds(innerIterEndTime - innerIterStartTime), + E_0, f.Value(), c_e.lpNorm<1>()); + } + + ++iterations; + + // Check for max iterations + if (iterations >= config.maxIterations) { + status->exitCondition = SolverExitCondition::kMaxIterationsExceeded; + return; + } + + // Check for max wall clock time + if (innerIterEndTime - solveStartTime > config.timeout) { + status->exitCondition = SolverExitCondition::kTimeout; + return; + } + + // Check for solve to acceptable tolerance + if (E_0 > config.tolerance && + acceptableIterCounter == config.maxAcceptableIterations) { + status->exitCondition = SolverExitCondition::kSolvedToAcceptableTolerance; + return; + } + } +} + +} // namespace sleipnir diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp index 6b757df3e6b..f1490028dd2 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/ErrorEstimate.hpp @@ -11,6 +11,42 @@ namespace sleipnir { +/** + * Returns the error estimate using the KKT conditions for SQP. + * + * @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. + */ +inline double ErrorEstimate(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, + const Eigen::VectorXd& y) { + int numEqualityConstraints = A_e.rows(); + + // 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 double s_max = 100.0; + double s_d = std::max(s_max, y.lpNorm<1>() / numEqualityConstraints) / s_max; + + return std::max({(g - A_e.transpose() * y).lpNorm() / s_d, + c_e.lpNorm()}); +} + /** * Returns the error estimate using the KKT conditions for the interior-point * method. diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp index 0f13676aea0..79b5d99ae27 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/FeasibilityRestoration.hpp @@ -20,6 +20,168 @@ namespace sleipnir { +/** + * 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. + * + * @param[in] decisionVariables The list of decision variables. + * @param[in] equalityConstraints The list of equality constraints. + * @param[in] callback The user callback. + * @param[in] config Configuration options for the solver. + * @param[in,out] x The current iterate from the normal solve. + * @param[out] status The solver status. + */ +inline void FeasibilityRestoration( + std::span decisionVariables, + std::span equalityConstraints, + function_ref callback, + const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status) { + // Feasibility restoration + // + // min ρ Σ (pₑ + nₑ) + ζ/2 (x - x_R)ᵀD_R(x - x_R) + // x + // pₑ,nₑ + // + // s.t. cₑ(x) - pₑ + nₑ = 0 + // pₑ ≥ 0 + // nₑ ≥ 0 + // + // where ρ = 1000, ζ = √μ where μ is the barrier parameter, x_R is original + // iterate before feasibility restoration, and D_R is a scaling matrix defined + // by + // + // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) + + constexpr double ρ = 1000.0; + double μ = config.tolerance / 10.0; + + wpi::SmallVector fr_decisionVariables; + fr_decisionVariables.reserve(decisionVariables.size() + + 2 * equalityConstraints.size()); + + // Assign x + fr_decisionVariables.assign(decisionVariables.begin(), + decisionVariables.end()); + + // Allocate pₑ and nₑ + for (size_t row = 0; row < 2 * equalityConstraints.size(); ++row) { + fr_decisionVariables.emplace_back(); + } + + auto it = fr_decisionVariables.begin(); + + VariableMatrix xAD{std::span{it, it + decisionVariables.size()}}; + it += decisionVariables.size(); + + VariableMatrix p_e{std::span{it, it + equalityConstraints.size()}}; + it += equalityConstraints.size(); + + VariableMatrix n_e{std::span{it, it + equalityConstraints.size()}}; + it += equalityConstraints.size(); + + // Set initial values for pₑ and nₑ. + // + // + // 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 = μ c(x) 2ρ + // -4(ρ)c = 2ρ μ c(x) + // -4c = 2μ c(x) + // c = -μ c(x)/2 (5) + // + // p = c(x) + n (6) + for (int row = 0; row < p_e.Rows(); ++row) { + double c_e = equalityConstraints[row].Value(); + + constexpr double a = 2 * ρ; + double b = ρ * c_e - μ; + double c = -μ * c_e / 2.0; + + double n = -b * std::sqrt(b * b - 4.0 * a * c) / (2.0 * a); + double p = c_e + n; + + p_e(row).SetValue(p); + n_e(row).SetValue(n); + } + + // cₑ(x) - pₑ + nₑ = 0 + wpi::SmallVector fr_equalityConstraints; + fr_equalityConstraints.assign(equalityConstraints.begin(), + equalityConstraints.end()); + for (size_t row = 0; row < fr_equalityConstraints.size(); ++row) { + auto& constraint = fr_equalityConstraints[row]; + constraint = constraint - p_e(row) + n_e(row); + } + + // cᵢ(x) - s - pᵢ + nᵢ = 0 + wpi::SmallVector fr_inequalityConstraints; + + // pₑ ≥ 0 + std::copy(p_e.begin(), p_e.end(), + std::back_inserter(fr_inequalityConstraints)); + + // nₑ ≥ 0 + std::copy(n_e.begin(), n_e.end(), + std::back_inserter(fr_inequalityConstraints)); + + Variable J = 0.0; + + // J += ρ Σ (pₑ + nₑ) + for (auto& elem : p_e) { + J += elem; + } + for (auto& elem : n_e) { + J += elem; + } + J *= ρ; + + // D_R = diag(min(1, 1/|x_R⁽¹⁾|), …, min(1, 1/|x_R|⁽ⁿ⁾) + Eigen::VectorXd D_R{x.rows()}; + for (int row = 0; row < D_R.rows(); ++row) { + D_R(row) = std::min(1.0, 1.0 / std::abs(x(row))); + } + + // J += ζ/2 (x - x_R)ᵀD_R(x - x_R) + for (int row = 0; row < x.rows(); ++row) { + J += std::sqrt(μ) / 2.0 * D_R(row) * sleipnir::pow(xAD(row) - x(row), 2); + } + + Eigen::VectorXd fr_x = VariableMatrix{fr_decisionVariables}.Value(); + + // Set up initial value for inequality constraint slack variables + Eigen::VectorXd fr_s{fr_inequalityConstraints.size()}; + fr_s.setOnes(); + + InteriorPoint(fr_decisionVariables, fr_equalityConstraints, + fr_inequalityConstraints, J, callback, config, true, fr_x, fr_s, + status); + + x = fr_x.segment(0, decisionVariables.size()); +} + /** * 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 diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp index 02bdb5a8db5..0c14efd7b8a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/Filter.hpp @@ -12,6 +12,8 @@ #include "sleipnir/autodiff/Variable.hpp" +// See docs/algorithms.md#Works_cited for citation definitions. + namespace sleipnir { /** @@ -32,26 +34,14 @@ struct FilterEntry { * @param cost The cost function's value. * @param constraintViolation The constraint violation. */ - FilterEntry(double cost, double constraintViolation) + constexpr FilterEntry(double cost, double constraintViolation) : cost{cost}, constraintViolation{constraintViolation} {} - - /** - * Constructs a FilterEntry. - * - * @param f The cost function. - * @param μ The barrier parameter. - * @param s The inequality constraint slack variables. - * @param c_e The equality constraint values (nonzero means violation). - * @param c_i The inequality constraint values (negative means violation). - */ - FilterEntry(Variable& f, double μ, const Eigen::VectorXd& s, - const Eigen::VectorXd& c_e, const Eigen::VectorXd& c_i) - : cost{f.Value() - μ * s.array().log().sum()}, - constraintViolation{c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()} {} }; /** - * Interior-point step filter. + * Step filter. + * + * See the section on filters in chapter 15 of [1]. */ class Filter { public: @@ -64,11 +54,9 @@ class Filter { * Construct an empty filter. * * @param f The cost function. - * @param μ The barrier parameter. */ - explicit Filter(Variable& f, double μ) { + explicit Filter(Variable& f) { m_f = &f; - m_μ = μ; // Initial filter entry rejects constraint violations above max m_filter.emplace_back(std::numeric_limits::infinity(), @@ -77,11 +65,8 @@ class Filter { /** * Reset the filter. - * - * @param μ The new barrier parameter. */ - void Reset(double μ) { - m_μ = μ; + void Reset() { m_filter.clear(); // Initial filter entry rejects constraint violations above max @@ -90,15 +75,26 @@ class Filter { } /** - * Creates a new filter entry. + * Creates a new Sequential Quadratic Programming filter entry. + * + * @param c_e The equality constraint values (nonzero means violation). + */ + FilterEntry MakeEntry(const Eigen::VectorXd& c_e) { + return FilterEntry{m_f->Value(), c_e.lpNorm<1>()}; + } + + /** + * Creates a new interior-point method filter entry. * * @param s The inequality constraint slack variables. * @param c_e The equality constraint values (nonzero means violation). * @param c_i The inequality constraint values (negative means violation). + * @param μ The barrier parameter. */ FilterEntry MakeEntry(Eigen::VectorXd& s, const Eigen::VectorXd& c_e, - const Eigen::VectorXd& c_i) { - return FilterEntry{*m_f, m_μ, s, c_e, c_i}; + const Eigen::VectorXd& c_i, double μ) { + return FilterEntry{m_f->Value() - μ * s.array().log().sum(), + c_e.lpNorm<1>() + (c_i - s).lpNorm<1>()}; } /** @@ -181,7 +177,6 @@ class Filter { private: Variable* m_f = nullptr; - double m_μ = 0.0; wpi::SmallVector m_filter; }; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp index 03fa112b6a6..3b603159d21 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/src/optimization/solver/util/KKTError.hpp @@ -9,6 +9,28 @@ namespace sleipnir { +/** + * Returns the KKT error for Sequential Quadratic Programming. + * + * @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. + */ +inline double KKTError(const Eigen::VectorXd& g, + const Eigen::SparseMatrix& A_e, + const Eigen::VectorXd& c_e, const Eigen::VectorXd& y) { + // Compute the KKT error as the 1-norm of the KKT conditions from equations + // (19.5a) through (19.5d) of [1]. + // + // ∇f − Aₑᵀy = 0 + // cₑ = 0 + + return (g - A_e.transpose() * y).lpNorm<1>() + c_e.lpNorm<1>(); +} + /** * Returns the KKT error for the interior-point method. *