[upstream_utils] Upgrade to Sleipnir 0.6.0 (#8923)

upstream_utils/sleipnir.py

@@ -46,7 +46,7 @@ using small_vector = wpi::util::SmallVector<T>;
 def main():
     name = "sleipnir"
     url = "https://github.com/SleipnirGroup/Sleipnir"
-    tag = "v0.5.2"
+    tag = "v0.6.0"
 
     sleipnir = Lib(name, url, tag, copy_upstream_src)
     sleipnir.main()

upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch

@@ -7,9 +7,10 @@ Subject: [PATCH 01/10] Use fmtlib
  include/sleipnir/autodiff/expression_type.hpp | 16 +++++++----
  include/sleipnir/optimization/problem.hpp     |  1 +
  .../optimization/solver/exit_status.hpp       | 16 +++++++----
+ .../optimization/solver/util/inertia.hpp      | 22 ++++++++-------
  include/sleipnir/util/assert.hpp              |  5 ++--
  include/sleipnir/util/print.hpp               | 27 ++++++++++---------
- 5 files changed, 40 insertions(+), 25 deletions(-)
+ 6 files changed, 53 insertions(+), 34 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/expression_type.hpp b/include/sleipnir/autodiff/expression_type.hpp
 index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac8446a3ab 100644
@@ -66,7 +67,7 @@ index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac
 +
 +// @endcond
 diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
-index a9553ffbcfed568c48f7d789d8b127790dfddb91..3de6d5bf89a65fe07784350c3b1a4691dfc0c822 100644
+index ccff8b9425b45fd1c790bc4da87b81f501a70d9c..41b44578b1e2783dbceb300b7c72ee0c8d1202df 100644
 --- a/include/sleipnir/optimization/problem.hpp
 +++ b/include/sleipnir/optimization/problem.hpp
 @@ -15,6 +15,7 @@
@@ -131,6 +132,68 @@ index 0a48df7423b5a3dccd8e611e91befd32487fafdc..8786d6d64ac44ac88133df65a79636ec
  };
 +
 +// @endcond
+diff --git a/include/sleipnir/optimization/solver/util/inertia.hpp b/include/sleipnir/optimization/solver/util/inertia.hpp
+index 49ee606f7e3f02f2f5c24079eb27a412dad3055f..2d2ce114be9c40e645d685b63465b977e0b70439 100644
+--- a/include/sleipnir/optimization/solver/util/inertia.hpp
++++ b/include/sleipnir/optimization/solver/util/inertia.hpp
+@@ -2,10 +2,10 @@
+ 
+ #pragma once
+ 
+-#include <format>
+ #include <limits>
+ 
+ #include <Eigen/Core>
++#include <fmt/format.h>
+ 
+ namespace slp {
+ 
+@@ -77,14 +77,16 @@ class Inertia {
+ 
+ }  // namespace slp
+ 
++// @cond Suppress Doxygen
++
+ /// Formatter for Inertia.
+ template <>
+-struct std::formatter<slp::Inertia> {
++struct fmt::formatter<slp::Inertia> {
+   /// Parses format string.
+   ///
+   /// @param ctx Format parse context.
+   /// @return Format parse context iterator.
+-  constexpr auto parse(std::format_parse_context& ctx) {
++  constexpr auto parse(fmt::format_parse_context& ctx) {
+     return m_underlying.parse(ctx);
+   }
+ 
+@@ -95,18 +97,20 @@ struct std::formatter<slp::Inertia> {
+   /// @param ctx Format context.
+   /// @return Format context iterator.
+   template <typename FmtContext>
+-  auto format(const slp::Inertia& inertia, FmtContext& ctx) const {
++  constexpr auto format(const slp::Inertia& inertia, FmtContext& ctx) const {
+     auto out = ctx.out();
+ 
+-    out = std::format_to(out, "(");
++    out = fmt::format_to(out, "(");
+     out = m_underlying.format(inertia.positive, ctx);
+-    out = std::format_to(out, ", ");
++    out = fmt::format_to(out, ", ");
+     out = m_underlying.format(inertia.negative, ctx);
+-    out = std::format_to(out, ", ");
++    out = fmt::format_to(out, ", ");
+     out = m_underlying.format(inertia.zero, ctx);
+-    return std::format_to(out, ")");
++    return fmt::format_to(out, ")");
+   }
+ 
+  private:
+-  std::formatter<int> m_underlying;
++  fmt::formatter<int> m_underlying;
+ };
++
++// @endcond
 diff --git a/include/sleipnir/util/assert.hpp b/include/sleipnir/util/assert.hpp
 index 29dfe5416530a476417de3a4b6434d3b28d73120..783a0e71a1cb54bd1d6d17fa9d2bedd2038a8ef5 100644
 --- a/include/sleipnir/util/assert.hpp

upstream_utils/sleipnir_patches/0002-Use-wpi-SmallVector.patch

@@ -4,13 +4,12 @@ Date: Sun, 16 Jun 2024 12:08:49 -0700
 Subject: [PATCH 02/10] Use wpi::SmallVector
 
 ---
- include/sleipnir/autodiff/expression.hpp      | 4 ++--
- include/sleipnir/autodiff/variable.hpp        | 4 ++--
- include/sleipnir/autodiff/variable_matrix.hpp | 4 ++--
- 3 files changed, 6 insertions(+), 6 deletions(-)
+ include/sleipnir/autodiff/expression.hpp | 4 ++--
+ include/sleipnir/autodiff/variable.hpp   | 4 ++--
+ 2 files changed, 4 insertions(+), 4 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/expression.hpp b/include/sleipnir/autodiff/expression.hpp
-index 76f6d4ed534ce685194ad7fc62d9b62eb2ca1096..10e20f5e2fb0d7459fcecac2f8ba1bdcc98efc3f 100644
+index af10b505ae44ca0eb656f66f3f49172d7fc98c6d..7a9dc089a9e2fb323ecc8725f924ebee1b213af0 100644
 --- a/include/sleipnir/autodiff/expression.hpp
 +++ b/include/sleipnir/autodiff/expression.hpp
 @@ -34,7 +34,7 @@ struct Expression;
@@ -22,7 +21,7 @@ index 76f6d4ed534ce685194ad7fc62d9b62eb2ca1096..10e20f5e2fb0d7459fcecac2f8ba1bdc
  
  /// Typedef for intrusive shared pointer to Expression.
  ///
-@@ -736,7 +736,7 @@ constexpr void inc_ref_count(Expression<Scalar>* expr) {
+@@ -744,7 +744,7 @@ constexpr void inc_ref_count(Expression<Scalar>* expr) {
  /// @tparam Scalar Scalar type.
  /// @param expr The shared pointer's managed object.
  template <typename Scalar>
@@ -53,22 +52,3 @@ index 910ebcf5266bdf76711db7849dd6584549094e3b..3c4f67c1f6224226620e2ffcc5973364
        : expr{std::move(expr)} {}
  
    /// Assignment operator for scalar.
-diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
-index 8ed39dfae69c70e0b32998eb5e0c5207383f4fa2..3eeaccac274cf38d554ea834b3ff666615939382 100644
---- a/include/sleipnir/autodiff/variable_matrix.hpp
-+++ b/include/sleipnir/autodiff/variable_matrix.hpp
-@@ -1134,12 +1134,12 @@ class VariableMatrix : public SleipnirBase {
-   /// Returns const begin iterator.
-   ///
-   /// @return Const begin iterator.
--  const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
-+  const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
- 
-   /// Returns const end iterator.
-   ///
-   /// @return Const end iterator.
--  const_iterator cend() const { return const_iterator{m_storage.cend()}; }
-+  const_iterator cend() const { return const_iterator{m_storage.end()}; }
- 
-   /// Returns reverse begin iterator.
-   ///

upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch

@@ -11,10 +11,10 @@ Subject: [PATCH 04/10] Replace std::to_underlying()
  create mode 100644 include/sleipnir/util/to_underlying.hpp
 
 diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
-index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb2094303ee2e 100644
+index 41b44578b1e2783dbceb300b7c72ee0c8d1202df..49921b98de3452b6ca9f2e33a86daa254c0d3a01 100644
 --- a/include/sleipnir/optimization/problem.hpp
 +++ b/include/sleipnir/optimization/problem.hpp
-@@ -37,6 +37,7 @@
+@@ -38,6 +38,7 @@
  #include "sleipnir/util/profiler.hpp"
  #include "sleipnir/util/spy.hpp"
  #include "sleipnir/util/symbol_exports.hpp"
@@ -22,7 +22,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209
  
  namespace slp {
  
-@@ -752,11 +753,11 @@ class Problem {
+@@ -773,11 +774,11 @@ class Problem {
      // Print problem structure
      slp::println("\nProblem structure:");
      slp::println("  ↳ {} cost function",
@@ -37,7 +37,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209
  
      if (m_decision_variables.size() == 1) {
        slp::print("\n1 decision variable\n");
-@@ -768,7 +769,7 @@ class Problem {
+@@ -789,7 +790,7 @@ class Problem {
          [](const gch::small_vector<Variable<Scalar>>& constraints) {
            std::array<size_t, 5> counts{};
            for (const auto& constraint : constraints) {
@@ -47,7 +47,7 @@ index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb209
            for (const auto& [count, name] :
                 std::views::zip(counts, std::array{"empty", "constant", "linear",
 diff --git a/include/sleipnir/util/print_diagnostics.hpp b/include/sleipnir/util/print_diagnostics.hpp
-index e16ef1b03e675b7ccb8db6781568eec7a44aace7..69cb99957cc7da5abc4162c53a229f7f642bbe4b 100644
+index 87ccad9ef4b05ee5aeaaf0e5ff9cfbc7e995d5ed..831ae3e5d1002005a8aa00fc80accd705d5cca7d 100644
 --- a/include/sleipnir/util/print_diagnostics.hpp
 +++ b/include/sleipnir/util/print_diagnostics.hpp
 @@ -17,6 +17,7 @@
@@ -58,15 +58,15 @@ index e16ef1b03e675b7ccb8db6781568eec7a44aace7..69cb99957cc7da5abc4162c53a229f7f
  
  namespace slp {
  
-@@ -239,7 +240,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
+@@ -230,7 +231,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
    slp::println(
-       "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
-       "{:.2e} {:2d}│",
+       "│{:4} {:1} {:9.3f} {:10.4e} {:11.4e} {:10.4e} {:8.2e} {:8.2e} {:<5} "
+       "{:<5} {:8.2e} {:8.2e} {:8.2e} {:8.2e} {:2d}│",
 -      iterations, ITERATION_TYPES[std::to_underlying(type)], to_ms(time), error,
 +      iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error,
-       cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
+       cost, infeasibility, complementarity, μ, power_of_10(δ), power_of_10(γ),
+       full_primal_step_inf_norm, full_dual_step_inf_norm, primal_α, dual_α,
        backtracks);
- }
 diff --git a/include/sleipnir/util/to_underlying.hpp b/include/sleipnir/util/to_underlying.hpp
 new file mode 100644
 index 0000000000000000000000000000000000000000..3f9b4835b912c5a0d998c43828f255c61d0f573e

upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch

@@ -9,26 +9,26 @@ Subject: [PATCH 05/10] Replace std::views::zip()
  2 files changed, 9 insertions(+), 4 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/gradient_expression_graph.hpp b/include/sleipnir/autodiff/gradient_expression_graph.hpp
-index 275c30b76d34efe7ee1608cd4eedfa54ab2dc1ec..c0e3c161343175837abcb25fb22da0c611a44799 100644
+index fda7931c686a315209acdbe6f1ac581944e2781c..d3e59068d04b3db01945c5657959b69cd4522cca 100644
 --- a/include/sleipnir/autodiff/gradient_expression_graph.hpp
 +++ b/include/sleipnir/autodiff/gradient_expression_graph.hpp
-@@ -161,7 +161,10 @@ class GradientExpressionGraph {
-         }
+@@ -139,7 +139,10 @@ class GradientExpressionGraph {
        }
-     } else {
--      for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) {
-+      for (size_t i = 0; i < m_top_list.size(); ++i) {
-+        const auto& col = m_col_list[i];
-+        const auto& node = m_top_list[i];
+     }
+ 
+-    for (const auto& [col, node] : std::views::zip(m_col_list, m_top_list)) {
++    for (size_t i = 0; i < m_top_list.size(); ++i) {
++      const auto& col = m_col_list[i];
++      const auto& node = m_top_list[i];
 +
-         // Append adjoints of wrt to sparse matrix triplets
-         if (col != -1 && node->adjoint != Scalar(0)) {
-           triplets.emplace_back(row, col, node->adjoint);
+       // Append adjoints of wrt to sparse matrix triplets
+       if (col != -1) {
+         triplets.emplace_back(row, col, node->adjoint);
 diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
-index 273363db2cf75c84d81f2fef36dcb2094303ee2e..c4a45f7f954c9e028b1c85c45d5034bde284474f 100644
+index 49921b98de3452b6ca9f2e33a86daa254c0d3a01..d3feadd577bd53147a8b07da76910e046ccbf95d 100644
 --- a/include/sleipnir/optimization/problem.hpp
 +++ b/include/sleipnir/optimization/problem.hpp
-@@ -771,9 +771,11 @@ class Problem {
+@@ -792,9 +792,11 @@ class Problem {
            for (const auto& constraint : constraints) {
              ++counts[slp::to_underlying(constraint.type())];
            }

upstream_utils/sleipnir_patches/0008-Suppress-GCC-12-warning-false-positive.patch

@@ -8,7 +8,7 @@ Subject: [PATCH 08/10] Suppress GCC 12 warning false positive
  1 file changed, 7 insertions(+)
 
 diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
-index 3eeaccac274cf38d554ea834b3ff666615939382..38b90b4a1c784cfd2fd806536d001db9335ebe2d 100644
+index 2cf6e00f16f58a5847a4515f13b3a91fffbb72b8..b5d15edc5b294a2ea50dca15a7612c61e5f2680b 100644
 --- a/include/sleipnir/autodiff/variable_matrix.hpp
 +++ b/include/sleipnir/autodiff/variable_matrix.hpp
 @@ -503,6 +503,10 @@ class VariableMatrix : public SleipnirBase {

upstream_utils/sleipnir_patches/0009-Suppress-Doxygen-warning-false-positives.patch

@@ -8,7 +8,7 @@ Subject: [PATCH 09/10] Suppress Doxygen warning false positives
  1 file changed, 4 insertions(+)
 
 diff --git a/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp b/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp
-index ec87b2b8160051224de870c22f5ded6b35f6dc2d..1c042f8599e8e179c1586b2648e1ff99e5b5d32a 100644
+index 6324ddcf2d24a0cd5367420af4330e1c59b58004..ed128e5a583d51bc963f03f6b88c0a15e6440545 100644
 --- a/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp
 +++ b/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp
 @@ -96,6 +96,8 @@ compute_p_n(const Eigen::Vector<Scalar, Eigen::Dynamic>& c, Scalar ρ,
@@ -20,7 +20,7 @@ index ec87b2b8160051224de870c22f5ded6b35f6dc2d..1c042f8599e8e179c1586b2648e1ff99
  /// Finds the iterate that minimizes the constraint violation while not
  /// deviating too far from the starting point. This is a fallback procedure when
  /// the normal Sequential Quadratic Programming method fails to converge to a
-@@ -592,6 +594,8 @@ ExitStatus feasibility_restoration(
+@@ -624,6 +626,8 @@ ExitStatus feasibility_restoration(
    }
  }
  

upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch

@@ -15,10 +15,10 @@ Subject: [PATCH 10/10] Use operator() instead of multidimensional array
  7 files changed, 135 insertions(+), 135 deletions(-)
 
 diff --git a/include/sleipnir/autodiff/hessian.hpp b/include/sleipnir/autodiff/hessian.hpp
-index 11af6033d1cf00b27cb9ff796c4a7f16b32df90a..d4cc02f54accd568d052ae5709213d45c4c8917a 100644
+index 2a8087eda23f49f8c01be35d5289568644a9742d..c48e01c7b89d80db8032f6d2e492a0ec1bfa9f42 100644
 --- a/include/sleipnir/autodiff/hessian.hpp
 +++ b/include/sleipnir/autodiff/hessian.hpp
-@@ -99,9 +99,9 @@ class Hessian {
+@@ -100,9 +100,9 @@ class Hessian {
        auto grad = m_graphs[row].generate_tree(m_wrt);
        for (int col = 0; col < m_wrt.rows(); ++col) {
          if (grad[col].expr != nullptr) {
@@ -31,7 +31,7 @@ index 11af6033d1cf00b27cb9ff796c4a7f16b32df90a..d4cc02f54accd568d052ae5709213d45
        }
      }
 diff --git a/include/sleipnir/autodiff/jacobian.hpp b/include/sleipnir/autodiff/jacobian.hpp
-index f1915a0d08c509f4eb4d032d4e6a5e6099b68ebc..edd949eefd018af1b1e091b4b2780fc1a0a492cf 100644
+index 7a5c3798dab50f14db35bee430f1d5e8e32196ff..1a5b01de99a7c011ebf8c5ed9fc64942675fcb4b 100644
 --- a/include/sleipnir/autodiff/jacobian.hpp
 +++ b/include/sleipnir/autodiff/jacobian.hpp
 @@ -104,9 +104,9 @@ class Jacobian {
@@ -388,7 +388,7 @@ index 4018606df45941b578c861caf934495f8c9e368e..cf554832b82adb17b4b1d7b56842a77d
      }
  
 diff --git a/include/sleipnir/autodiff/variable_matrix.hpp b/include/sleipnir/autodiff/variable_matrix.hpp
-index 38b90b4a1c784cfd2fd806536d001db9335ebe2d..e9f90fb12d230e40497e8557b985ff9c82ebaaeb 100644
+index b5d15edc5b294a2ea50dca15a7612c61e5f2680b..02bccb660474d9380444f5e20124fb687e372982 100644
 --- a/include/sleipnir/autodiff/variable_matrix.hpp
 +++ b/include/sleipnir/autodiff/variable_matrix.hpp
 @@ -154,7 +154,7 @@ class VariableMatrix : public SleipnirBase {
@@ -959,10 +959,10 @@ index 5e20b9b8debc13611d5d719b589d8fd896c35a90..e5163f9c2dbbfa0a580d029e61c741e6
        if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) {
          x_end = rk4<const decltype(m_dynamics)&, VariableMatrix<Scalar>,
 diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp
-index c4a45f7f954c9e028b1c85c45d5034bde284474f..4e38a082855c07c47f1aa191613114a5a392a4e4 100644
+index d3feadd577bd53147a8b07da76910e046ccbf95d..18afc996590de98050bfd3ddc629952bf0c8c2e2 100644
 --- a/include/sleipnir/optimization/problem.hpp
 +++ b/include/sleipnir/optimization/problem.hpp
-@@ -97,7 +97,7 @@ class Problem {
+@@ -98,7 +98,7 @@ class Problem {
      for (int row = 0; row < rows; ++row) {
        for (int col = 0; col < cols; ++col) {
          m_decision_variables.emplace_back();
@@ -971,7 +971,7 @@ index c4a45f7f954c9e028b1c85c45d5034bde284474f..4e38a082855c07c47f1aa191613114a5
        }
      }
  
-@@ -132,8 +132,8 @@ class Problem {
+@@ -133,8 +133,8 @@ class Problem {
      for (int row = 0; row < rows; ++row) {
        for (int col = 0; col <= row; ++col) {
          m_decision_variables.emplace_back();

wpimath/src/main/java/org/wpilib/math/autodiff/VariableMatrix.java

@@ -904,11 +904,11 @@ public class VariableMatrix implements AutoCloseable, Iterable<Variable> {
   }
 
   /**
-   * Returns a variable matrix filled with zeroes.
+   * Returns a variable matrix filled with zeros.
    *
    * @param rows The number of matrix rows.
    * @param cols The number of matrix columns.
-   * @return A variable matrix filled with zeroes.
+   * @return A variable matrix filled with zeros.
    */
   public static VariableMatrix zero(int rows, int cols) {
     return new VariableMatrix(new SimpleMatrix(rows, cols));

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp

@@ -153,14 +153,16 @@ struct Expression {
 
     // Prune expression
     if (lhs->is_constant(Scalar(0))) {
-      // Return zero
+      // Return zero, which lhs currently is
       return lhs;
     } else if (rhs->is_constant(Scalar(0))) {
-      // Return zero
+      // Return zero, which rhs currently is
       return rhs;
     } else if (lhs->is_constant(Scalar(1))) {
+      // Return rhs unmodified
       return rhs;
     } else if (rhs->is_constant(Scalar(1))) {
+      // Return lhs unmodified
       return lhs;
     }
 
@@ -203,9 +205,10 @@ struct Expression {
 
     // Prune expression
     if (lhs->is_constant(Scalar(0))) {
-      // Return zero
+      // Return zero, which lhs currently is
       return lhs;
     } else if (rhs->is_constant(Scalar(1))) {
+      // Return lhs unmodified
       return lhs;
     }
 
@@ -236,10 +239,13 @@ struct Expression {
                                          const ExpressionPtr<Scalar>& rhs) {
     using enum ExpressionType;
 
-    // Prune expression
+    // Prune expression. We check for nullptr because operator+ is used in
+    // adjoint accumulation, and child nodes can be null.
     if (lhs == nullptr || lhs->is_constant(Scalar(0))) {
+      // Return rhs unmodified
       return rhs;
     } else if (rhs == nullptr || rhs->is_constant(Scalar(0))) {
+      // Return lhs unmodified
       return lhs;
     }
 
@@ -281,12 +287,14 @@ struct Expression {
     // Prune expression
     if (lhs->is_constant(Scalar(0))) {
       if (rhs->is_constant(Scalar(0))) {
-        // Return zero
+        // Return zero, which rhs currently is
         return rhs;
       } else {
+        // Return rhs negated
         return -rhs;
       }
     } else if (rhs->is_constant(Scalar(0))) {
+      // Return lhs unmodified
       return lhs;
     }
 
@@ -316,7 +324,7 @@ struct Expression {
 
     // Prune expression
     if (lhs->is_constant(Scalar(0))) {
-      // Return zero
+      // Return zero, which lhs currently is
       return lhs;
     }
 
@@ -825,7 +833,7 @@ ExpressionPtr<Scalar> abs(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -934,7 +942,7 @@ ExpressionPtr<Scalar> asin(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -988,7 +996,7 @@ ExpressionPtr<Scalar> atan(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -1056,7 +1064,7 @@ ExpressionPtr<Scalar> atan2(const ExpressionPtr<Scalar>& y,
 
   // Prune expression
   if (y->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which y currently is
     return y;
   } else if (x->is_constant(Scalar(0))) {
     return constant_ptr(Scalar(std::numbers::pi) / Scalar(2));
@@ -1223,7 +1231,7 @@ ExpressionPtr<Scalar> erf(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -1406,7 +1414,7 @@ ExpressionPtr<Scalar> log(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -1460,7 +1468,7 @@ ExpressionPtr<Scalar> log10(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -1708,15 +1716,16 @@ ExpressionPtr<Scalar> pow(const ExpressionPtr<Scalar>& base,
 
   // Prune expression
   if (base->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which base currently is
     return base;
   } else if (base->is_constant(Scalar(1))) {
-    // Return one
+    // Return one, which base currently is
     return base;
   }
   if (power->is_constant(Scalar(0))) {
     return constant_ptr(Scalar(1));
   } else if (power->is_constant(Scalar(1))) {
+    // Return base unmodified
     return base;
   }
 
@@ -1828,7 +1837,7 @@ ExpressionPtr<Scalar> sin(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -1883,7 +1892,7 @@ ExpressionPtr<Scalar> sinh(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -1997,7 +2006,7 @@ ExpressionPtr<Scalar> tan(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 
@@ -2055,7 +2064,7 @@ ExpressionPtr<Scalar> tanh(const ExpressionPtr<Scalar>& x) {
 
   // Prune expression
   if (x->is_constant(Scalar(0))) {
-    // Return zero
+    // Return zero, which x currently is
     return x;
   }
 

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp

@@ -102,22 +102,11 @@ class GradientExpressionGraph {
   ///
   /// @param triplets The sparse matrix triplets.
   /// @param row The row of wrt.
-  /// @param wrt Vector of variables with respect to which to compute the
-  ///     Jacobian.
   void append_triplets(gch::small_vector<Eigen::Triplet<Scalar>>& triplets,
-                       int row, const VariableMatrix<Scalar>& wrt) const {
-    slp_assert(wrt.cols() == 1);
-
+                       int row) const {
     // Read docs/algorithms.md#Reverse_accumulation_automatic_differentiation
     // for background on reverse accumulation automatic differentiation.
 
-    // If wrt has fewer nodes than graph, zero wrt's adjoints
-    if (static_cast<size_t>(wrt.rows()) < m_top_list.size()) {
-      for (const auto& elem : wrt) {
-        elem.expr->adjoint = Scalar(0);
-      }
-    }
-
     if (m_top_list.empty()) {
       return;
     }
@@ -150,25 +139,13 @@ class GradientExpressionGraph {
       }
     }
 
-    // If wrt has fewer nodes than graph, iterate over wrt
-    if (static_cast<size_t>(wrt.rows()) < m_top_list.size()) {
-      for (int col = 0; col < wrt.rows(); ++col) {
-        const auto& node = wrt[col].expr;
+    for (size_t i = 0; i < m_top_list.size(); ++i) {
+      const auto& col = m_col_list[i];
+      const auto& node = m_top_list[i];
 
-        // Append adjoints of wrt to sparse matrix triplets
-        if (node->adjoint != Scalar(0)) {
-          triplets.emplace_back(row, col, node->adjoint);
-        }
-      }
-    } else {
-      for (size_t i = 0; i < m_top_list.size(); ++i) {
-        const auto& col = m_col_list[i];
-        const auto& node = m_top_list[i];
-
-        // Append adjoints of wrt to sparse matrix triplets
-        if (col != -1 && node->adjoint != Scalar(0)) {
-          triplets.emplace_back(row, col, node->adjoint);
-        }
+      // Append adjoints of wrt to sparse matrix triplets
+      if (col != -1) {
+        triplets.emplace_back(row, col, node->adjoint);
       }
     }
   }

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp

@@ -23,6 +23,7 @@ namespace slp {
 ///
 /// @tparam Scalar Scalar type.
 /// @tparam UpLo Which part of the Hessian to compute (Lower or Lower | Upper).
+///     Default is Lower | Upper.
 template <typename Scalar, int UpLo>
   requires(UpLo == Eigen::Lower) || (UpLo == (Eigen::Lower | Eigen::Upper))
 class Hessian {
@@ -69,7 +70,7 @@ class Hessian {
         // If the row is linear, compute its gradient once here and cache its
         // triplets. Constant rows are ignored because their gradients have no
         // nonzero triplets.
-        m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt);
+        m_graphs[row].append_triplets(m_cached_triplets, row);
       } else if (m_variables[row].type() > ExpressionType::LINEAR) {
         // If the row is quadratic or nonlinear, add it to the list of nonlinear
         // rows to be recomputed in value().
@@ -127,7 +128,7 @@ class Hessian {
 
     // Compute each nonlinear row of the Hessian
     for (int row : m_nonlinear_rows) {
-      m_graphs[row].append_triplets(triplets, row, m_wrt);
+      m_graphs[row].append_triplets(triplets, row);
     }
 
     m_H.setFromTriplets(triplets.begin(), triplets.end());

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp

@@ -77,7 +77,7 @@ class Jacobian {
         // If the row is linear, compute its gradient once here and cache its
         // triplets. Constant rows are ignored because their gradients have no
         // nonzero triplets.
-        m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt);
+        m_graphs[row].append_triplets(m_cached_triplets, row);
       } else if (m_variables[row].type() > ExpressionType::LINEAR) {
         // If the row is quadratic or nonlinear, add it to the list of nonlinear
         // rows to be recomputed in value().
@@ -132,7 +132,7 @@ class Jacobian {
 
     // Compute each nonlinear row of the Jacobian
     for (int row : m_nonlinear_rows) {
-      m_graphs[row].append_triplets(triplets, row, m_wrt);
+      m_graphs[row].append_triplets(triplets, row);
     }
 
     m_J.setFromTriplets(triplets.begin(), triplets.end());

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp

@@ -1141,12 +1141,12 @@ class VariableMatrix : public SleipnirBase {
   /// Returns const begin iterator.
   ///
   /// @return Const begin iterator.
-  const_iterator cbegin() const { return const_iterator{m_storage.begin()}; }
+  const_iterator cbegin() const { return const_iterator{m_storage.cbegin()}; }
 
   /// Returns const end iterator.
   ///
   /// @return Const end iterator.
-  const_iterator cend() const { return const_iterator{m_storage.end()}; }
+  const_iterator cend() const { return const_iterator{m_storage.cend()}; }
 
   /// Returns reverse begin iterator.
   ///
@@ -1191,11 +1191,11 @@ class VariableMatrix : public SleipnirBase {
   /// @return Number of elements in matrix.
   size_t size() const { return m_storage.size(); }
 
-  /// Returns a variable matrix filled with zeroes.
+  /// Returns a variable matrix filled with zeros.
   ///
   /// @param rows The number of matrix rows.
   /// @param cols The number of matrix columns.
-  /// @return A variable matrix filled with zeroes.
+  /// @return A variable matrix filled with zeros.
   static VariableMatrix<Scalar> zero(int rows, int cols) {
     VariableMatrix<Scalar> result{detail::empty, rows, cols};
 

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp

@@ -31,6 +31,7 @@
 #include "sleipnir/optimization/solver/options.hpp"
 #include "sleipnir/optimization/solver/sqp.hpp"
 #include "sleipnir/optimization/solver/util/bounds.hpp"
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
 #include "sleipnir/util/empty.hpp"
 #include "sleipnir/util/print.hpp"
 #include "sleipnir/util/print_diagnostics.hpp"
@@ -113,7 +114,7 @@ class Problem {
   /// Decision variables have an initial value of zero.
   ///
   /// @param rows Number of matrix rows.
-  /// @return A symmetric matrix of decision varaibles in the optimization
+  /// @return A symmetric matrix of decision variables in the optimization
   ///     problem.
   [[nodiscard]]
   VariableMatrix<Scalar> symmetric_decision_variable(int rows) {
@@ -377,20 +378,26 @@ class Problem {
       }
 #endif
 
+      // Automatically scale the cost. The problem scaling procedure is
+      // described in more detail in docs/algorithms.md#problem-scaling.
+      x_ad.set_value(x);
+      const ProblemScaling<Scalar> scaling{g.value()};
+
       NewtonMatrixCallbacks<Scalar> matrix_callbacks{
           num_decision_variables,
           [&](const DenseVector& x) -> Scalar {
             x_ad.set_value(x);
-            return f.value();
+            return scaling.f * f.value();
           },
           [&](const DenseVector& x) -> SparseVector {
             x_ad.set_value(x);
-            return g.value();
+            return scaling.f * g.value();
           },
           [&](const DenseVector& x) -> SparseMatrix {
             x_ad.set_value(x);
-            return H.value();
-          }};
+            return scaling.f * H.value();
+          },
+          scaling};
 
       // Invoke Newton solver
       status =
@@ -465,35 +472,42 @@ class Problem {
       }
 #endif
 
+      // Automatically scale the cost and constraints. The problem scaling
+      // procedure is described in more detail in
+      // docs/algorithms.md#problem-scaling.
+      x_ad.set_value(x);
+      const ProblemScaling<Scalar> scaling{g.value(), A_e.value()};
+
       SQPMatrixCallbacks<Scalar> matrix_callbacks{
           num_decision_variables,
           num_equality_constraints,
           [&](const DenseVector& x) -> Scalar {
             x_ad.set_value(x);
-            return f.value();
+            return scaling.f * f.value();
           },
           [&](const DenseVector& x) -> SparseVector {
             x_ad.set_value(x);
-            return g.value();
+            return scaling.f * g.value();
           },
           [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
             x_ad.set_value(x);
-            y_ad.set_value(y);
-            return H_f.value() + H_c.value();
+            y_ad.set_value(scaling.c_e.cwiseProduct(y));
+            return scaling.f * H_f.value() + H_c.value();
           },
           [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
             x_ad.set_value(x);
-            y_ad.set_value(y);
+            y_ad.set_value(scaling.c_e.cwiseProduct(y));
             return H_c.value();
           },
           [&](const DenseVector& x) -> DenseVector {
             x_ad.set_value(x);
-            return c_e_ad.value();
+            return scaling.c_e.cwiseProduct(c_e_ad.value());
           },
           [&](const DenseVector& x) -> SparseMatrix {
             x_ad.set_value(x);
-            return A_e.value();
-          }};
+            return scaling.c_e.asDiagonal() * A_e.value();
+          },
+          scaling};
 
       // Invoke SQP solver
       status = sqp<Scalar>(matrix_callbacks, iteration_callbacks, options, x);
@@ -597,48 +611,55 @@ class Problem {
       project_onto_bounds(x, bounds);
 #endif
 
+      // Automatically scale the cost and constraints. The problem scaling
+      // procedure is described in more detail in
+      // docs/algorithms.md#problem-scaling.
+      x_ad.set_value(x);
+      const ProblemScaling<Scalar> scaling{g.value(), A_e.value(), A_i.value()};
+
       InteriorPointMatrixCallbacks<Scalar> matrix_callbacks{
           num_decision_variables,
           num_equality_constraints,
           num_inequality_constraints,
           [&](const DenseVector& x) -> Scalar {
             x_ad.set_value(x);
-            return f.value();
+            return scaling.f * f.value();
           },
           [&](const DenseVector& x) -> SparseVector {
             x_ad.set_value(x);
-            return g.value();
+            return scaling.f * g.value();
           },
           [&](const DenseVector& x, const DenseVector& y,
               const DenseVector& z) -> SparseMatrix {
             x_ad.set_value(x);
-            y_ad.set_value(y);
-            z_ad.set_value(z);
-            return H_f.value() + H_c.value();
+            y_ad.set_value(scaling.c_e.cwiseProduct(y));
+            z_ad.set_value(scaling.c_i.cwiseProduct(z));
+            return scaling.f * H_f.value() + H_c.value();
           },
           [&](const DenseVector& x, const DenseVector& y,
               const DenseVector& z) -> SparseMatrix {
             x_ad.set_value(x);
-            y_ad.set_value(y);
-            z_ad.set_value(z);
+            y_ad.set_value(scaling.c_e.cwiseProduct(y));
+            z_ad.set_value(scaling.c_i.cwiseProduct(z));
             return H_c.value();
           },
           [&](const DenseVector& x) -> DenseVector {
             x_ad.set_value(x);
-            return c_e_ad.value();
+            return scaling.c_e.cwiseProduct(c_e_ad.value());
           },
           [&](const DenseVector& x) -> SparseMatrix {
             x_ad.set_value(x);
-            return A_e.value();
+            return scaling.c_e.asDiagonal() * A_e.value();
           },
           [&](const DenseVector& x) -> DenseVector {
             x_ad.set_value(x);
-            return c_i_ad.value();
+            return scaling.c_i.cwiseProduct(c_i_ad.value());
           },
           [&](const DenseVector& x) -> SparseMatrix {
             x_ad.set_value(x);
-            return A_i.value();
-          }};
+            return scaling.c_i.asDiagonal() * A_i.value();
+          },
+          scaling};
 
       // Invoke interior-point method solver
       status =

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp

@@ -6,7 +6,6 @@
 #include <chrono>
 #include <cmath>
 #include <functional>
-#include <limits>
 #include <span>
 
 #include <Eigen/Core>
@@ -77,13 +76,14 @@ ExitStatus interior_point(
   DenseVector y = DenseVector::Zero(matrix_callbacks.num_equality_constraints);
   DenseVector z =
       DenseVector::Ones(matrix_callbacks.num_inequality_constraints);
-  Scalar μ(0.1);
+  Scalar μ = Scalar(0.1) * matrix_callbacks.scaling.f;
+  int iterations = 0;
 
   return interior_point(matrix_callbacks, iteration_callbacks, options, false,
 #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
                         bound_constraint_mask,
 #endif
-                        x, s, y, z, μ);
+                        x, s, y, z, μ, iterations);
 }
 
 /// Finds the optimal solution to a nonlinear program using the interior-point
@@ -117,6 +117,7 @@ ExitStatus interior_point(
 ///     constraint dual variables.
 /// @param[in,out] μ The initial guess and output location for the barrier
 ///     parameter.
+/// @param[in,out] iterations The iteration counter.
 /// @return The exit status.
 template <typename Scalar>
 ExitStatus interior_point(
@@ -130,7 +131,7 @@ ExitStatus interior_point(
     Eigen::Vector<Scalar, Eigen::Dynamic>& x,
     Eigen::Vector<Scalar, Eigen::Dynamic>& s,
     Eigen::Vector<Scalar, Eigen::Dynamic>& y,
-    Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar& μ) {
+    Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar& μ, int& iterations) {
   using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
   using SparseMatrix = Eigen::SparseMatrix<Scalar>;
   using SparseVector = Eigen::SparseVector<Scalar>;
@@ -162,7 +163,7 @@ ExitStatus interior_point(
   solve_profilers.emplace_back("  ↳ KKT system solve");
   solve_profilers.emplace_back("  ↳ line search");
   solve_profilers.emplace_back("    ↳ SOC");
-  solve_profilers.emplace_back("  ↳ next iter prep");
+  solve_profilers.emplace_back("  ↳ feas. restoration");
   solve_profilers.emplace_back("  ↳ f(x)");
   solve_profilers.emplace_back("  ↳ ∇f(x)");
   solve_profilers.emplace_back("  ↳ ∇²ₓₓL");
@@ -182,7 +183,7 @@ ExitStatus interior_point(
   auto& kkt_system_solve_prof = solve_profilers[7];
   auto& line_search_prof = solve_profilers[8];
   auto& soc_prof = solve_profilers[9];
-  auto& next_iter_prep_prof = solve_profilers[10];
+  auto& feasibility_restoration_prof = solve_profilers[10];
 
   // Set up profiled matrix callbacks
 #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
@@ -232,7 +233,8 @@ ExitStatus interior_point(
       [&](const DenseVector& x) -> SparseMatrix {
         ScopedProfiler prof{A_i_prof};
         return matrix_callbacks.A_i(x);
-      }};
+      },
+      matrix_callbacks.scaling};
 #else
   const auto& matrices = matrix_callbacks;
 #endif
@@ -259,6 +261,15 @@ ExitStatus interior_point(
   slp_assert(A_i.rows() == matrices.num_inequality_constraints);
   slp_assert(A_i.cols() == matrices.num_decision_variables);
 
+  DenseVector trial_x;
+  DenseVector trial_s;
+  DenseVector trial_y;
+  DenseVector trial_z;
+
+  Scalar trial_f;
+  DenseVector trial_c_e;
+  DenseVector trial_c_i;
+
   // Check for overconstrained problem
   if (matrices.num_equality_constraints > matrices.num_decision_variables) {
     if (options.diagnostics) {
@@ -279,10 +290,9 @@ ExitStatus interior_point(
   s = bound_constraint_mask.select(c_i, s);
 #endif
 
-  int iterations = 0;
-
   // Barrier parameter minimum
-  const Scalar μ_min = Scalar(options.tolerance) / Scalar(10);
+  const Scalar μ_min =
+      matrices.scaling.f * Scalar(options.tolerance) / Scalar(10);
 
   // Fraction-to-the-boundary rule scale factor minimum
   constexpr Scalar τ_min(0.99);
@@ -325,10 +335,20 @@ ExitStatus interior_point(
   // Kept outside the loop so its storage can be reused
   gch::small_vector<Eigen::Triplet<Scalar>> triplets;
 
-  // Constraint regularization is forced to zero in feasibility restoration
-  // because the equality constraint Jacobian cannot be rank-deficient
+  const int lhs_rows =
+      matrices.num_decision_variables + matrices.num_equality_constraints;
   RegularizedLDLT<Scalar> solver{
+      // Use sparse solver if lower triangle fills < 25% of system
+      H.nonZeros() +
+              (A_i.transpose() * A_i)
+                  .template triangularView<Eigen::Lower>()
+                  .eval()
+                  .nonZeros() +
+              A_e.nonZeros() <
+          0.25 * lhs_rows * lhs_rows,
       matrices.num_decision_variables, matrices.num_equality_constraints,
+      // Constraint regularization is forced to zero in feasibility restoration
+      // because the equality constraint Jacobian cannot be rank-deficient
       in_feasibility_restoration ? Scalar(0) : Scalar(1e-10)};
 
   // Variables for determining when a step is acceptable
@@ -338,7 +358,8 @@ ExitStatus interior_point(
   int full_step_rejected_counter = 0;
 
   // Error
-  Scalar E_0 = std::numeric_limits<Scalar>::infinity();
+  Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+      matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
 
   setup_prof.stop();
 
@@ -479,13 +500,22 @@ ExitStatus interior_point(
 
     // Loop until a step is accepted
     while (1) {
-      DenseVector trial_x = x + α * step.p_x;
-      DenseVector trial_y = y + α_z * step.p_y;
-      DenseVector trial_z = z + α_z * step.p_z;
+      trial_x = x + α * step.p_x;
+      if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) {
+        // If the inequality constraints are all feasible, prevent them from
+        // becoming infeasible again.
+        //
+        // See equation (19.30) in [1].
+        trial_s = trial_c_i;
+      } else {
+        trial_s = s + α * step.p_s;
+      }
+      trial_y = y + α_z * step.p_y;
+      trial_z = z + α_z * step.p_z;
 
-      Scalar trial_f = matrices.f(trial_x);
-      DenseVector trial_c_e = matrices.c_e(trial_x);
-      DenseVector trial_c_i = matrices.c_i(trial_x);
+      trial_f = matrices.f(trial_x);
+      trial_c_e = matrices.c_e(trial_x);
+      trial_c_i = matrices.c_i(trial_x);
 
       // If f(xₖ + αpₖˣ), cₑ(xₖ + αpₖˣ), or cᵢ(xₖ + αpₖˣ) aren't finite, reduce
       // step size immediately
@@ -501,17 +531,6 @@ ExitStatus interior_point(
         continue;
       }
 
-      DenseVector trial_s;
-      if (options.feasible_ipm && c_i.cwiseGreater(Scalar(0)).all()) {
-        // If the inequality constraints are all feasible, prevent them from
-        // becoming infeasible again.
-        //
-        // See equation (19.30) in [1].
-        trial_s = trial_c_i;
-      } else {
-        trial_s = s + α * step.p_s;
-      }
-
       // Check whether filter accepts trial iterate
       FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
       if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
@@ -538,6 +557,8 @@ ExitStatus interior_point(
         Scalar α_z_soc = α_z;
         DenseVector c_e_soc = c_e;
 
+        Scalar soc_constraint_violation = next_constraint_violation;
+
         bool step_acceptable = false;
         for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;
              ++soc_iteration) {
@@ -546,19 +567,22 @@ ExitStatus interior_point(
           scope_exit soc_exit{[&] {
             soc_profiler.stop();
 
-            if (options.diagnostics) {
+            if (options.diagnostics && step_acceptable) {
               print_iteration_diagnostics(
-                  iterations,
-                  step_acceptable ? IterationType::ACCEPTED_SOC
-                                  : IterationType::REJECTED_SOC,
+                  iterations, IterationType::SECOND_ORDER_CORRECTION,
                   soc_profiler.current_duration(),
-                  kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
-                      g, A_e, trial_c_e, A_i, trial_c_i, trial_s, trial_y,
-                      trial_z, Scalar(0)),
+                  unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+                      matrices.scaling, g, A_e, trial_c_e, A_i, trial_c_i,
+                      trial_s, trial_y, trial_z, Scalar(0)),
                   trial_f,
                   trial_c_e.template lpNorm<1>() +
                       (trial_c_i - trial_s).template lpNorm<1>(),
                   trial_s.dot(trial_z), μ, solver.hessian_regularization(),
+                  solver.constraint_jacobian_regularization(),
+                  std::max(soc_step.p_x.template lpNorm<Eigen::Infinity>(),
+                           soc_step.p_s.template lpNorm<Eigen::Infinity>()),
+                  std::max(soc_step.p_y.template lpNorm<Eigen::Infinity>(),
+                           soc_step.p_z.template lpNorm<Eigen::Infinity>()),
                   α_soc, Scalar(1), α_reduction_factor, α_z_soc);
             }
           }};
@@ -576,12 +600,12 @@ ExitStatus interior_point(
           compute_step(soc_step);
 
           // αˢᵒᶜ = max(α ∈ (0, 1] : sₖ + αpₖˢ ≥ (1−τⱼ)sₖ)
+          // αₖᶻˢᵒᶜ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
           α_soc = fraction_to_the_boundary_rule<Scalar>(s, soc_step.p_s, τ);
+          α_z_soc = fraction_to_the_boundary_rule<Scalar>(z, soc_step.p_z, τ);
+
           trial_x = x + α_soc * soc_step.p_x;
           trial_s = s + α_soc * soc_step.p_s;
-
-          // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
-          α_z_soc = fraction_to_the_boundary_rule<Scalar>(z, soc_step.p_z, τ);
           trial_y = y + α_z_soc * soc_step.p_y;
           trial_z = z + α_z_soc * soc_step.p_z;
 
@@ -589,6 +613,16 @@ ExitStatus interior_point(
           trial_c_e = matrices.c_e(trial_x);
           trial_c_i = matrices.c_i(trial_x);
 
+          // Check whether filter accepts trial iterate
+          FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
+          if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
+            step = soc_step;
+            α = α_soc;
+            α_z = α_z_soc;
+            step_acceptable = true;
+            break;
+          }
+
           // Constraint violation scale factor for second-order corrections
           constexpr Scalar κ_soc(0.99);
 
@@ -597,18 +631,11 @@ ExitStatus interior_point(
           next_constraint_violation =
               trial_c_e.template lpNorm<1>() +
               (trial_c_i - trial_s).template lpNorm<1>();
-          if (next_constraint_violation > κ_soc * prev_constraint_violation) {
+          if (next_constraint_violation > κ_soc * soc_constraint_violation) {
             break;
           }
 
-          // Check whether filter accepts trial iterate
-          FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
-          if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
-            step = soc_step;
-            α = α_soc;
-            α_z = α_z_soc;
-            step_acceptable = true;
-          }
+          soc_constraint_violation = next_constraint_violation;
         }
 
         if (step_acceptable) {
@@ -648,21 +675,19 @@ ExitStatus interior_point(
 
         trial_x = x + α_max * step.p_x;
         trial_s = s + α_max * step.p_s;
-
         trial_y = y + α_z * step.p_y;
         trial_z = z + α_z * step.p_z;
 
+        trial_f = matrices.f(trial_x);
         trial_c_e = matrices.c_e(trial_x);
         trial_c_i = matrices.c_i(trial_x);
 
         Scalar next_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(
-            matrices.g(trial_x), matrices.A_e(trial_x), matrices.c_e(trial_x),
+            matrices.g(trial_x), matrices.A_e(trial_x), trial_c_e,
             matrices.A_i(trial_x), trial_c_i, trial_s, trial_y, trial_z, μ);
 
         // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
         if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
-          α = α_max;
-
           // Accept step
           break;
         }
@@ -675,6 +700,9 @@ ExitStatus interior_point(
     line_search_profiler.stop();
 
     if (call_feasibility_restoration) {
+      ScopedProfiler feasibility_restoration_profiler{
+          feasibility_restoration_prof};
+
       // If already in feasibility restoration mode, running it again won't help
       if (in_feasibility_restoration) {
         return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
@@ -705,27 +733,32 @@ ExitStatus interior_point(
                    Scalar(0.9) * initial_entry.constraint_violation &&
                filter.try_add(initial_entry, trial_entry, trial_x - x, g, α);
       });
-      auto status = feasibility_restoration<Scalar>(matrices, callbacks,
-                                                    options, x, s, y, z, μ);
+      auto status =
+          feasibility_restoration<Scalar>(matrices, callbacks, options,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+                                          bound_constraint_mask,
+#endif
+                                          x, s, y, z, μ, iterations);
 
       if (status != ExitStatus::SUCCESS) {
         // Report failure
         return status;
       }
+
+      f = matrices.f(x);
+      c_e = matrices.c_e(x);
+      c_i = matrices.c_i(x);
     } else {
       // If full step was accepted, reset full-step rejected counter
       if (α == α_max) {
         full_step_rejected_counter = 0;
       }
 
-      // xₖ₊₁ = xₖ + αₖpₖˣ
-      // sₖ₊₁ = sₖ + αₖpₖˢ
-      // yₖ₊₁ = yₖ + αₖᶻpₖʸ
-      // zₖ₊₁ = zₖ + αₖᶻpₖᶻ
-      x += α * step.p_x;
-      s += α * step.p_s;
-      y += α_z * step.p_y;
-      z += α_z * step.p_z;
+      // Update iterates
+      x = trial_x;
+      s = trial_s;
+      y = trial_y;
+      z = trial_z;
 
       // A requirement for the convergence proof is that the primal-dual barrier
       // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal
@@ -745,23 +778,21 @@ ExitStatus interior_point(
         z[row] =
             std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]);
       }
+
+      f = trial_f;
+      c_e = trial_c_e;
+      c_i = trial_c_i;
     }
 
     // Update autodiff for Jacobians and Hessian
-    f = matrices.f(x);
     A_e = matrices.A_e(x);
     A_i = matrices.A_i(x);
     g = matrices.g(x);
     H = matrices.H(x, y, z);
 
-    ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof};
-
-    c_e = matrices.c_e(x);
-    c_i = matrices.c_i(x);
-
     // Update the error
-    E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
-        g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
+    E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+        matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
 
     // Update the barrier parameter if necessary
     if (E_0 > Scalar(options.tolerance)) {
@@ -779,7 +810,6 @@ ExitStatus interior_point(
       }
     }
 
-    next_iter_prep_profiler.stop();
     inner_iter_profiler.stop();
 
     if (options.diagnostics) {
@@ -789,8 +819,13 @@ ExitStatus interior_point(
                                      : IterationType::NORMAL,
           inner_iter_profiler.current_duration(), E_0, f,
           c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>(), s.dot(z),
-          μ, solver.hessian_regularization(), α, α_max, α_reduction_factor,
-          α_z);
+          μ, solver.hessian_regularization(),
+          solver.constraint_jacobian_regularization(),
+          std::max(step.p_x.template lpNorm<Eigen::Infinity>(),
+                   step.p_s.template lpNorm<Eigen::Infinity>()),
+          std::max(step.p_y.template lpNorm<Eigen::Infinity>(),
+                   step.p_z.template lpNorm<Eigen::Infinity>()),
+          α, α_max, α_reduction_factor, α_z);
     }
 
     ++iterations;

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
 
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
+
 namespace slp {
 
 /// Matrix callbacks for the interior-point method solver.
@@ -76,6 +78,8 @@ struct InteriorPointMatrixCallbacks {
   ///
   /// L(x, y, z) = f(x) − yᵀcₑ(x) − zᵀcᵢ(x)
   ///
+  /// Only the lower triangle is used.
+  ///
   /// <table>
   ///   <tr>
   ///     <th>Variable</th>
@@ -109,6 +113,8 @@ struct InteriorPointMatrixCallbacks {
 
   /// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x) − zᵀcᵢ(x)) getter.
   ///
+  /// Only the lower triangle is used.
+  ///
   /// <table>
   ///   <tr>
   ///     <th>Variable</th>
@@ -237,6 +243,10 @@ struct InteriorPointMatrixCallbacks {
   ///   </tr>
   /// </table>
   std::function<SparseMatrix(const DenseVector& x)> A_i;
+
+  /// Automatic problem scaling factors. Used to scale the cost, constraints,
+  /// and tolerance inside the interior-point solver.
+  ProblemScaling<Scalar> scaling;
 };
 
 }  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp

@@ -5,7 +5,6 @@
 #include <chrono>
 #include <cmath>
 #include <functional>
-#include <limits>
 #include <span>
 
 #include <Eigen/Core>
@@ -71,7 +70,6 @@ ExitStatus newton(
   solve_profilers.emplace_back("  ↳ KKT matrix decomp");
   solve_profilers.emplace_back("  ↳ KKT system solve");
   solve_profilers.emplace_back("  ↳ line search");
-  solve_profilers.emplace_back("  ↳ next iter prep");
   solve_profilers.emplace_back("  ↳ f(x)");
   solve_profilers.emplace_back("  ↳ ∇f(x)");
   solve_profilers.emplace_back("  ↳ ∇²ₓₓL");
@@ -84,13 +82,12 @@ ExitStatus newton(
   auto& kkt_matrix_decomp_prof = solve_profilers[5];
   auto& kkt_system_solve_prof = solve_profilers[6];
   auto& line_search_prof = solve_profilers[7];
-  auto& next_iter_prep_prof = solve_profilers[8];
 
   // Set up profiled matrix callbacks
 #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
-  auto& f_prof = solve_profilers[9];
-  auto& g_prof = solve_profilers[10];
-  auto& H_prof = solve_profilers[11];
+  auto& f_prof = solve_profilers[8];
+  auto& g_prof = solve_profilers[9];
+  auto& H_prof = solve_profilers[10];
 
   NewtonMatrixCallbacks<Scalar> matrices{
       matrix_callbacks.num_decision_variables,
@@ -105,7 +102,8 @@ ExitStatus newton(
       [&](const DenseVector& x) -> SparseMatrix {
         ScopedProfiler prof{H_prof};
         return matrix_callbacks.H(x);
-      }};
+      },
+      matrix_callbacks.scaling};
 #else
   const auto& matrices = matrix_callbacks;
 #endif
@@ -122,6 +120,10 @@ ExitStatus newton(
   slp_assert(H.rows() == matrices.num_decision_variables);
   slp_assert(H.cols() == matrices.num_decision_variables);
 
+  DenseVector trial_x;
+
+  Scalar trial_f;
+
   // Check whether initial guess has finite cost and derivatives
   if (!isfinite(f) || !all_finite(g) || !all_finite(H)) {
     return ExitStatus::NONFINITE_INITIAL_GUESS;
@@ -131,14 +133,17 @@ ExitStatus newton(
 
   Filter<Scalar> filter;
 
-  RegularizedLDLT<Scalar> solver{matrices.num_decision_variables, 0};
+  RegularizedLDLT<Scalar> solver{
+      // Use sparse solver if lower triangle fills < 25% of system
+      H.nonZeros() < 0.25 * H.size(), matrices.num_decision_variables, 0};
 
   // Variables for determining when a step is acceptable
   constexpr Scalar α_reduction_factor(0.5);
   constexpr Scalar α_min(1e-20);
 
   // Error
-  Scalar E_0 = std::numeric_limits<Scalar>::infinity();
+  Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+      matrices.scaling, g);
 
   setup_prof.stop();
 
@@ -194,9 +199,9 @@ ExitStatus newton(
     // Loop until a step is accepted. If a step becomes acceptable, the loop
     // will exit early.
     while (1) {
-      DenseVector trial_x = x + α * p_x;
+      trial_x = x + α * p_x;
 
-      Scalar trial_f = matrices.f(trial_x);
+      trial_f = matrices.f(trial_x);
 
       // If f(xₖ + αpₖˣ) isn't finite, reduce step size immediately
       if (!isfinite(trial_f)) {
@@ -223,14 +228,14 @@ ExitStatus newton(
       if (α < α_min) {
         Scalar current_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(g);
 
-        DenseVector trial_x = x + α_max * p_x;
+        trial_x = x + α_max * p_x;
 
         Scalar next_kkt_error =
             kkt_error<Scalar, KKTErrorType::ONE_NORM>(matrices.g(trial_x));
 
         // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
         if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
-          α = α_max;
+          trial_f = matrices.f(trial_x);
 
           // Accept step
           break;
@@ -242,28 +247,29 @@ ExitStatus newton(
 
     line_search_profiler.stop();
 
-    // xₖ₊₁ = xₖ + αₖpₖˣ
-    x += α * p_x;
+    // Update iterates
+    x = trial_x;
+
+    f = trial_f;
 
     // Update autodiff for Hessian
-    f = matrices.f(x);
     g = matrices.g(x);
     H = matrices.H(x);
 
-    ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof};
-
     // Update the error
-    E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g);
+    E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+        matrices.scaling, g);
 
-    next_iter_prep_profiler.stop();
     inner_iter_profiler.stop();
 
     if (options.diagnostics) {
-      print_iteration_diagnostics(iterations, IterationType::NORMAL,
-                                  inner_iter_profiler.current_duration(), E_0,
-                                  f, Scalar(0), Scalar(0), Scalar(0),
-                                  solver.hessian_regularization(), α, α_max,
-                                  α_reduction_factor, Scalar(1));
+      print_iteration_diagnostics(
+          iterations, IterationType::NORMAL,
+          inner_iter_profiler.current_duration(), E_0, f, Scalar(0), Scalar(0),
+          Scalar(0), solver.hessian_regularization(),
+          solver.constraint_jacobian_regularization(),
+          p_x.template lpNorm<Eigen::Infinity>(), Scalar(1), α, α_max,
+          α_reduction_factor, Scalar(1));
     }
 
     ++iterations;

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
 
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
+
 namespace slp {
 
 /// Matrix callbacks for the Newton's method solver.
@@ -70,6 +72,8 @@ struct NewtonMatrixCallbacks {
   ///
   /// L(x) = f(x)
   ///
+  /// Only the lower triangle is used.
+  ///
   /// <table>
   ///   <tr>
   ///     <th>Variable</th>
@@ -88,6 +92,10 @@ struct NewtonMatrixCallbacks {
   ///   </tr>
   /// </table>
   std::function<SparseMatrix(const DenseVector& x)> H;
+
+  /// Automatic problem scaling factors. Used to scale the cost and tolerance
+  /// inside the Newton solver.
+  ProblemScaling<Scalar> scaling;
 };
 
 }  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp

@@ -5,7 +5,6 @@
 #include <chrono>
 #include <cmath>
 #include <functional>
-#include <limits>
 #include <span>
 
 #include <Eigen/Core>
@@ -121,7 +120,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
   solve_profilers.emplace_back("  ↳ KKT system solve");
   solve_profilers.emplace_back("  ↳ line search");
   solve_profilers.emplace_back("    ↳ SOC");
-  solve_profilers.emplace_back("  ↳ next iter prep");
+  solve_profilers.emplace_back("  ↳ feas. restoration");
   solve_profilers.emplace_back("  ↳ f(x)");
   solve_profilers.emplace_back("  ↳ ∇f(x)");
   solve_profilers.emplace_back("  ↳ ∇²ₓₓL");
@@ -139,7 +138,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
   auto& kkt_system_solve_prof = solve_profilers[7];
   auto& line_search_prof = solve_profilers[8];
   auto& soc_prof = solve_profilers[9];
-  auto& next_iter_prep_prof = solve_profilers[10];
+  auto& feasibility_restoration_prof = solve_profilers[10];
 
   // Set up profiled matrix callbacks
 #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
@@ -176,7 +175,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
       [&](const DenseVector& x) -> SparseMatrix {
         ScopedProfiler prof{A_e_prof};
         return matrix_callbacks.A_e(x);
-      }};
+      },
+      matrix_callbacks.scaling};
 #else
   const auto& matrices = matrix_callbacks;
 #endif
@@ -198,6 +198,12 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
   slp_assert(A_e.rows() == matrices.num_equality_constraints);
   slp_assert(A_e.cols() == matrices.num_decision_variables);
 
+  DenseVector trial_x;
+  DenseVector trial_y;
+
+  Scalar trial_f;
+  DenseVector trial_c_e;
+
   // Check for overconstrained problem
   if (matrices.num_equality_constraints > matrices.num_decision_variables) {
     if (options.diagnostics) {
@@ -220,8 +226,12 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
   // Kept outside the loop so its storage can be reused
   gch::small_vector<Eigen::Triplet<Scalar>> triplets;
 
-  RegularizedLDLT<Scalar> solver{matrices.num_decision_variables,
-                                 matrices.num_equality_constraints};
+  const int lhs_rows =
+      matrices.num_decision_variables + matrices.num_equality_constraints;
+  RegularizedLDLT<Scalar> solver{
+      // Use sparse solver if lower triangle fills < 25% of system
+      H.nonZeros() + A_e.nonZeros() < 0.25 * lhs_rows * lhs_rows,
+      matrices.num_decision_variables, matrices.num_equality_constraints};
 
   // Variables for determining when a step is acceptable
   constexpr Scalar α_reduction_factor(0.5);
@@ -230,7 +240,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
   int full_step_rejected_counter = 0;
 
   // Error
-  Scalar E_0 = std::numeric_limits<Scalar>::infinity();
+  Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+      matrices.scaling, g, A_e, c_e, y);
 
   setup_prof.stop();
 
@@ -331,11 +342,11 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
 
     // Loop until a step is accepted
     while (1) {
-      DenseVector trial_x = x + α * step.p_x;
-      DenseVector trial_y = y + α * step.p_y;
+      trial_x = x + α * step.p_x;
+      trial_y = y + α * step.p_y;
 
-      Scalar trial_f = matrices.f(trial_x);
-      DenseVector trial_c_e = matrices.c_e(trial_x);
+      trial_f = matrices.f(trial_x);
+      trial_c_e = matrices.c_e(trial_x);
 
       // If f(xₖ + αpₖˣ) or cₑ(xₖ + αpₖˣ) aren't finite, reduce step size
       // immediately
@@ -372,6 +383,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
         Scalar α_soc = α;
         DenseVector c_e_soc = c_e;
 
+        Scalar soc_constraint_violation = next_constraint_violation;
+
         bool step_acceptable = false;
         for (int soc_iteration = 0; soc_iteration < 5 && !step_acceptable;
              ++soc_iteration) {
@@ -380,17 +393,18 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
           scope_exit soc_exit{[&] {
             soc_profiler.stop();
 
-            if (options.diagnostics) {
+            if (options.diagnostics && step_acceptable) {
               print_iteration_diagnostics(
-                  iterations,
-                  step_acceptable ? IterationType::ACCEPTED_SOC
-                                  : IterationType::REJECTED_SOC,
+                  iterations, IterationType::SECOND_ORDER_CORRECTION,
                   soc_profiler.current_duration(),
                   kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
                       g, A_e, trial_c_e, trial_y),
                   trial_f, trial_c_e.template lpNorm<1>(), Scalar(0), Scalar(0),
-                  solver.hessian_regularization(), α_soc, Scalar(1),
-                  α_reduction_factor, Scalar(1));
+                  solver.hessian_regularization(),
+                  solver.constraint_jacobian_regularization(),
+                  soc_step.p_x.template lpNorm<Eigen::Infinity>(),
+                  soc_step.p_y.template lpNorm<Eigen::Infinity>(), α_soc,
+                  Scalar(1), α_reduction_factor, Scalar(1));
             }
           }};
 
@@ -412,23 +426,26 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
           trial_f = matrices.f(trial_x);
           trial_c_e = matrices.c_e(trial_x);
 
+          // Check whether the filter accepts trial iterate
+          FilterEntry trial_entry{trial_f, trial_c_e};
+          if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
+            step = soc_step;
+            α = α_soc;
+            step_acceptable = true;
+            break;
+          }
+
           // Constraint violation scale factor for second-order corrections
           constexpr Scalar κ_soc(0.99);
 
           // If constraint violation hasn't been sufficiently reduced, stop
           // making second-order corrections
           next_constraint_violation = trial_c_e.template lpNorm<1>();
-          if (next_constraint_violation > κ_soc * prev_constraint_violation) {
+          if (next_constraint_violation > κ_soc * soc_constraint_violation) {
             break;
           }
 
-          // Check whether filter accepts trial iterate
-          FilterEntry trial_entry{trial_f, trial_c_e};
-          if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
-            step = soc_step;
-            α = α_soc;
-            step_acceptable = true;
-          }
+          soc_constraint_violation = next_constraint_violation;
         }
 
         if (step_acceptable) {
@@ -469,6 +486,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
         trial_x = x + α_max * step.p_x;
         trial_y = y + α_max * step.p_y;
 
+        trial_f = matrices.f(trial_x);
         trial_c_e = matrices.c_e(trial_x);
 
         Scalar next_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(
@@ -476,8 +494,6 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
 
         // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
         if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
-          α = α_max;
-
           // Accept step
           break;
         }
@@ -490,6 +506,9 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
     line_search_profiler.stop();
 
     if (call_feasibility_restoration) {
+      ScopedProfiler feasibility_restoration_profiler{
+          feasibility_restoration_prof};
+
       FilterEntry initial_entry{matrices.f(x), c_e};
 
       // Feasibility restoration phase
@@ -512,47 +531,50 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
                    Scalar(0.9) * initial_entry.constraint_violation &&
                filter.try_add(initial_entry, trial_entry, trial_x - x, g, α);
       });
-      auto status =
-          feasibility_restoration<Scalar>(matrices, callbacks, options, x, y);
+      auto status = feasibility_restoration<Scalar>(matrices, callbacks,
+                                                    options, x, y, iterations);
 
       if (status != ExitStatus::SUCCESS) {
         // Report failure
         return status;
       }
+
+      f = matrices.f(x);
+      c_e = matrices.c_e(x);
     } else {
       // If full step was accepted, reset full-step rejected counter
       if (α == α_max) {
         full_step_rejected_counter = 0;
       }
 
-      // xₖ₊₁ = xₖ + αₖpₖˣ
-      // yₖ₊₁ = yₖ + αₖpₖʸ
-      x += α * step.p_x;
-      y += α * step.p_y;
+      // Update iterates
+      x = trial_x;
+      y = trial_y;
+
+      f = trial_f;
+      c_e = trial_c_e;
     }
 
     // Update autodiff for Jacobians and Hessian
-    f = matrices.f(x);
     A_e = matrices.A_e(x);
     g = matrices.g(x);
     H = matrices.H(x, y);
 
-    ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof};
-
-    c_e = matrices.c_e(x);
-
     // Update the error
-    E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g, A_e, c_e, y);
+    E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+        matrices.scaling, g, A_e, c_e, y);
 
-    next_iter_prep_profiler.stop();
     inner_iter_profiler.stop();
 
     if (options.diagnostics) {
       print_iteration_diagnostics(iterations, IterationType::NORMAL,
                                   inner_iter_profiler.current_duration(), E_0,
                                   f, c_e.template lpNorm<1>(), Scalar(0),
-                                  Scalar(0), solver.hessian_regularization(), α,
-                                  α_max, α_reduction_factor, α);
+                                  Scalar(0), solver.hessian_regularization(),
+                                  solver.constraint_jacobian_regularization(),
+                                  step.p_x.template lpNorm<Eigen::Infinity>(),
+                                  step.p_y.template lpNorm<Eigen::Infinity>(),
+                                  α, α_max, α_reduction_factor, α);
     }
 
     ++iterations;

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
 
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
+
 namespace slp {
 
 /// Matrix callbacks for the Sequential Quadratic Programming (SQP) solver.
@@ -73,6 +75,8 @@ struct SQPMatrixCallbacks {
   ///
   /// L(x, y) = f(x) − yᵀcₑ(x)
   ///
+  /// Only the lower triangle is used.
+  ///
   /// <table>
   ///   <tr>
   ///     <th>Variable</th>
@@ -99,6 +103,8 @@ struct SQPMatrixCallbacks {
 
   /// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x)) getter.
   ///
+  /// Only the lower triangle is used.
+  ///
   /// <table>
   ///   <tr>
   ///     <th>Variable</th>
@@ -171,6 +177,10 @@ struct SQPMatrixCallbacks {
   ///   </tr>
   /// </table>
   std::function<SparseMatrix(const DenseVector& x)> A_e;
+
+  /// Automatic problem scaling factors. Used to scale the cost, constraints,
+  /// and tolerance inside the SQP solver.
+  ProblemScaling<Scalar> scaling;
 };
 
 }  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp

@@ -6,6 +6,8 @@
 
 #include <Eigen/SparseCore>
 
+namespace slp {
+
 /// Returns true if elements of sparse matrix are all finite.
 ///
 /// @param mat Sparse matrix.
@@ -24,3 +26,5 @@ bool all_finite(const Eigen::SparseCompressedBase<Derived>& mat) {
 
   return true;
 }
+
+}  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/SparseCore>
 #include <gch/small_vector.hpp>
 
+namespace slp {
+
 /// Appends sparse matrices to list of triplets at the given offset.
 ///
 /// The triplets are appended in column-major order (e.g., first column of mat1,
@@ -58,3 +60,5 @@ void append_diagonal_as_triplets(
     triplets.emplace_back(row_offset + row, col_offset + row, diag[row]);
   }
 }
+
+}  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp

@@ -0,0 +1,210 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <limits>
+
+#include <Eigen/Cholesky>
+#include <Eigen/Core>
+
+#include "sleipnir/optimization/solver/util/inertia.hpp"
+
+namespace slp {
+
+/// Solves dense systems of linear equations using a regularized LDLT
+/// factorization.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+class DenseRegularizedLDLT {
+ public:
+  /// Type alias for dense matrix.
+  using DenseMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
+  /// Type alias for dense vector.
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  /// Constructs a DenseRegularizedLDLT instance.
+  ///
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  DenseRegularizedLDLT(int num_decision_variables, int num_equality_constraints)
+      : m_num_decision_variables{num_decision_variables},
+        m_num_equality_constraints{num_equality_constraints} {}
+
+  /// Constructs a DenseRegularizedLDLT instance.
+  ///
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  /// @param γ_min The minimum constraint regularization.
+  DenseRegularizedLDLT(int num_decision_variables, int num_equality_constraints,
+                       Scalar γ_min)
+      : m_num_decision_variables{num_decision_variables},
+        m_num_equality_constraints{num_equality_constraints},
+        m_γ_min{γ_min} {}
+
+  /// Reports whether previous computation was successful.
+  ///
+  /// @return Whether previous computation was successful.
+  Eigen::ComputationInfo info() const { return m_info; }
+
+  /// Computes the regularized LDLT factorization of a matrix.
+  ///
+  /// @param lhs Left-hand side of the system.
+  /// @return The factorization.
+  DenseRegularizedLDLT& compute(const DenseMatrix& lhs) {
+    m_info = m_solver.compute(lhs).info();
+
+    if (m_info == Eigen::Success) {
+      auto D = m_solver.vectorD();
+
+      // If the inertia is ideal and D from LDLT is sufficiently far from zero,
+      // don't regularize the system
+      if (Inertia{D} == ideal_inertia &&
+          (D.cwiseAbs().array() >= Scalar(1e-4)).all()) {
+        m_prev_δ = Scalar(0);
+        m_prev_γ = Scalar(0);
+        return *this;
+      }
+    }
+
+    // We'll give lhs the correct inertia by adding [δI, 0; 0, −γI] where δ and
+    // γ regularize the Hessian and equality constraint Jacobian respectively.
+
+    // If the previous δ was zero, attempt a small value. Otherwise, attempt a
+    // smaller value than the previous δ so δ trends downward.
+    Scalar δ = m_prev_δ == Scalar(0)
+                   ? Scalar(1e-4)
+                   : std::max(m_prev_δ / Scalar(2),
+                              std::numeric_limits<Scalar>::epsilon());
+
+    // Start γ at the minimum to minimize equality constraint Jacobian
+    // distortion.
+    Scalar γ = m_γ_min;
+
+    while (true) {
+      m_info = m_solver.compute(lhs + regularization(δ, γ)).info();
+
+      if (m_info == Eigen::Success) {
+        Inertia inertia{m_solver.vectorD()};
+
+        if (inertia == ideal_inertia) {
+          // If the inertia is ideal, report success
+          m_prev_δ = δ;
+          m_prev_γ = γ;
+          return *this;
+        } else if (inertia.zero > 0) {
+          if (γ == Scalar(0)) {
+            // If there's zero eigenvalues and γ = 0, increase γ to potentially
+            // compensate for a rank-deficient equality constraint Jacobian
+            γ = Scalar(1e-10);
+          } else {
+            // If there's zero eigenvalues and γ > 0, increase δ and γ to drive
+            // all eigenvalues away from zero
+            δ *= Scalar(10);
+            γ *= Scalar(10);
+          }
+        } else if (inertia.negative > ideal_inertia.negative) {
+          // If there's too many negative eigenvalues, increase δ to add more
+          // positive eigenvalues
+          δ *= Scalar(10);
+        } else if (inertia.positive > ideal_inertia.positive) {
+          // If there's too many positive eigenvalues, increase γ to add more
+          // negative eigenvalues
+          γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
+        }
+      } else {
+        // If the decomposition failed, increase δ and γ to drive all
+        // eigenvalues away from zero
+        δ *= Scalar(10);
+        γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
+      }
+
+      // If the lhs perturbation is too high, report failure. This can be caused
+      // by ill-conditioning.
+      if (δ > Scalar(1e20) || γ > Scalar(1e20)) {
+        m_info = Eigen::NumericalIssue;
+        m_prev_δ = δ;
+        m_prev_γ = γ;
+        return *this;
+      }
+    }
+  }
+
+  /// Solves the system of equations using a regularized LDLT factorization.
+  ///
+  /// @param rhs Right-hand side of the system.
+  /// @return The solution.
+  template <typename Rhs>
+  DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
+    return m_solver.solve(rhs);
+  }
+
+  /// Solves the system of equations using a regularized LDLT factorization.
+  ///
+  /// @param rhs Right-hand side of the system.
+  /// @return The solution.
+  template <typename Rhs>
+  DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
+    return m_solver.solve(rhs.toDense());
+  }
+
+  /// Returns the Hessian regularization factor.
+  ///
+  /// @return Hessian regularization factor.
+  Scalar hessian_regularization() const { return m_prev_δ; }
+
+  /// Returns the constraint Jacobian regularization factor.
+  ///
+  /// @return Constraint Jacobian regularization factor.
+  Scalar constraint_jacobian_regularization() const { return m_prev_γ; }
+
+ private:
+  using Solver = Eigen::LDLT<DenseMatrix>;
+
+  Solver m_solver;
+
+  Eigen::ComputationInfo m_info = Eigen::Success;
+
+  /// The number of decision variables in the system.
+  int m_num_decision_variables = 0;
+
+  /// The number of equality constraints in the system.
+  int m_num_equality_constraints = 0;
+
+  /// The minimum constraint regularization.
+  Scalar m_γ_min{1e-10};
+
+  /// The ideal system inertia.
+  Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
+                        0};
+
+  /// The value of δ from the previous run of compute().
+  Scalar m_prev_δ{0};
+
+  /// The value of γ from the previous run of compute().
+  Scalar m_prev_γ{0};
+
+  /// Returns regularization matrix.
+  ///
+  ///   [δI    0]
+  ///   [ 0  −γI]
+  ///
+  /// @param δ The Hessian regularization factor.
+  /// @param γ The equality constraint Jacobian regularization factor.
+  /// @return Regularization matrix.
+  DenseMatrix regularization(Scalar δ, Scalar γ) const {
+    DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
+    vec.segment(0, m_num_decision_variables).setConstant(δ);
+    vec.segment(m_num_decision_variables, m_num_equality_constraints)
+        .setConstant(-γ);
+
+    return vec.asDiagonal().toDenseMatrix();
+  }
+};
+
+}  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp

@@ -33,7 +33,7 @@ ExitStatus interior_point(
     Eigen::Vector<Scalar, Eigen::Dynamic>& x,
     Eigen::Vector<Scalar, Eigen::Dynamic>& s,
     Eigen::Vector<Scalar, Eigen::Dynamic>& y,
-    Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar& μ);
+    Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar& μ, int& iterations);
 
 /// Computes initial values for p and n in feasibility restoration.
 ///
@@ -111,6 +111,7 @@ compute_p_n(const Eigen::Vector<Scalar, Eigen::Dynamic>& c, Scalar ρ,
 /// @param[in,out] x The decision variables from the normal solve.
 /// @param[in,out] y The equality constraint dual variables from the normal
 ///     solve.
+/// @param[in,out] iterations The iteration counter.
 /// @return The exit status.
 template <typename Scalar>
 ExitStatus feasibility_restoration(
@@ -118,7 +119,7 @@ ExitStatus feasibility_restoration(
     std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
         iteration_callbacks,
     const Options& options, Eigen::Vector<Scalar, Eigen::Dynamic>& x,
-    Eigen::Vector<Scalar, Eigen::Dynamic>& y) {
+    Eigen::Vector<Scalar, Eigen::Dynamic>& y, int& iterations) {
   // Feasibility restoration
   //
   //        min  ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
@@ -148,12 +149,14 @@ ExitStatus feasibility_restoration(
 
   constexpr Scalar ρ(1e3);
   const Scalar μ(options.tolerance / 10.0);
-  const Scalar ζ = sqrt(μ);
 
   const DenseVector c_e = matrices.c_e(x);
 
+  Scalar fr_μ = std::max(μ, c_e.template lpNorm<Eigen::Infinity>());
+  const Scalar ζ = sqrt(fr_μ);
+
   const auto& x_r = x;
-  const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ);
+  const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, fr_μ);
 
   // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
   const DiagonalMatrix D_r =
@@ -166,10 +169,15 @@ ExitStatus feasibility_restoration(
 
   DenseVector fr_y = DenseVector::Zero(num_eq);
 
+  // Force the duals to start with perfect complementarity with the slacks
   DenseVector fr_z{2 * num_eq};
-  fr_z << μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse();
+  fr_z << fr_μ * p_e_0.cwiseInverse(), fr_μ * n_e_0.cwiseInverse();
 
-  Scalar fr_μ = std::max(μ, c_e.template lpNorm<Eigen::Infinity>());
+  // Inherit the parent problem's scaling for the constraints, and use no
+  // scaling for the cost function since it has changed. The new rows introduced
+  // are not scaled.
+  const ProblemScaling<Scalar> fr_scaling{Scalar(1), matrices.scaling.c_e,
+                                          DenseVector::Ones(2 * num_eq)};
 
   InteriorPointMatrixCallbacks<Scalar> fr_matrix_callbacks{
       static_cast<int>(fr_x.rows()),
@@ -289,14 +297,15 @@ ExitStatus feasibility_restoration(
         SparseMatrix A_i_p{2 * num_eq, x_p.rows()};
         A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
         return A_i_p;
-      }};
+      },
+      fr_scaling};
 
-  auto status = interior_point<Scalar>(fr_matrix_callbacks, iteration_callbacks,
-                                       options, true,
+  auto status = interior_point<Scalar>(
+      fr_matrix_callbacks, iteration_callbacks, options, true,
 #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
-                                       {},
+      Eigen::ArrayX<bool>::Constant(2 * num_eq, true),
 #endif
-                                       fr_x, fr_s, fr_y, fr_z, fr_μ);
+      fr_x, fr_s, fr_y, fr_z, fr_μ, iterations);
 
   x = fr_x.segment(0, x.rows());
 
@@ -331,16 +340,21 @@ ExitStatus feasibility_restoration(
 /// @param[in,out] z The current inequality constraint duals from the normal
 ///     solve.
 /// @param[in] μ Barrier parameter.
+/// @param[in,out] iterations The iteration counter.
 /// @return The exit status.
 template <typename Scalar>
 ExitStatus feasibility_restoration(
     const InteriorPointMatrixCallbacks<Scalar>& matrix_callbacks,
     std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
         iteration_callbacks,
-    const Options& options, Eigen::Vector<Scalar, Eigen::Dynamic>& x,
+    const Options& options,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+    const Eigen::ArrayX<bool>& bound_constraint_mask,
+#endif
+    Eigen::Vector<Scalar, Eigen::Dynamic>& x,
     Eigen::Vector<Scalar, Eigen::Dynamic>& s,
     Eigen::Vector<Scalar, Eigen::Dynamic>& y,
-    Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar μ) {
+    Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar μ, int& iterations) {
   // Feasibility restoration
   //
   //        min  ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
@@ -374,14 +388,17 @@ ExitStatus feasibility_restoration(
   const auto& num_ineq = matrices.num_inequality_constraints;
 
   constexpr Scalar ρ(1e3);
-  const Scalar ζ = sqrt(μ);
 
   const DenseVector c_e = matrices.c_e(x);
   const DenseVector c_i = matrices.c_i(x);
 
+  Scalar fr_μ = std::max({μ, c_e.template lpNorm<Eigen::Infinity>(),
+                          (c_i - s).template lpNorm<Eigen::Infinity>()});
+  const Scalar ζ = sqrt(fr_μ);
+
   const auto& x_r = x;
-  const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ);
-  const auto [p_i_0, n_i_0] = compute_p_n((c_i - s).eval(), ρ, μ);
+  const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, fr_μ);
+  const auto [p_i_0, n_i_0] = compute_p_n((c_i - s).eval(), ρ, fr_μ);
 
   // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
   const DiagonalMatrix D_r =
@@ -396,12 +413,20 @@ ExitStatus feasibility_restoration(
 
   DenseVector fr_y = DenseVector::Zero(c_e.rows());
 
+  // Force the duals to start with perfect complementarity with the slacks
   DenseVector fr_z{c_i.rows() + 2 * num_eq + 2 * num_ineq};
-  fr_z << z.cwiseMin(ρ), μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse(),
-      μ * p_i_0.cwiseInverse(), μ * n_i_0.cwiseInverse();
+  fr_z << fr_μ * s.cwiseInverse(), fr_μ * p_e_0.cwiseInverse(),
+      fr_μ * n_e_0.cwiseInverse(), fr_μ * p_i_0.cwiseInverse(),
+      fr_μ * n_i_0.cwiseInverse();
 
-  Scalar fr_μ = std::max({μ, c_e.template lpNorm<Eigen::Infinity>(),
-                          (c_i - s).template lpNorm<Eigen::Infinity>()});
+  // Inherit the parent problem's scaling for the constraints, and use no
+  // scaling for the cost function since it has changed. The new rows introduced
+  // are not scaled.
+  DenseVector fr_d_c_i{c_i.rows() + 2 * num_eq + 2 * num_ineq};
+  fr_d_c_i << matrices.scaling.c_i,
+      DenseVector::Ones(2 * num_eq + 2 * num_ineq);
+  const ProblemScaling<Scalar> fr_scaling{Scalar(1), matrices.scaling.c_e,
+                                          fr_d_c_i};
 
   InteriorPointMatrixCallbacks<Scalar> fr_matrix_callbacks{
       static_cast<int>(fr_x.rows()),
@@ -564,14 +589,21 @@ ExitStatus feasibility_restoration(
         SparseMatrix A_i_p{2 * num_eq + 3 * num_ineq, x_p.rows()};
         A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
         return A_i_p;
-      }};
+      },
+      fr_scaling};
 
-  auto status = interior_point<Scalar>(fr_matrix_callbacks, iteration_callbacks,
-                                       options, true,
 #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
-                                       {},
+  Eigen::ArrayX<bool> fr_bound_constraint_mask{2 * num_eq + 3 * num_ineq};
+  fr_bound_constraint_mask.segment(0, num_ineq) = bound_constraint_mask;
+  fr_bound_constraint_mask.segment(num_ineq, 2 * num_eq + 2 * num_ineq) = true;
 #endif
-                                       fr_x, fr_s, fr_y, fr_z, fr_μ);
+
+  auto status = interior_point<Scalar>(
+      fr_matrix_callbacks, iteration_callbacks, options, true,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+      fr_bound_constraint_mask,
+#endif
+      fr_x, fr_s, fr_y, fr_z, fr_μ, iterations);
 
   x = fr_x.segment(0, x.rows());
   s = fr_s.segment(0, s.rows());

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp

@@ -4,7 +4,6 @@
 
 #include <algorithm>
 #include <cmath>
-#include <limits>
 
 #include <Eigen/Core>
 #include <Eigen/SparseCore>

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/inertia.hpp

@@ -2,7 +2,10 @@
 
 #pragma once
 
+#include <limits>
+
 #include <Eigen/Core>
+#include <fmt/format.h>
 
 namespace slp {
 
@@ -36,9 +39,9 @@ class Inertia {
   template <typename Scalar>
   explicit Inertia(const Eigen::Vector<Scalar, Eigen::Dynamic>& D) {
     for (const auto& elem : D) {
-      if (elem > Scalar(0)) {
+      if (elem > std::numeric_limits<Scalar>::epsilon()) {
         ++positive;
-      } else if (elem < Scalar(0)) {
+      } else if (elem < -std::numeric_limits<Scalar>::epsilon()) {
         ++negative;
       } else {
         ++zero;
@@ -56,9 +59,9 @@ class Inertia {
       const Eigen::Diagonal<
           const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>& D) {
     for (const auto& elem : D) {
-      if (elem > Scalar(0)) {
+      if (elem > std::numeric_limits<Scalar>::epsilon()) {
         ++positive;
-      } else if (elem < Scalar(0)) {
+      } else if (elem < -std::numeric_limits<Scalar>::epsilon()) {
         ++negative;
       } else {
         ++zero;
@@ -73,3 +76,41 @@ class Inertia {
 };
 
 }  // namespace slp
+
+// @cond Suppress Doxygen
+
+/// Formatter for Inertia.
+template <>
+struct fmt::formatter<slp::Inertia> {
+  /// Parses format string.
+  ///
+  /// @param ctx Format parse context.
+  /// @return Format parse context iterator.
+  constexpr auto parse(fmt::format_parse_context& ctx) {
+    return m_underlying.parse(ctx);
+  }
+
+  /// Formats Inertia.
+  ///
+  /// @tparam FmtContext Format context type.
+  /// @param inertia Inertia.
+  /// @param ctx Format context.
+  /// @return Format context iterator.
+  template <typename FmtContext>
+  constexpr auto format(const slp::Inertia& inertia, FmtContext& ctx) const {
+    auto out = ctx.out();
+
+    out = fmt::format_to(out, "(");
+    out = m_underlying.format(inertia.positive, ctx);
+    out = fmt::format_to(out, ", ");
+    out = m_underlying.format(inertia.negative, ctx);
+    out = fmt::format_to(out, ", ");
+    out = m_underlying.format(inertia.zero, ctx);
+    return fmt::format_to(out, ")");
+  }
+
+ private:
+  fmt::formatter<int> m_underlying;
+};
+
+// @endcond

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/is_locally_infeasible.hpp

@@ -12,10 +12,8 @@ namespace slp {
 /// Returns true if the problem's equality constraints are locally infeasible.
 ///
 /// @tparam Scalar Scalar type.
-/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
-///     current iterate.
-/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
-///     iterate.
+/// @param A_e Equality constraint Jacobian Aₑ(x).
+/// @param c_e Equality constraints cₑ(x).
 template <typename Scalar>
 bool is_equality_locally_infeasible(
     const Eigen::SparseMatrix<Scalar>& A_e,
@@ -33,10 +31,8 @@ bool is_equality_locally_infeasible(
 /// Returns true if the problem's inequality constraints are locally infeasible.
 ///
 /// @tparam Scalar Scalar type.
-/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
-///     the current iterate.
-/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
-///     current iterate.
+/// @param A_i Inequality constraint Jacobian Aᵢ(x).
+/// @param c_i Inequality constraints cᵢ(x).
 template <typename Scalar>
 bool is_inequality_locally_infeasible(
     const Eigen::SparseMatrix<Scalar>& A_i,

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp

@@ -7,6 +7,8 @@
 #include <Eigen/Core>
 #include <Eigen/SparseCore>
 
+#include "sleipnir/optimization/solver/util/problem_scaling.hpp"
+
 // See docs/algorithms.md#Works_cited for citation definitions
 
 namespace slp {
@@ -22,8 +24,8 @@ enum class KKTErrorType {
 /// Returns the KKT error for Newton's method.
 ///
 /// @tparam Scalar Scalar type.
-/// @tparam T The type of KKT error to compute.
-/// @param g Gradient of the cost function ∇f.
+/// @tparam T Type of KKT error to compute.
+/// @param g Cost function gradient ∇f.
 template <typename Scalar, KKTErrorType T>
 Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
   // The KKT conditions from docs/algorithms.md:
@@ -40,12 +42,10 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
 /// Returns the KKT error for Sequential Quadratic Programming.
 ///
 /// @tparam Scalar Scalar type.
-/// @tparam T The type of KKT error to compute.
-/// @param g Gradient of the cost function ∇f.
-/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
-///     current iterate.
-/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
-///     iterate.
+/// @tparam T Type of KKT error to compute.
+/// @param g Cost function gradient ∇f.
+/// @param A_e Equality constraint Jacobian Aₑ(x).
+/// @param c_e Equality constraints cₑ(x).
 /// @param y Equality constraint dual variables.
 template <typename Scalar, KKTErrorType T>
 Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
@@ -79,16 +79,12 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
 /// Returns the KKT error for the interior-point method.
 ///
 /// @tparam Scalar Scalar type.
-/// @tparam T The type of KKT error to compute.
-/// @param g Gradient of the cost function ∇f.
-/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
-///     current iterate.
-/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
-///     iterate.
-/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
-///     the current iterate.
-/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
-///     current iterate.
+/// @tparam T Type of KKT error to compute.
+/// @param g Cost function gradient ∇f.
+/// @param A_e Equality constraint Jacobian Aₑ(x).
+/// @param c_e Equality constraints cₑ(x).
+/// @param A_i Inequality constraint Jacobian Aᵢ(x).
+/// @param c_i Inequality constraints cᵢ(x).
 /// @param s Inequality constraint slack variables.
 /// @param y Equality constraint dual variables.
 /// @param z Inequality constraint dual variables.
@@ -149,4 +145,109 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
   }
 }
 
+/// Returns the unscaled KKT error for Newton's method.
+///
+/// @tparam Scalar Scalar type.
+/// @tparam T Type of KKT error to compute.
+/// @param scaling Problem scaling.
+/// @param g Scaled cost function gradient d_f·∇f.
+template <typename Scalar, KKTErrorType T>
+Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  if (scaling.is_identity()) {
+    return kkt_error<Scalar, T>(g);
+  }
+
+  const DenseVector g_unscaled = (Scalar(1) / scaling.f) * g;
+
+  return kkt_error<Scalar, T>(g_unscaled);
+}
+
+/// Returns the unscaled KKT error for Sequential Quadratic Programming.
+///
+/// @tparam Scalar Scalar type.
+/// @tparam T Type of KKT error to compute.
+/// @param scaling Problem scaling.
+/// @param g Scaled cost function gradient d_f·∇f.
+/// @param A_e Scaled equality constraint Jacobian D_cₑ·Aₑ(x).
+/// @param c_e Scaled equality constraints D_cₑ·cₑ(x).
+/// @param y Scaled equality constraint dual variables.
+template <typename Scalar, KKTErrorType T>
+Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
+                          const Eigen::SparseMatrix<Scalar>& A_e,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& y) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+  if (scaling.is_identity()) {
+    return kkt_error<Scalar, T>(g, A_e, c_e, y);
+  }
+
+  const Scalar inv_d_f = Scalar(1) / scaling.f;
+  const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse();
+
+  const DenseVector g_unscaled = inv_d_f * g;
+  const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e;
+  const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e);
+  const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f;
+
+  return kkt_error<Scalar, T>(g_unscaled, A_e_unscaled, c_e_unscaled,
+                              y_unscaled);
+}
+
+/// Returns the unscaled KKT error for the interior-point method.
+///
+/// @tparam Scalar Scalar type.
+/// @tparam T Type of KKT error to compute.
+/// @param scaling Problem scaling.
+/// @param g Scaled cost function gradient d_f·∇f.
+/// @param A_e Scaled equality constraint Jacobian D_cₑ·Aₑ(x).
+/// @param c_e Scaled equality constraints D_cₑ·cₑ(x).
+/// @param A_i Scaled inequality constraint Jacobian D_cᵢ·Aᵢ(x).
+/// @param c_i Scaled inequality constraints D_cᵢ·cᵢ(x).
+/// @param s Scaled inequality constraint slack variables.
+/// @param y Scaled equality constraint dual variables.
+/// @param z Scaled inequality constraint dual variables.
+/// @param μ Scaled barrier parameter.
+template <typename Scalar, KKTErrorType T>
+Scalar unscaled_kkt_error(const ProblemScaling<Scalar>& scaling,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
+                          const Eigen::SparseMatrix<Scalar>& A_e,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
+                          const Eigen::SparseMatrix<Scalar>& A_i,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& c_i,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& s,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
+                          const Eigen::Vector<Scalar, Eigen::Dynamic>& z,
+                          Scalar μ) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+  if (scaling.is_identity()) {
+    return kkt_error<Scalar, T>(g, A_e, c_e, A_i, c_i, s, y, z, μ);
+  }
+
+  const Scalar inv_d_f = Scalar(1) / scaling.f;
+  const DenseVector inv_d_c_e = scaling.c_e.cwiseInverse();
+  const DenseVector inv_d_c_i = scaling.c_i.cwiseInverse();
+
+  const DenseVector g_unscaled = inv_d_f * g;
+  const SparseMatrix A_e_unscaled = inv_d_c_e.asDiagonal() * A_e;
+  const DenseVector c_e_unscaled = inv_d_c_e.cwiseProduct(c_e);
+  const SparseMatrix A_i_unscaled = inv_d_c_i.asDiagonal() * A_i;
+  const DenseVector c_i_unscaled = inv_d_c_i.cwiseProduct(c_i);
+  const DenseVector s_unscaled = inv_d_c_i.cwiseProduct(s);
+  const DenseVector y_unscaled = scaling.c_e.cwiseProduct(y) * inv_d_f;
+  const DenseVector z_unscaled = scaling.c_i.cwiseProduct(z) * inv_d_f;
+  const Scalar μ_unscaled = inv_d_f * μ;
+
+  return kkt_error<Scalar, T>(g_unscaled, A_e_unscaled, c_e_unscaled,
+                              A_i_unscaled, c_i_unscaled, s_unscaled,
+                              y_unscaled, z_unscaled, μ_unscaled);
+}
+
 }  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp

@@ -27,9 +27,8 @@ struct LagrangeMultiplierEstimate {
 /// Estimates Lagrange multipliers for SQP.
 ///
 /// @tparam Scalar Scalar type.
-/// @param g Gradient of the cost function ∇f.
-/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
-///     current iterate.
+/// @param g Cost function gradient ∇f.
+/// @param A_e Equality constraint Jacobian Aₑ(x).
 template <typename Scalar>
 Eigen::Vector<Scalar, Eigen::Dynamic> lagrange_multiplier_estimate(
     const Eigen::SparseVector<Scalar>& g,
@@ -47,11 +46,9 @@ Eigen::Vector<Scalar, Eigen::Dynamic> lagrange_multiplier_estimate(
 /// Estimates Lagrange multipliers for interior-point method.
 ///
 /// @tparam Scalar Scalar type.
-/// @param g Gradient of the cost function ∇f.
-/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
-///     current iterate.
-/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
-///     the current iterate.
+/// @param g Cost function gradient ∇f.
+/// @param A_e Equality constraint Jacobian Aₑ(x).
+/// @param A_i Inequality constraint Jacobian Aᵢ(x).
 /// @param s Inequality constraint slack variables.
 /// @param μ Barrier parameter.
 template <typename Scalar>

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/problem_scaling.hpp

@@ -0,0 +1,116 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+#include "sleipnir/optimization/solver/util/sparse_inf_norms.hpp"
+
+// See docs/algorithms.md#Works_cited for citation definitions
+
+namespace slp {
+
+/// Automatic problem scaling factors.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+struct ProblemScaling {
+  /// Type alias for dense vector.
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  /// Type alias for sparse matrix.
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+  /// Type alias for sparse vector.
+  using SparseVector = Eigen::SparseVector<Scalar>;
+
+  /// Cost scaling factor d_f.
+  Scalar f = Scalar(1);
+
+  /// Equality constraint scaling factors d_cₑ.
+  DenseVector c_e;
+
+  /// Inequality constraint scaling factors d_cᵢ.
+  DenseVector c_i;
+
+  /// Constructs identity problem scaling.
+  ProblemScaling() = default;
+
+  /// Constructs problem scaling from explicit factors.
+  ///
+  /// @param f Cost scaling factor d_f.
+  /// @param c_e Equality constraint scaling factors d_cₑ.
+  /// @param c_i Inequality constraint scaling factors d_cᵢ.
+  ProblemScaling(Scalar f, const DenseVector& c_e, const DenseVector& c_i)
+      : f{f}, c_e{c_e}, c_i{c_i} {}
+
+  /// Computes Newton problem scaling.
+  ///
+  /// Scales the cost so the largest gradient component at the starting point
+  /// is at most gₘₐₓ:
+  ///
+  ///   d_f = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
+  ///
+  /// See §3.8 Automatic Scaling of the Problem Statement in [2].
+  ///
+  /// @param g Cost gradient ∇f, evaluated at the starting point.
+  explicit ProblemScaling(const DenseVector& g) {
+    constexpr Scalar g_max(100);
+
+    f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
+  }
+
+  /// Computes SQP problem scaling.
+  ///
+  /// Scales the cost and each equality constraint so the largest gradient
+  /// component at the starting point is at most gₘₐₓ:
+  ///
+  ///   d_f     = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
+  ///   d_cₑ[j] = min(1, gₘₐₓ / ‖∇cₑⱼ(x₀)‖_∞)
+  ///
+  /// See §3.8 Automatic Scaling of the Problem Statement in [2].
+  ///
+  /// @param g Cost gradient ∇f, evaluated at the starting point.
+  /// @param A_e Equality constraint Jacobian Aₑ, evaluated at the starting
+  ///     point.
+  ProblemScaling(const DenseVector& g, const SparseMatrix& A_e) {
+    constexpr Scalar g_max(100);
+
+    f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
+    c_e = (g_max / sparse_inf_norms(A_e).array()).min(Scalar(1)).matrix();
+  }
+
+  /// Computes interior-point problem scaling.
+  ///
+  /// Scales the cost and each constraint so the largest gradient
+  /// component at the starting point is at most gₘₐₓ:
+  ///
+  ///   d_f    = min(1, gₘₐₓ / ‖∇f(x₀)‖_∞)
+  ///   d_c[j] = min(1, gₘₐₓ / ‖∇cⱼ(x₀)‖_∞)
+  ///
+  /// See §3.8 Automatic Scaling of the Problem Statement in [2].
+  ///
+  /// @param g Cost gradient ∇f, evaluated at the starting point.
+  /// @param A_e Equality constraint Jacobian Aₑ, evaluated at the starting
+  ///     point.
+  /// @param A_i Inequality constraint Jacobian Aᵢ, evaluated at the starting
+  ///     point.
+  ProblemScaling(const DenseVector& g, const SparseMatrix& A_e,
+                 const SparseMatrix& A_i) {
+    constexpr Scalar g_max(100);
+
+    f = std::min(Scalar(1), g_max / g.template lpNorm<Eigen::Infinity>());
+    c_e = (g_max / sparse_inf_norms(A_e).array()).min(Scalar(1)).matrix();
+    c_i = (g_max / sparse_inf_norms(A_i).array()).min(Scalar(1)).matrix();
+  }
+
+  /// Whether the problem scaling is identity.
+  ///
+  /// @return True if the problem scaling is identity.
+  bool is_identity() const {
+    return f == Scalar(1) && c_e.size() == 0 && c_i.size() == 0;
+  }
+};
+
+}  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp

@@ -2,14 +2,11 @@
 
 #pragma once
 
-#include <Eigen/Cholesky>
 #include <Eigen/Core>
-#include <Eigen/SparseCholesky>
 #include <Eigen/SparseCore>
 
-#include "sleipnir/optimization/solver/util/inertia.hpp"
-
-// See docs/algorithms.md#Works_cited for citation definitions
+#include "sleipnir/optimization/solver/util/dense_regularized_ldlt.hpp"
+#include "sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp"
 
 namespace slp {
 
@@ -19,8 +16,6 @@ namespace slp {
 template <typename Scalar>
 class RegularizedLDLT {
  public:
-  /// Type alias for dense matrix.
-  using DenseMatrix = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
   /// Type alias for dense vector.
   using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
   /// Type alias for sparse matrix.
@@ -28,118 +23,60 @@ class RegularizedLDLT {
 
   /// Constructs a RegularizedLDLT instance.
   ///
+  /// @param use_sparse_solver Whether to use sparse or dense solver.
   /// @param num_decision_variables The number of decision variables in the
   ///     system.
   /// @param num_equality_constraints The number of equality constraints in the
   ///     system.
-  RegularizedLDLT(int num_decision_variables, int num_equality_constraints)
-      : m_num_decision_variables{num_decision_variables},
-        m_num_equality_constraints{num_equality_constraints} {}
+  RegularizedLDLT(bool use_sparse_solver, int num_decision_variables,
+                  int num_equality_constraints)
+      : m_use_sparse_solver{use_sparse_solver},
+        m_sparse_solver{num_decision_variables, num_equality_constraints},
+        m_dense_solver{num_decision_variables, num_equality_constraints} {}
 
   /// Constructs a RegularizedLDLT instance.
   ///
+  /// @param use_sparse_solver Whether to use sparse or dense solver.
   /// @param num_decision_variables The number of decision variables in the
   ///     system.
   /// @param num_equality_constraints The number of equality constraints in the
   ///     system.
   /// @param γ_min The minimum constraint regularization.
-  RegularizedLDLT(int num_decision_variables, int num_equality_constraints,
-                  Scalar γ_min)
-      : m_num_decision_variables{num_decision_variables},
-        m_num_equality_constraints{num_equality_constraints},
-        m_γ_min{γ_min} {}
+  RegularizedLDLT(bool use_sparse_solver, int num_decision_variables,
+                  int num_equality_constraints, Scalar γ_min)
+      : m_use_sparse_solver{use_sparse_solver},
+        m_sparse_solver{num_decision_variables, num_equality_constraints,
+                        γ_min},
+        m_dense_solver{num_decision_variables, num_equality_constraints,
+                       γ_min} {}
 
   /// Reports whether previous computation was successful.
   ///
   /// @return Whether previous computation was successful.
-  Eigen::ComputationInfo info() const { return m_info; }
+  Eigen::ComputationInfo info() const {
+    if (m_use_sparse_solver) {
+      return m_sparse_solver.info();
+    } else {
+      return m_dense_solver.info();
+    }
+  }
 
   /// Computes the regularized LDLT factorization of a matrix.
   ///
+  /// In sparse mode, the matrix's symbolic decomposition is reused in
+  /// subsequent calls, so subsequent calls must be given a matrix with the same
+  /// sparsity pattern.
+  ///
   /// @param lhs Left-hand side of the system.
   /// @return The factorization.
   RegularizedLDLT& compute(const SparseMatrix& lhs) {
-    // The regularization procedure is based on algorithm B.1 of [1]
-
-    // Max density is 50% due to the caller only providing the lower triangle.
-    // We consider less than 25% to be sparse.
-    m_is_sparse = lhs.nonZeros() < 0.25 * lhs.size();
-
-    m_info = m_is_sparse ? compute_sparse(lhs).info()
-                         : m_dense_solver.compute(lhs).info();
-
-    if (m_info == Eigen::Success) {
-      auto D =
-          m_is_sparse ? m_sparse_solver.vectorD() : m_dense_solver.vectorD();
-
-      // If the inertia is ideal and D from LDLT is sufficiently far from zero,
-      // don't regularize the system
-      if (Inertia{D} == ideal_inertia &&
-          (D.cwiseAbs().array() >= Scalar(1e-4)).all()) {
-        m_prev_δ = Scalar(0);
-        return *this;
-      }
+    if (m_use_sparse_solver) {
+      m_sparse_solver.compute(lhs);
+    } else {
+      m_dense_solver.compute(lhs);
     }
 
-    // Also regularize the Hessian. If the Hessian wasn't regularized in a
-    // previous run of compute(), start at small values of δ and γ. Otherwise,
-    // attempt a δ and γ half as big as the previous run so δ and γ can trend
-    // downwards over time.
-    Scalar δ = m_prev_δ == Scalar(0) ? Scalar(1e-4) : m_prev_δ / Scalar(2);
-    Scalar γ = m_γ_min;
-
-    while (true) {
-      Inertia inertia;
-
-      // Regularize lhs by adding a multiple of the identity matrix
-      //
-      // lhs = [H + AᵢᵀΣAᵢ + δI  Aₑᵀ]
-      //       [      Aₑ         −γI]
-      if (m_is_sparse) {
-        m_info = compute_sparse(lhs + regularization(δ, γ)).info();
-        if (m_info == Eigen::Success) {
-          inertia = Inertia{m_sparse_solver.vectorD()};
-        }
-      } else {
-        m_info = m_dense_solver.compute(lhs + regularization(δ, γ)).info();
-        if (m_info == Eigen::Success) {
-          inertia = Inertia{m_dense_solver.vectorD()};
-        }
-      }
-
-      if (m_info == Eigen::Success) {
-        if (inertia == ideal_inertia) {
-          // If the inertia is ideal, store δ and return
-          m_prev_δ = δ;
-          return *this;
-        } else if (inertia.zero > 0) {
-          // If there's zero eigenvalues, increase δ and γ by an order of
-          // magnitude and try again
-          δ *= Scalar(10);
-          γ *= Scalar(10);
-        } else if (inertia.negative > ideal_inertia.negative) {
-          // If there's too many negative eigenvalues, increase δ by an order of
-          // magnitude and try again
-          δ *= Scalar(10);
-        } else if (inertia.positive > ideal_inertia.positive) {
-          // If there's too many positive eigenvalues, increase γ by an order of
-          // magnitude and try again
-          γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
-        }
-      } else {
-        // If the decomposition failed, increase δ and γ by an order of
-        // magnitude and try again
-        δ *= Scalar(10);
-        γ *= Scalar(10);
-      }
-
-      // If the Hessian perturbation is too high, report failure. This can be
-      // caused by ill-conditioning.
-      if (δ > Scalar(1e20) || γ > Scalar(1e20)) {
-        m_info = Eigen::NumericalIssue;
-        return *this;
-      }
-    }
+    return *this;
   }
 
   /// Solves the system of equations using a regularized LDLT factorization.
@@ -148,7 +85,7 @@ class RegularizedLDLT {
   /// @return The solution.
   template <typename Rhs>
   DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
-    if (m_is_sparse) {
+    if (m_use_sparse_solver) {
       return m_sparse_solver.solve(rhs);
     } else {
       return m_dense_solver.solve(rhs);
@@ -161,77 +98,39 @@ class RegularizedLDLT {
   /// @return The solution.
   template <typename Rhs>
   DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
-    if (m_is_sparse) {
+    if (m_use_sparse_solver) {
       return m_sparse_solver.solve(rhs);
     } else {
-      return m_dense_solver.solve(rhs.toDense());
+      return m_dense_solver.solve(rhs);
     }
   }
 
   /// Returns the Hessian regularization factor.
   ///
   /// @return Hessian regularization factor.
-  Scalar hessian_regularization() const { return m_prev_δ; }
+  Scalar hessian_regularization() const {
+    if (m_use_sparse_solver) {
+      return m_sparse_solver.hessian_regularization();
+    } else {
+      return m_dense_solver.hessian_regularization();
+    }
+  }
+
+  /// Returns the constraint Jacobian regularization factor.
+  ///
+  /// @return Constraint Jacobian regularization factor.
+  Scalar constraint_jacobian_regularization() const {
+    if (m_use_sparse_solver) {
+      return m_sparse_solver.constraint_jacobian_regularization();
+    } else {
+      return m_dense_solver.constraint_jacobian_regularization();
+    }
+  }
 
  private:
-  using SparseSolver = Eigen::SimplicialLDLT<SparseMatrix>;
-  using DenseSolver = Eigen::LDLT<DenseMatrix>;
-
-  SparseSolver m_sparse_solver;
-  DenseSolver m_dense_solver;
-  bool m_is_sparse = true;
-
-  Eigen::ComputationInfo m_info = Eigen::Success;
-
-  /// The number of decision variables in the system.
-  int m_num_decision_variables = 0;
-
-  /// The number of equality constraints in the system.
-  int m_num_equality_constraints = 0;
-
-  /// The minimum constraint regularization.
-  Scalar m_γ_min{1e-10};
-
-  /// The ideal system inertia.
-  Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
-                        0};
-
-  /// The value of δ from the previous run of compute().
-  Scalar m_prev_δ{0};
-
-  // Number of non-zeros in LHS.
-  int m_non_zeros = -1;
-
-  /// Computes factorization of a sparse matrix.
-  ///
-  /// @param lhs Matrix to factorize.
-  /// @return The factorization.
-  SparseSolver& compute_sparse(const SparseMatrix& lhs) {
-    // Reanalize lhs's sparsity pattern if it changed
-    int non_zeros = lhs.nonZeros();
-    if (m_non_zeros != non_zeros) {
-      m_sparse_solver.analyzePattern(lhs);
-      m_non_zeros = non_zeros;
-    }
-
-    m_sparse_solver.factorize(lhs);
-
-    return m_sparse_solver;
-  }
-
-  /// Returns regularization matrix.
-  ///
-  /// @param δ The Hessian regularization factor.
-  /// @param γ The equality constraint Jacobian regularization factor.
-  /// @return Regularization matrix.
-  SparseMatrix regularization(Scalar δ, Scalar γ) const {
-    DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
-    vec.segment(0, m_num_decision_variables).setConstant(δ);
-    vec.segment(m_num_decision_variables, m_num_equality_constraints)
-        .setConstant(-γ);
-
-    return SparseMatrix{vec.asDiagonal()};
-  }
+  bool m_use_sparse_solver;
+  SparseRegularizedLDLT<Scalar> m_sparse_solver;
+  DenseRegularizedLDLT<Scalar> m_dense_solver;
 };
 
 }  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_inf_norms.hpp

@@ -0,0 +1,34 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCore>
+
+namespace slp {
+
+/// Computes the row-wise infinity norm of a sparse matrix.
+///
+/// @param mat Sparse matrix.
+/// @return A dense vector where each element is the infinity norm of the
+///         corresponding row of the sparse matrix.
+template <typename Derived>
+auto sparse_inf_norms(const Eigen::SparseCompressedBase<Derived>& mat)
+    -> Eigen::Vector<typename Derived::Scalar, Eigen::Dynamic> {
+  using Scalar = typename Derived::Scalar;
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  DenseVector norms = DenseVector::Zero(mat.rows());
+  for (int k = 0; k < mat.outerSize(); ++k) {
+    for (typename Derived::InnerIterator it{mat, k}; it; ++it) {
+      using std::abs;
+      norms[it.row()] = std::max(norms[it.row()], abs(it.value()));
+    }
+  }
+
+  return norms;
+}
+
+}  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/sparse_regularized_ldlt.hpp

@@ -0,0 +1,227 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <limits>
+
+#include <Eigen/Core>
+#include <Eigen/SparseCholesky>
+#include <Eigen/SparseCore>
+
+#include "sleipnir/optimization/solver/util/inertia.hpp"
+
+namespace slp {
+
+/// Solves sparse systems of linear equations using a regularized LDLT
+/// factorization.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+class SparseRegularizedLDLT {
+ public:
+  /// Type alias for dense vector.
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  /// Type alias for sparse matrix.
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+  /// Constructs a SparseRegularizedLDLT instance.
+  ///
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  SparseRegularizedLDLT(int num_decision_variables,
+                        int num_equality_constraints)
+      : m_num_decision_variables{num_decision_variables},
+        m_num_equality_constraints{num_equality_constraints} {}
+
+  /// Constructs a SparseRegularizedLDLT instance.
+  ///
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  /// @param γ_min The minimum constraint regularization.
+  SparseRegularizedLDLT(int num_decision_variables,
+                        int num_equality_constraints, Scalar γ_min)
+      : m_num_decision_variables{num_decision_variables},
+        m_num_equality_constraints{num_equality_constraints},
+        m_γ_min{γ_min} {}
+
+  /// Reports whether previous computation was successful.
+  ///
+  /// @return Whether previous computation was successful.
+  Eigen::ComputationInfo info() const { return m_info; }
+
+  /// Computes the regularized LDLT factorization of a matrix.
+  ///
+  /// The matrix's symbolic decomposition is reused in subsequent calls, so
+  /// subsequent calls must be given a matrix with the same sparsity pattern.
+  ///
+  /// @param lhs Left-hand side of the system.
+  /// @return The factorization.
+  SparseRegularizedLDLT& compute(const SparseMatrix& lhs) {
+    // Regularization with zeros ensures the pattern analysis in the sparse
+    // solver is reused by all factorizations
+    SparseMatrix unregularized_lhs = lhs + regularization(Scalar(0), Scalar(0));
+
+    if (!m_analyzed_pattern) {
+      m_solver.analyzePattern(unregularized_lhs);
+      m_analyzed_pattern = true;
+    }
+
+    m_solver.factorize(unregularized_lhs);
+    m_info = m_solver.info();
+
+    if (m_info == Eigen::Success) {
+      auto D = m_solver.vectorD();
+
+      // If the inertia is ideal and D from LDLT is sufficiently far from zero,
+      // don't regularize the system
+      if (Inertia{D} == ideal_inertia &&
+          (D.cwiseAbs().array() >= Scalar(1e-4)).all()) {
+        m_prev_δ = Scalar(0);
+        m_prev_γ = Scalar(0);
+        return *this;
+      }
+    }
+
+    // We'll give lhs the correct inertia by adding [δI, 0; 0, −γI] where δ and
+    // γ regularize the Hessian and equality constraint Jacobian respectively.
+
+    // If the previous δ was zero, attempt a small value. Otherwise, attempt a
+    // smaller value than the previous δ so δ trends downward.
+    Scalar δ = m_prev_δ == Scalar(0)
+                   ? Scalar(1e-4)
+                   : std::max(m_prev_δ / Scalar(2),
+                              std::numeric_limits<Scalar>::epsilon());
+
+    // Start γ at the minimum to minimize equality constraint Jacobian
+    // distortion.
+    Scalar γ = m_γ_min;
+
+    while (true) {
+      m_solver.factorize(lhs + regularization(δ, γ));
+      m_info = m_solver.info();
+
+      if (m_info == Eigen::Success) {
+        Inertia inertia{m_solver.vectorD()};
+
+        if (inertia == ideal_inertia) {
+          // If the inertia is ideal, report success
+          m_prev_δ = δ;
+          m_prev_γ = γ;
+          return *this;
+        } else if (inertia.zero > 0) {
+          if (γ == Scalar(0)) {
+            // If there's zero eigenvalues and γ = 0, increase γ to potentially
+            // compensate for a rank-deficient equality constraint Jacobian
+            γ = Scalar(1e-10);
+          } else {
+            // If there's zero eigenvalues and γ > 0, increase δ and γ to drive
+            // all eigenvalues away from zero
+            δ *= Scalar(10);
+            γ *= Scalar(10);
+          }
+        } else if (inertia.negative > ideal_inertia.negative) {
+          // If there's too many negative eigenvalues, increase δ to add more
+          // positive eigenvalues
+          δ *= Scalar(10);
+        } else if (inertia.positive > ideal_inertia.positive) {
+          // If there's too many positive eigenvalues, increase γ to add more
+          // negative eigenvalues
+          γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
+        }
+      } else {
+        // If the decomposition failed, increase δ and γ to drive all
+        // eigenvalues away from zero
+        δ *= Scalar(10);
+        γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
+      }
+
+      // If the lhs perturbation is too high, report failure. This can be caused
+      // by ill-conditioning.
+      if (δ > Scalar(1e20) || γ > Scalar(1e20)) {
+        m_info = Eigen::NumericalIssue;
+        m_prev_δ = δ;
+        m_prev_γ = γ;
+        return *this;
+      }
+    }
+  }
+
+  /// Solves the system of equations using a regularized LDLT factorization.
+  ///
+  /// @param rhs Right-hand side of the system.
+  /// @return The solution.
+  template <typename Rhs>
+  DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
+    return m_solver.solve(rhs);
+  }
+
+  /// Solves the system of equations using a regularized LDLT factorization.
+  ///
+  /// @param rhs Right-hand side of the system.
+  /// @return The solution.
+  template <typename Rhs>
+  DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
+    return m_solver.solve(rhs);
+  }
+
+  /// Returns the Hessian regularization factor.
+  ///
+  /// @return Hessian regularization factor.
+  Scalar hessian_regularization() const { return m_prev_δ; }
+
+  /// Returns the constraint Jacobian regularization factor.
+  ///
+  /// @return Constraint Jacobian regularization factor.
+  Scalar constraint_jacobian_regularization() const { return m_prev_γ; }
+
+ private:
+  using Solver = Eigen::SimplicialLDLT<SparseMatrix>;
+
+  Solver m_solver;
+  bool m_analyzed_pattern = false;
+
+  Eigen::ComputationInfo m_info = Eigen::Success;
+
+  /// The number of decision variables in the system.
+  int m_num_decision_variables = 0;
+
+  /// The number of equality constraints in the system.
+  int m_num_equality_constraints = 0;
+
+  /// The minimum constraint regularization.
+  Scalar m_γ_min{1e-10};
+
+  /// The ideal system inertia.
+  Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
+                        0};
+
+  /// The value of δ from the previous run of compute().
+  Scalar m_prev_δ{0};
+
+  /// The value of γ from the previous run of compute().
+  Scalar m_prev_γ{0};
+
+  /// Returns regularization matrix.
+  ///
+  ///   [δI    0]
+  ///   [ 0  −γI]
+  ///
+  /// @param δ The Hessian regularization factor.
+  /// @param γ The equality constraint Jacobian regularization factor.
+  /// @return Regularization matrix.
+  SparseMatrix regularization(Scalar δ, Scalar γ) const {
+    DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
+    vec.segment(0, m_num_decision_variables).setConstant(δ);
+    vec.segment(m_num_decision_variables, m_num_equality_constraints)
+        .setConstant(-γ);
+
+    return SparseMatrix{vec.asDiagonal()};
+  }
+};
+
+}  // namespace slp

wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp

@@ -25,10 +25,8 @@ namespace slp {
 enum class IterationType : uint8_t {
   /// Normal iteration.
   NORMAL,
-  /// Accepted second-order correction iteration.
-  ACCEPTED_SOC,
-  /// Rejected second-order correction iteration.
-  REJECTED_SOC,
+  /// Second-order correction iteration.
+  SECOND_ORDER_CORRECTION,
   /// Feasibility restoration iteration.
   FEASIBILITY_RESTORATION
 };
@@ -73,8 +71,8 @@ std::string power_of_10(Scalar value) {
       if (exponent < 0) {
         output += "⁻";
       }
-      constexpr std::array strs = {"⁰", "¹", "²", "³", "⁴",
-                                   "⁵", "⁶", "⁷", "⁸", "⁹"};
+      constexpr std::array strs{"⁰", "¹", "²", "³", "⁴",
+                                "⁵", "⁶", "⁷", "⁸", "⁹"};
       for (const auto& digit : digits | std::views::reverse) {
         output += strs[digit];
       }
@@ -183,6 +181,9 @@ inline void print_bound_constraint_global_infeasibility_error(
 /// @param complementarity The complementarity.
 /// @param μ The barrier parameter.
 /// @param δ The Hessian regularization factor.
+/// @param γ The constraint Jacobian regularization factor.
+/// @param full_primal_step_inf_norm The infinity norm of the full primal step.
+/// @param full_dual_step_inf_norm The infinity norm of the full dual step.
 /// @param primal_α The primal step size.
 /// @param primal_α_max The max primal step size.
 /// @param α_reduction_factor Factor by which primal_α is reduced during
@@ -193,33 +194,23 @@ void print_iteration_diagnostics(int iterations, IterationType type,
                                  const std::chrono::duration<Rep, Period>& time,
                                  Scalar error, Scalar cost,
                                  Scalar infeasibility, Scalar complementarity,
-                                 Scalar μ, Scalar δ, Scalar primal_α,
-                                 Scalar primal_α_max, Scalar α_reduction_factor,
-                                 Scalar dual_α) {
+                                 Scalar μ, Scalar δ, Scalar γ,
+                                 Scalar full_primal_step_inf_norm,
+                                 Scalar full_dual_step_inf_norm,
+                                 Scalar primal_α, Scalar primal_α_max,
+                                 Scalar α_reduction_factor, Scalar dual_α) {
   if (iterations % 20 == 0) {
     if (iterations == 0) {
-      slp::print("┏");
+      slp::println("┏{:━^119}┓", "");
     } else {
-      slp::print("┢");
-    }
-    slp::print(
-        "{:━^4}┯{:━^4}┯{:━^9}┯{:━^12}┯{:━^13}┯{:━^12}┯{:━^12}┯{:━^8}┯{:━^5}┯"
-        "{:━^8}┯{:━^8}┯{:━^2}",
-        "", "", "", "", "", "", "", "", "", "", "", "");
-    if (iterations == 0) {
-      slp::println("┓");
-    } else {
-      slp::println("┪");
+      slp::println("┢{:━^119}┪", "");
     }
     slp::println(
-        "┃{:^4}│{:^4}│{:^9}│{:^12}│{:^13}│{:^12}│{:^12}│{:^8}│{:^5}│{:^8}│{:^8}"
-        "│{:^2}┃",
-        "iter", "type", "time (ms)", "error", "cost", "infeas.", "complement.",
-        "μ", "reg", "primal α", "dual α", "↩");
-    slp::println(
-        "┡{:━^4}┷{:━^4}┷{:━^9}┷{:━^12}┷{:━^13}┷{:━^12}┷{:━^12}┷{:━^8}┷{:━^5}┷"
-        "{:━^8}┷{:━^8}┷{:━^2}┩",
-        "", "", "", "", "", "", "", "", "", "", "", "");
+        "┃{:^4}   {:^9} {:^10} {:^11} {:^10} {:^8} {:^8} {:^5} {:^5} {:^8} "
+        "{:^8} {:^8} {:^8} {:^2}┃",
+        "iter", "duration", "error", "cost", "infeas.", "complem.", "μ", "δ",
+        "γ", "|p_pr|", "|p_du|", "α_pr", "α_du", "↩");
+    slp::println("┡{:━^119}┩", "");
   }
 
   // For the number of backtracks, we want x such that:
@@ -236,12 +227,13 @@ void print_iteration_diagnostics(int iterations, IterationType type,
   int backtracks =
       static_cast<int>(log(primal_α / primal_α_max) / log(α_reduction_factor));
 
-  constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC", "rest"};
+  constexpr std::array ITERATION_TYPES{" ", "s", "r"};
   slp::println(
-      "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
-      "{:.2e} {:2d}│",
+      "│{:4} {:1} {:9.3f} {:10.4e} {:11.4e} {:10.4e} {:8.2e} {:8.2e} {:<5} "
+      "{:<5} {:8.2e} {:8.2e} {:8.2e} {:8.2e} {:2d}│",
       iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error,
-      cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
+      cost, infeasibility, complementarity, μ, power_of_10(δ), power_of_10(γ),
+      full_primal_step_inf_norm, full_dual_step_inf_norm, primal_α, dual_α,
       backtracks);
 }
 #else
@@ -251,7 +243,7 @@ void print_iteration_diagnostics(int iterations, IterationType type,
 #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
 /// Prints bottom of iteration diagnostics table.
 inline void print_bottom_iteration_diagnostics() {
-  slp::println("└{:─^108}┘", "");
+  slp::println("└{:─^119}┘", "");
 }
 #else
 #define print_bottom_iteration_diagnostics(...)
@@ -269,7 +261,7 @@ std::string histogram(double value) {
   double ipart;
   int fpart = static_cast<int>(std::modf(value * Width, &ipart) * 8);
 
-  constexpr std::array strs = {" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"};
+  constexpr std::array strs{" ", "▏", "▎", "▍", "▌", "▋", "▊", "▉", "█"};
   std::string hist;
 
   int index = 0;
@@ -297,10 +289,10 @@ inline void print_solver_diagnostics(
     const gch::small_vector<SolveProfiler>& solve_profilers) {
   auto solve_duration = to_ms(solve_profilers[0].total_duration());
 
-  slp::println("┏{:━^21}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", "");
-  slp::println("┃{:^21}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent",
-               "total (ms)", "each (ms)", "runs");
-  slp::println("┡{:━^21}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", "");
+  slp::println("┏{:━^66}┓", "");
+  slp::println("┃{:^21} {:^18} {:^10} {:^9} {:^4}┃", "time trace", "percentage",
+               "total", "each", "runs");
+  slp::println("┡{:━^66}┩", "");
 
   for (auto& profiler : solve_profilers) {
     double norm = solve_duration == 0.0
@@ -326,23 +318,26 @@ inline void print_setup_diagnostics(
     const gch::small_vector<SetupProfiler>& setup_profilers) {
   auto setup_duration = to_ms(setup_profilers[0].duration());
 
+  // Print link to diagnostic output description
+  slp::println(
+      "See https://sleipnirgroup.github.io/Sleipnir/md_usage.html#output for "
+      "diagnostic output description.\n");
+
   // Print heading
-  slp::println("┏{:━^21}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", "");
-  slp::println("┃{:^21}│{:^18}│{:^10}│{:^9}│{:^4}┃", "setup trace", "percent",
-               "total (ms)", "each (ms)", "runs");
-  slp::println("┡{:━^21}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", "");
+  slp::println("┏{:━^50}┓", "");
+  slp::println("┃{:^21} {:^18} {:^9}┃", "time trace", "percentage", "duration");
+  slp::println("┡{:━^50}┩", "");
 
   // Print setup profilers
   for (auto& profiler : setup_profilers) {
     double norm = setup_duration == 0.0
                       ? (&profiler == &setup_profilers[0] ? 1.0 : 0.0)
                       : to_ms(profiler.duration()) / setup_duration;
-    slp::println("│{:<21} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│",
-                 profiler.name(), norm * 100.0, histogram<9>(norm),
-                 to_ms(profiler.duration()), to_ms(profiler.duration()), "1");
+    slp::println("│{:<21} {:>6.2f}%▕{}▏ {:>9.3f}│", profiler.name(),
+                 norm * 100.0, histogram<9>(norm), to_ms(profiler.duration()));
   }
 
-  slp::println("└{:─^66}┘", "");
+  slp::println("└{:─^50}┘", "");
 }
 #else
 #define print_setup_diagnostics(...)

wpimath/src/test/java/org/wpilib/math/optimization/FlywheelOCPTest.java

@@ -103,7 +103,7 @@ class FlywheelOCPTest {
             // steady-states.
             assertEquals(u, problem.U().value(0, k), 2.0);
           } else {
-            assertEquals(u, problem.U().value(0, k), 1e-4);
+            assertEquals(u, problem.U().value(0, k), 2e-4);
           }
         }
 

wpimath/src/test/java/org/wpilib/math/optimization/solver/ExitStatusTest.java

@@ -27,13 +27,16 @@ class ExitStatusTest {
       problem.minimize(x.times(x));
 
       problem.addCallback(info -> false);
+      x.setValue(1.0);
       assertEquals(ExitStatus.SUCCESS, problem.solve());
 
       problem.addCallback(info -> true);
+      x.setValue(1.0);
       assertEquals(ExitStatus.CALLBACK_REQUESTED_STOP, problem.solve());
 
       problem.clearCallbacks();
       problem.addCallback(info -> false);
+      x.setValue(1.0);
       assertEquals(ExitStatus.SUCCESS, problem.solve());
     }
 
@@ -187,6 +190,7 @@ class ExitStatusTest {
 
     try (var problem = new Problem()) {
       var x = problem.decisionVariable();
+      x.setValue(1.0);
 
       problem.minimize(x.times(x));
 
@@ -207,6 +211,7 @@ class ExitStatusTest {
 
     try (var problem = new Problem()) {
       var x = problem.decisionVariable();
+      x.setValue(1.0);
 
       problem.minimize(x.times(x));