[upstream_utils] Upgrade to Sleipnir 0.5.0 (#8711)

2026-06-19 00:41:43 +00:00 · 2026-03-29 20:39:18 -07:00
parent 962168acf1
commit f3757bdeae
47 changed files with 2413 additions and 980 deletions
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression.hpp
@@ -1472,6 +1472,165 @@ ExpressionPtr<Scalar> log10(const ExpressionPtr<Scalar>& x) {
  return make_expression_ptr<Log10Expression<Scalar>>(x);
 }

+/// Derived expression type for max().
+///
+/// Returns the greater of a and b. If the values are equivalent, returns a.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+struct MaxExpression final : Expression<Scalar> {
+  /// Constructs a binary expression (an operator with two arguments).
+  ///
+  /// @param lhs Binary operator's left operand.
+  /// @param rhs Binary operator's right operand.
+  constexpr MaxExpression(ExpressionPtr<Scalar> lhs, ExpressionPtr<Scalar> rhs)
+      : Expression<Scalar>{std::move(lhs), std::move(rhs)} {}
+
+  Scalar value(Scalar a, Scalar b) const override {
+    using std::max;
+    return max(a, b);
+  }
+
+  ExpressionType type() const override { return ExpressionType::NONLINEAR; }
+
+  std::string_view name() const override { return "max"; }
+
+  Scalar grad_l(Scalar a, Scalar b, Scalar parent_adjoint) const override {
+    if (a >= b) {
+      return parent_adjoint;
+    } else {
+      return Scalar(0);
+    }
+  }
+
+  Scalar grad_r(Scalar a, Scalar b, Scalar parent_adjoint) const override {
+    if (b > a) {
+      return parent_adjoint;
+    } else {
+      return Scalar(0);
+    }
+  }
+
+  ExpressionPtr<Scalar> grad_expr_l(
+      const ExpressionPtr<Scalar>& a, const ExpressionPtr<Scalar>& b,
+      const ExpressionPtr<Scalar>& parent_adjoint) const override {
+    if (a->val >= b->val) {
+      return parent_adjoint;
+    } else {
+      return constant_ptr(Scalar(0));
+    }
+  }
+
+  ExpressionPtr<Scalar> grad_expr_r(
+      const ExpressionPtr<Scalar>& a, const ExpressionPtr<Scalar>& b,
+      const ExpressionPtr<Scalar>& parent_adjoint) const override {
+    if (b->val > a->val) {
+      return parent_adjoint;
+    } else {
+      return constant_ptr(Scalar(0));
+    }
+  }
+};
+
+/// max() for Expressions.
+///
+/// @tparam Scalar Scalar type.
+/// @param a The a argument.
+/// @param b The b argument.
+template <typename Scalar>
+ExpressionPtr<Scalar> max(const ExpressionPtr<Scalar>& a,
+                          const ExpressionPtr<Scalar>& b) {
+  using enum ExpressionType;
+  using std::max;
+
+  // Evaluate constant
+  if (a->type() == CONSTANT && b->type() == CONSTANT) {
+    return constant_ptr(max(a->val, b->val));
+  }
+
+  return make_expression_ptr<MaxExpression<Scalar>>(a, b);
+}
+
+/// Derived expression type for min().
+///
+/// Returns the lesser of a and b. If the values are equivalent, returns a.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+struct MinExpression final : Expression<Scalar> {
+  /// Constructs a binary expression (an operator with two arguments).
+  ///
+  /// @param lhs Binary operator's left operand.
+  /// @param rhs Binary operator's right operand.
+  constexpr MinExpression(ExpressionPtr<Scalar> lhs, ExpressionPtr<Scalar> rhs)
+      : Expression<Scalar>{std::move(lhs), std::move(rhs)} {}
+
+  Scalar value(Scalar a, Scalar b) const override {
+    using std::min;
+    return min(a, b);
+  }
+
+  ExpressionType type() const override { return ExpressionType::NONLINEAR; }
+
+  std::string_view name() const override { return "min"; }
+
+  Scalar grad_l(Scalar a, Scalar b, Scalar parent_adjoint) const override {
+    if (a <= b) {
+      return parent_adjoint;
+    } else {
+      return Scalar(0);
+    }
+  }
+
+  Scalar grad_r([[maybe_unused]] Scalar a, [[maybe_unused]] Scalar b,
+                Scalar parent_adjoint) const override {
+    if (b < a) {
+      return parent_adjoint;
+    } else {
+      return Scalar(0);
+    }
+  }
+
+  ExpressionPtr<Scalar> grad_expr_l(
+      const ExpressionPtr<Scalar>& a, const ExpressionPtr<Scalar>& b,
+      const ExpressionPtr<Scalar>& parent_adjoint) const override {
+    if (a->val <= b->val) {
+      return parent_adjoint;
+    } else {
+      return constant_ptr(Scalar(0));
+    }
+  }
+
+  ExpressionPtr<Scalar> grad_expr_r(
+      const ExpressionPtr<Scalar>& a, const ExpressionPtr<Scalar>& b,
+      const ExpressionPtr<Scalar>& parent_adjoint) const override {
+    if (b->val < a->val) {
+      return parent_adjoint;
+    } else {
+      return constant_ptr(Scalar(0));
+    }
+  }
+};
+
+/// min() for Expressions.
+///
+/// @tparam Scalar Scalar type.
+/// @param a The a argument.
+/// @param b The b argument.
+template <typename Scalar>
+ExpressionPtr<Scalar> min(const ExpressionPtr<Scalar>& a,
+                          const ExpressionPtr<Scalar>& b) {
+  using enum ExpressionType;
+  using std::min;
+
+  // Evaluate constant
+  if (a->type() == CONSTANT && b->type() == CONSTANT) {
+    return constant_ptr(min(a->val, b->val));
+  }
+
+  return make_expression_ptr<MinExpression<Scalar>>(a, b);
+}
+
 template <typename Scalar>
 ExpressionPtr<Scalar> pow(const ExpressionPtr<Scalar>& base,
                          const ExpressionPtr<Scalar>& power);
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_graph.hpp
@@ -10,7 +10,7 @@

 namespace slp::detail {

-/// Generate a topological sort of an expression graph from parent to child.
+/// Generates a topological sort of an expression graph from parent to child.
 ///
 /// https://en.wikipedia.org/wiki/Topological_sorting
 ///
@@ -68,7 +68,7 @@ gch::small_vector<Expression<Scalar>*> topological_sort(
  return list;
 }

-/// Update the values of all nodes in this graph based on the values of
+/// Updates the values of all nodes in this graph based on the values of
 /// their dependent nodes.
 ///
 /// @tparam Scalar Scalar type.
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/expression_type.hpp
@@ -28,6 +28,8 @@ enum class ExpressionType : uint8_t {

 }  // namespace slp

+// @cond Suppress Doxygen
+
 /// Formatter for ExpressionType.
 template <>
 struct fmt::formatter<slp::ExpressionType> {
@@ -39,14 +41,15 @@ struct fmt::formatter<slp::ExpressionType> {
    return m_underlying.parse(ctx);
  }

-  /// Format ExpressionType.
+  /// Formats ExpressionType.
  ///
  /// @tparam FmtContext Format context type.
  /// @param type Expression type.
  /// @param ctx Format context.
  /// @return Format context iterator.
  template <typename FmtContext>
-  auto format(const slp::ExpressionType& type, FmtContext& ctx) const {
+  constexpr auto format(const slp::ExpressionType& type,
+                        FmtContext& ctx) const {
    using enum slp::ExpressionType;

    switch (type) {
@@ -68,3 +71,5 @@ struct fmt::formatter<slp::ExpressionType> {
 private:
  fmt::formatter<const char*> m_underlying;
 };
+
+// @endcond
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/gradient_expression_graph.hpp
@@ -34,7 +34,7 @@ class GradientExpressionGraph {
    }
  }

-  /// Update the values of all nodes in this graph based on the values of their
+  /// Updates the values of all nodes in this graph based on the values of their
  /// dependent nodes.
  void update_values() { detail::update_values(m_top_list); }

@@ -174,10 +174,10 @@ class GradientExpressionGraph {
  }

 private:
-  // Topological sort of graph from parent to child
+  /// Topological sort of graph from parent to child
  gch::small_vector<Expression<Scalar>*> m_top_list;

-  // List that maps nodes to their respective column
+  /// List that maps nodes to their respective column
  gch::small_vector<int> m_col_list;
 };

--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/hessian.hpp
@@ -40,8 +40,9 @@ class Hessian {
  /// @param wrt Vector of variables with respect to which to compute the
  ///     Hessian.
  Hessian(Variable<Scalar> variable, SleipnirMatrixLike<Scalar> auto wrt)
-      : m_variables{detail::GradientExpressionGraph<Scalar>{variable}
-                        .generate_tree(wrt)},
+      : m_variables{
+            detail::GradientExpressionGraph<Scalar>{variable}.generate_tree(
+                wrt)},
        m_wrt{wrt} {
    slp_assert(m_wrt.cols() == 1);

@@ -71,7 +72,7 @@ class Hessian {
        m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt);
      } else if (m_variables[row].type() > ExpressionType::LINEAR) {
        // If the row is quadratic or nonlinear, add it to the list of nonlinear
-        // rows to be recomputed in Value().
+        // rows to be recomputed in value().
        m_nonlinear_rows.emplace_back(row);
      }
    }
@@ -149,7 +150,7 @@ class Hessian {
  gch::small_vector<Eigen::Triplet<Scalar>> m_cached_triplets;

  // List of row indices for nonlinear rows whose graients will be computed in
-  // Value()
+  // value()
  gch::small_vector<int> m_nonlinear_rows;
 };

--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/jacobian.hpp
@@ -80,7 +80,7 @@ class Jacobian {
        m_graphs[row].append_triplets(m_cached_triplets, row, m_wrt);
      } else if (m_variables[row].type() > ExpressionType::LINEAR) {
        // If the row is quadratic or nonlinear, add it to the list of nonlinear
-        // rows to be recomputed in Value().
+        // rows to be recomputed in value().
        m_nonlinear_rows.emplace_back(row);
      }
    }
@@ -148,11 +148,11 @@ class Jacobian {

  Eigen::SparseMatrix<Scalar> m_J{m_variables.rows(), m_wrt.rows()};

-  // Cached triplets for gradients of linear rows
+  /// Cached triplets for gradients of linear rows
  gch::small_vector<Eigen::Triplet<Scalar>> m_cached_triplets;

-  // List of row indices for nonlinear rows whose graients will be computed in
-  // Value()
+  /// List of row indices for nonlinear rows whose graients will be computed in
+  /// value()
  gch::small_vector<int> m_nonlinear_rows;
 };

--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable.hpp
@@ -311,6 +311,24 @@ class Variable : public SleipnirBase {
  template <typename Scalar>
  friend Variable<Scalar> log10(const Variable<Scalar>& x);
  template <typename Scalar>
+  friend Variable<Scalar> max(const ScalarLike auto& a,
+                              const Variable<Scalar>& b);
+  template <typename Scalar>
+  friend Variable<Scalar> max(const Variable<Scalar>& a,
+                              const ScalarLike auto& b);
+  template <typename Scalar>
+  friend Variable<Scalar> max(const Variable<Scalar>& a,
+                              const Variable<Scalar>& b);
+  template <typename Scalar>
+  friend Variable<Scalar> min(const ScalarLike auto& a,
+                              const Variable<Scalar>& b);
+  template <typename Scalar>
+  friend Variable<Scalar> min(const Variable<Scalar>& a,
+                              const ScalarLike auto& b);
+  template <typename Scalar>
+  friend Variable<Scalar> min(const Variable<Scalar>& a,
+                              const Variable<Scalar>& b);
+  template <typename Scalar>
  friend Variable<Scalar> pow(const ScalarLike auto& base,
                              const Variable<Scalar>& power);
  template <typename Scalar>
@@ -513,6 +531,78 @@ Variable<Scalar> log10(const Variable<Scalar>& x) {
  return Variable{detail::log10(x.expr)};
 }

+/// max() for Variables.
+///
+/// Returns the greater of a and b. If the values are equivalent, returns a.
+///
+/// @tparam Scalar Scalar type.
+/// @param a The a argument.
+/// @param b The b argument.
+template <typename Scalar>
+Variable<Scalar> max(const ScalarLike auto& a, const Variable<Scalar>& b) {
+  return Variable{detail::max(Variable<Scalar>(a).expr, b.expr)};
+}
+
+/// max() for Variables.
+///
+/// Returns the greater of a and b. If the values are equivalent, returns a.
+///
+/// @tparam Scalar Scalar type.
+/// @param a The a argument.
+/// @param b The b argument.
+template <typename Scalar>
+Variable<Scalar> max(const Variable<Scalar>& a, const ScalarLike auto& b) {
+  return Variable{detail::max(a.expr, Variable<Scalar>(b).expr)};
+}
+
+/// max() for Variables.
+///
+/// Returns the greater of a and b. If the values are equivalent, returns a.
+///
+/// @tparam Scalar Scalar type.
+/// @param a The a argument.
+/// @param b The b argument.
+template <typename Scalar>
+Variable<Scalar> max(const Variable<Scalar>& a, const Variable<Scalar>& b) {
+  return Variable{detail::max(a.expr, b.expr)};
+}
+
+/// min() for Variables.
+///
+/// Returns the lesser of a and b. If the values are equivalent, returns a.
+///
+/// @tparam Scalar Scalar type.
+/// @param a The a argument.
+/// @param b The b argument.
+template <typename Scalar>
+Variable<Scalar> min(const ScalarLike auto& a, const Variable<Scalar>& b) {
+  return Variable{detail::min(Variable<Scalar>(a).expr, b.expr)};
+}
+
+/// min() for Variables.
+///
+/// Returns the lesser of a and b. If the values are equivalent, returns a.
+///
+/// @tparam Scalar Scalar type.
+/// @param a The a argument.
+/// @param b The b argument.
+template <typename Scalar>
+Variable<Scalar> min(const Variable<Scalar>& a, const ScalarLike auto& b) {
+  return Variable{detail::min(a.expr, Variable<Scalar>(b).expr)};
+}
+
+/// min() for Variables.
+///
+/// Returns the lesser of a and b. If the values are equivalent, returns a.
+///
+/// @tparam Scalar Scalar type.
+/// @param a The a argument.
+/// @param b The b argument.
+template <typename Scalar>
+Variable<Scalar> min(const Variable<Scalar>& a, const Variable<Scalar>& b) {
+  return Variable{detail::min(a.expr, b.expr)};
+}
+
 /// pow() for Variables.
 ///
 /// @tparam Scalar Scalar type.
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/autodiff/variable_matrix.hpp
@@ -1211,7 +1211,7 @@ class VariableMatrix : public SleipnirBase {
  /// @param rows The number of matrix rows.
  /// @param cols The number of matrix columns.
  /// @return A variable matrix filled with ones.
-  static VariableMatrix<Scalar> ones(int rows, int cols) {
+  static VariableMatrix<Scalar> one(int rows, int cols) {
    VariableMatrix<Scalar> result{detail::empty, rows, cols};

    for (auto& elem : result) {
@@ -1221,6 +1221,22 @@ class VariableMatrix : public SleipnirBase {
    return result;
  }

+  /// Returns a variable matrix filled with a constant.
+  ///
+  /// @param rows The number of matrix rows.
+  /// @param cols The number of matrix columns.
+  /// @param constant The constant.
+  /// @return A variable matrix filled with a constant.
+  static VariableMatrix<Scalar> constant(int rows, int cols, Scalar constant) {
+    VariableMatrix<Scalar> result{detail::empty, rows, cols};
+
+    for (auto& elem : result) {
+      elem = constant;
+    }
+
+    return result;
+  }
+
 private:
  gch::small_vector<Variable<Scalar>> m_storage;
  int m_rows = 0;
@@ -1260,7 +1276,7 @@ VariableMatrix<Scalar> cwise_reduce(
  return result;
 }

-/// Assemble a VariableMatrix from a nested list of blocks.
+/// Assembles a VariableMatrix from a nested list of blocks.
 ///
 /// Each row's blocks must have the same height, and the assembled block rows
 /// must have the same width. For example, for the block matrix [[A, B], [C]] to
@@ -1315,7 +1331,7 @@ VariableMatrix<Scalar> block(
  return result;
 }

-/// Assemble a VariableMatrix from a nested list of blocks.
+/// Assembles a VariableMatrix from a nested list of blocks.
 ///
 /// Each row's blocks must have the same height, and the assembled block rows
 /// must have the same width. For example, for the block matrix [[A, B], [C]] to
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/ocp.hpp
@@ -48,7 +48,7 @@ namespace slp {
 template <typename Scalar>
 class OCP : public Problem<Scalar> {
 public:
-  /// Build an optimization problem using a system evolution function (explicit
+  /// Builds an optimization problem using a system evolution function (explicit
  /// ODE or discrete state transition function).
  ///
  /// @param num_states The number of system states.
@@ -87,7 +87,7 @@ class OCP : public Problem<Scalar> {
            timestep_method,
            transcription_method} {}

-  /// Build an optimization problem using a system evolution function (explicit
+  /// Builds an optimization problem using a system evolution function (explicit
  /// ODE or discrete state transition function).
  ///
  /// @param num_states The number of system states.
@@ -156,7 +156,7 @@ class OCP : public Problem<Scalar> {
    }
  }

-  /// Utility function to constrain the initial state.
+  /// Constrains the initial state.
  ///
  /// @param initial_state the initial state to constrain to.
  template <typename T>
@@ -165,7 +165,7 @@ class OCP : public Problem<Scalar> {
    this->subject_to(this->initial_state() == initial_state);
  }

-  /// Utility function to constrain the final state.
+  /// Constrains the final state.
  ///
  /// @param final_state the final state to constrain to.
  template <typename T>
@@ -174,7 +174,7 @@ class OCP : public Problem<Scalar> {
    this->subject_to(this->final_state() == final_state);
  }

-  /// Set the constraint evaluation function. This function is called
+  /// Sets the constraint evaluation function. This function is called
  /// `num_steps+1` times, with the corresponding state and input
  /// VariableMatrices.
  ///
@@ -190,7 +190,7 @@ class OCP : public Problem<Scalar> {
    }
  }

-  /// Set the constraint evaluation function. This function is called
+  /// Sets the constraint evaluation function. This function is called
  /// `num_steps+1` times, with the corresponding state and input
  /// VariableMatrices.
  ///
@@ -213,7 +213,7 @@ class OCP : public Problem<Scalar> {
    }
  }

-  /// Convenience function to set a lower bound on the input.
+  /// Sets a lower bound on the input.
  ///
  /// @param lower_bound The lower bound that inputs must always be above. Must
  ///     be shaped (num_inputs)x1.
@@ -225,7 +225,7 @@ class OCP : public Problem<Scalar> {
    }
  }

-  /// Convenience function to set an upper bound on the input.
+  /// Sets an upper bound on the input.
  ///
  /// @param upper_bound The upper bound that inputs must always be below. Must
  ///     be shaped (num_inputs)x1.
@@ -237,21 +237,21 @@ class OCP : public Problem<Scalar> {
    }
  }

-  /// Convenience function to set a lower bound on the timestep.
+  /// Sets a lower bound on the timestep.
  ///
  /// @param min_timestep The minimum timestep.
  void set_min_timestep(std::chrono::duration<Scalar> min_timestep) {
    this->subject_to(dt() >= min_timestep.count());
  }

-  /// Convenience function to set an upper bound on the timestep.
+  /// Sets an upper bound on the timestep.
  ///
  /// @param max_timestep The maximum timestep.
  void set_max_timestep(std::chrono::duration<Scalar> max_timestep) {
    this->subject_to(dt() <= max_timestep.count());
  }

-  /// Get the state variables. After the problem is solved, this will contain
+  /// Gets the state variables. After the problem is solved, this will contain
  /// the optimized trajectory.
  ///
  /// Shaped (num_states)x(num_steps+1).
@@ -259,7 +259,7 @@ class OCP : public Problem<Scalar> {
  /// @return The state variable matrix.
  VariableMatrix<Scalar>& X() { return m_X; }

-  /// Get the input variables. After the problem is solved, this will contain
+  /// Gets the input variables. After the problem is solved, this will contain
  /// the inputs corresponding to the optimized trajectory.
  ///
  /// Shaped (num_inputs)x(num_steps+1), although the last input step is unused
@@ -268,8 +268,8 @@ class OCP : public Problem<Scalar> {
  /// @return The input variable matrix.
  VariableMatrix<Scalar>& U() { return m_U; }

-  /// Get the timestep variables. After the problem is solved, this will contain
-  /// the timesteps corresponding to the optimized trajectory.
+  /// Gets the timestep variables. After the problem is solved, this will
+  /// contain the timesteps corresponding to the optimized trajectory.
  ///
  /// Shaped 1x(num_steps+1), although the last timestep is unused in the
  /// trajectory.
@@ -277,12 +277,12 @@ class OCP : public Problem<Scalar> {
  /// @return The timestep variable matrix.
  VariableMatrix<Scalar>& dt() { return m_DT; }

-  /// Convenience function to get the initial state in the trajectory.
+  /// Gets the initial state in the trajectory.
  ///
  /// @return The initial state of the trajectory.
  VariableMatrix<Scalar> initial_state() { return m_X.col(0); }

-  /// Convenience function to get the final state in the trajectory.
+  /// Gets the final state in the trajectory.
  ///
  /// @return The final state of the trajectory.
  VariableMatrix<Scalar> final_state() { return m_X.col(m_num_steps); }
@@ -318,7 +318,7 @@ class OCP : public Problem<Scalar> {
    return x + (k1 + k2 * Scalar(2) + k3 * Scalar(2) + k4) * (dt / Scalar(6));
  }

-  /// Apply direct collocation dynamics constraints.
+  /// Applies direct collocation dynamics constraints.
  void constrain_direct_collocation() {
    slp_assert(m_dynamics_type == DynamicsType::EXPLICIT_ODE);

@@ -355,7 +355,7 @@ class OCP : public Problem<Scalar> {
    }
  }

-  /// Apply direct transcription dynamics constraints.
+  /// Applies direct transcription dynamics constraints.
  void constrain_direct_transcription() {
    Variable<Scalar> time{0};

@@ -378,7 +378,7 @@ class OCP : public Problem<Scalar> {
    }
  }

-  /// Apply single shooting dynamics constraints.
+  /// Applies single shooting dynamics constraints.
  void constrain_single_shooting() {
    Variable<Scalar> time{0};

--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp
@@ -34,9 +34,10 @@
 #include "sleipnir/util/empty.hpp"
 #include "sleipnir/util/print.hpp"
 #include "sleipnir/util/print_diagnostics.hpp"
-#include "sleipnir/util/setup_profiler.hpp"
+#include "sleipnir/util/profiler.hpp"
 #include "sleipnir/util/spy.hpp"
 #include "sleipnir/util/symbol_exports.hpp"
+#include "sleipnir/util/to_underlying.hpp"

 namespace slp {

@@ -66,10 +67,12 @@ namespace slp {
 template <typename Scalar>
 class Problem {
 public:
-  /// Construct the optimization problem.
+  /// Constructs the optimization problem.
  Problem() noexcept = default;

-  /// Create a decision variable in the optimization problem.
+  /// Creates a decision variable in the optimization problem.
+  ///
+  /// Decision variables have an initial value of zero.
  ///
  /// @return A decision variable in the optimization problem.
  [[nodiscard]]
@@ -78,7 +81,9 @@ class Problem {
    return m_decision_variables.back();
  }

-  /// Create a matrix of decision variables in the optimization problem.
+  /// Creates a matrix of decision variables in the optimization problem.
+  ///
+  /// Decision variables have an initial value of zero.
  ///
  /// @param rows Number of matrix rows.
  /// @param cols Number of matrix columns.
@@ -99,12 +104,14 @@ class Problem {
    return vars;
  }

-  /// Create a symmetric matrix of decision variables in the optimization
+  /// Creates a symmetric matrix of decision variables in the optimization
  /// problem.
  ///
  /// Variable instances are reused across the diagonal, which helps reduce
  /// problem dimensionality.
  ///
+  /// Decision variables have an initial value of zero.
+  ///
  /// @param rows Number of matrix rows.
  /// @return A symmetric matrix of decision varaibles in the optimization
  ///     problem.
@@ -139,7 +146,9 @@ class Problem {
  /// will find the closest solution to the initial conditions that's in the
  /// feasible set.
  ///
-  /// @param cost The cost function to minimize.
+  /// @param cost The cost function to minimize. A 1x1 VariableMatrix will
+  ///     implicitly convert to a Variable, and a non-1x1 VariableMatrix will
+  ///     raise an assertion.
  void minimize(const Variable<Scalar>& cost) { m_f = cost; }

  /// Tells the solver to minimize the output of the given cost function.
@@ -148,7 +157,9 @@ class Problem {
  /// will find the closest solution to the initial conditions that's in the
  /// feasible set.
  ///
-  /// @param cost The cost function to minimize.
+  /// @param cost The cost function to minimize. A 1x1 VariableMatrix will
+  ///     implicitly convert to a Variable, and a non-1x1 VariableMatrix will
+  ///     raise an assertion.
  void minimize(Variable<Scalar>&& cost) { m_f = std::move(cost); }

  /// Tells the solver to maximize the output of the given objective function.
@@ -157,7 +168,9 @@ class Problem {
  /// will find the closest solution to the initial conditions that's in the
  /// feasible set.
  ///
-  /// @param objective The objective function to maximize.
+  /// @param objective The objective function to maximize. A 1x1 VariableMatrix
+  ///     will implicitly convert to a Variable, and a non-1x1 VariableMatrix
+  ///     will raise an assertion.
  void maximize(const Variable<Scalar>& objective) {
    // Maximizing a cost function is the same as minimizing its negative
    m_f = -objective;
@@ -169,7 +182,9 @@ class Problem {
  /// will find the closest solution to the initial conditions that's in the
  /// feasible set.
  ///
-  /// @param objective The objective function to maximize.
+  /// @param objective The objective function to maximize. A 1x1 VariableMatrix
+  ///     will implicitly convert to a Variable, and a non-1x1 VariableMatrix
+  ///     will raise an assertion.
  void maximize(Variable<Scalar>&& objective) {
    // Maximizing a cost function is the same as minimizing its negative
    m_f = -std::move(objective);
@@ -256,7 +271,7 @@ class Problem {
    }
  }

-  /// Solve the optimization problem. The solution will be stored in the
+  /// Solves the optimization problem. The solution will be stored in the
  /// original variables used to construct the problem.
  ///
  /// @param options Solver options.
@@ -311,20 +326,17 @@ class Problem {
    Gradient g{f, x_ad};
    ad_setup_profilers.back().stop();

-    [[maybe_unused]]
    int num_decision_variables = m_decision_variables.size();
-    [[maybe_unused]]
    int num_equality_constraints = m_equality_constraints.size();
-    [[maybe_unused]]
    int num_inequality_constraints = m_inequality_constraints.size();

    gch::small_vector<std::function<bool(const IterationInfo<Scalar>& info)>>
-        callbacks;
+        iteration_callbacks;
    for (const auto& callback : m_iteration_callbacks) {
-      callbacks.emplace_back(callback);
+      iteration_callbacks.emplace_back(callback);
    }
    for (const auto& callback : m_persistent_iteration_callbacks) {
-      callbacks.emplace_back(callback);
+      iteration_callbacks.emplace_back(callback);
    }

    // Solve the optimization problem
@@ -349,28 +361,32 @@ class Problem {
        H_spy = std::make_unique<Spy<Scalar>>(
            "H.spy", "Hessian", "Decision variables", "Decision variables",
            num_decision_variables, num_decision_variables);
-        callbacks.push_back([&](const IterationInfo<Scalar>& info) -> bool {
-          H_spy->add(info.H);
-          return false;
-        });
+        iteration_callbacks.push_back(
+            [&](const IterationInfo<Scalar>& info) -> bool {
+              H_spy->add(info.H);
+              return false;
+            });
      }
 #endif

+      NewtonMatrixCallbacks<Scalar> matrix_callbacks{
+          num_decision_variables,
+          [&](const DenseVector& x) -> Scalar {
+            x_ad.set_value(x);
+            return f.value();
+          },
+          [&](const DenseVector& x) -> SparseVector {
+            x_ad.set_value(x);
+            return g.value();
+          },
+          [&](const DenseVector& x) -> SparseMatrix {
+            x_ad.set_value(x);
+            return H.value();
+          }};
+
      // Invoke Newton solver
-      status = newton<Scalar>(NewtonMatrixCallbacks<Scalar>{
-                                  [&](const DenseVector& x) -> Scalar {
-                                    x_ad.set_value(x);
-                                    return f.value();
-                                  },
-                                  [&](const DenseVector& x) -> SparseVector {
-                                    x_ad.set_value(x);
-                                    return g.value();
-                                  },
-                                  [&](const DenseVector& x) -> SparseMatrix {
-                                    x_ad.set_value(x);
-                                    return H.value();
-                                  }},
-                              callbacks, options, x);
+      status =
+          newton<Scalar>(matrix_callbacks, iteration_callbacks, options, x);
    } else if (m_inequality_constraints.empty()) {
      if (options.diagnostics) {
        slp::println("\nInvoking SQP solver\n");
@@ -390,6 +406,7 @@ class Problem {
      // Set up Lagrangian Hessian autodiff
      ad_setup_profilers.emplace_back("  ↳ ∇²ₓₓL").start();
      Hessian<Scalar, Eigen::Lower> H{L, x_ad};
+      Hessian<Scalar, Eigen::Lower> H_c{-y_ad.T() * c_e_ad, x_ad};
      ad_setup_profilers.back().stop();

      ad_setup_profilers[0].stop();
@@ -407,39 +424,47 @@ class Problem {
            "A_e.spy", "Equality constraint Jacobian", "Constraints",
            "Decision variables", num_equality_constraints,
            num_decision_variables);
-        callbacks.push_back([&](const IterationInfo<Scalar>& info) -> bool {
-          H_spy->add(info.H);
-          A_e_spy->add(info.A_e);
-          return false;
-        });
+        iteration_callbacks.push_back(
+            [&](const IterationInfo<Scalar>& info) -> bool {
+              H_spy->add(info.H);
+              A_e_spy->add(info.A_e);
+              return false;
+            });
      }
 #endif

+      SQPMatrixCallbacks<Scalar> matrix_callbacks{
+          num_decision_variables,
+          num_equality_constraints,
+          [&](const DenseVector& x) -> Scalar {
+            x_ad.set_value(x);
+            return f.value();
+          },
+          [&](const DenseVector& x) -> SparseVector {
+            x_ad.set_value(x);
+            return g.value();
+          },
+          [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
+            x_ad.set_value(x);
+            y_ad.set_value(y);
+            return H.value();
+          },
+          [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
+            x_ad.set_value(x);
+            y_ad.set_value(y);
+            return H_c.value();
+          },
+          [&](const DenseVector& x) -> DenseVector {
+            x_ad.set_value(x);
+            return c_e_ad.value();
+          },
+          [&](const DenseVector& x) -> SparseMatrix {
+            x_ad.set_value(x);
+            return A_e.value();
+          }};
+
      // Invoke SQP solver
-      status = sqp<Scalar>(
-          SQPMatrixCallbacks<Scalar>{
-              [&](const DenseVector& x) -> Scalar {
-                x_ad.set_value(x);
-                return f.value();
-              },
-              [&](const DenseVector& x) -> SparseVector {
-                x_ad.set_value(x);
-                return g.value();
-              },
-              [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
-                x_ad.set_value(x);
-                y_ad.set_value(y);
-                return H.value();
-              },
-              [&](const DenseVector& x) -> DenseVector {
-                x_ad.set_value(x);
-                return c_e_ad.value();
-              },
-              [&](const DenseVector& x) -> SparseMatrix {
-                x_ad.set_value(x);
-                return A_e.value();
-              }},
-          callbacks, options, x);
+      status = sqp<Scalar>(matrix_callbacks, iteration_callbacks, options, x);
    } else {
      if (options.diagnostics) {
        slp::println("\nInvoking IPM solver...\n");
@@ -466,6 +491,8 @@ class Problem {
      // Set up Lagrangian Hessian autodiff
      ad_setup_profilers.emplace_back("  ↳ ∇²ₓₓL").start();
      Hessian<Scalar, Eigen::Lower> H{L, x_ad};
+      Hessian<Scalar, Eigen::Lower> H_c{-y_ad.T() * c_e_ad - z_ad.T() * c_i_ad,
+                                        x_ad};
      ad_setup_profilers.back().stop();

      ad_setup_profilers[0].stop();
@@ -488,12 +515,13 @@ class Problem {
            "A_i.spy", "Inequality constraint Jacobian", "Constraints",
            "Decision variables", num_inequality_constraints,
            num_decision_variables);
-        callbacks.push_back([&](const IterationInfo<Scalar>& info) -> bool {
-          H_spy->add(info.H);
-          A_e_spy->add(info.A_e);
-          A_i_spy->add(info.A_i);
-          return false;
-        });
+        iteration_callbacks.push_back(
+            [&](const IterationInfo<Scalar>& info) -> bool {
+              H_spy->add(info.H);
+              A_e_spy->add(info.A_e);
+              A_i_spy->add(info.A_i);
+              return false;
+            });
      }
 #endif

@@ -511,45 +539,57 @@ class Problem {
 #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
      project_onto_bounds(x, bounds);
 #endif
+
+      InteriorPointMatrixCallbacks<Scalar> matrix_callbacks{
+          num_decision_variables,
+          num_equality_constraints,
+          num_inequality_constraints,
+          [&](const DenseVector& x) -> Scalar {
+            x_ad.set_value(x);
+            return f.value();
+          },
+          [&](const DenseVector& x) -> SparseVector {
+            x_ad.set_value(x);
+            return g.value();
+          },
+          [&](const DenseVector& x, const DenseVector& y,
+              const DenseVector& z) -> SparseMatrix {
+            x_ad.set_value(x);
+            y_ad.set_value(y);
+            z_ad.set_value(z);
+            return H.value();
+          },
+          [&](const DenseVector& x, const DenseVector& y,
+              const DenseVector& z) -> SparseMatrix {
+            x_ad.set_value(x);
+            y_ad.set_value(y);
+            z_ad.set_value(z);
+            return H_c.value();
+          },
+          [&](const DenseVector& x) -> DenseVector {
+            x_ad.set_value(x);
+            return c_e_ad.value();
+          },
+          [&](const DenseVector& x) -> SparseMatrix {
+            x_ad.set_value(x);
+            return A_e.value();
+          },
+          [&](const DenseVector& x) -> DenseVector {
+            x_ad.set_value(x);
+            return c_i_ad.value();
+          },
+          [&](const DenseVector& x) -> SparseMatrix {
+            x_ad.set_value(x);
+            return A_i.value();
+          }};
+
      // Invoke interior-point method solver
-      status = interior_point<Scalar>(
-          InteriorPointMatrixCallbacks<Scalar>{
-              [&](const DenseVector& x) -> Scalar {
-                x_ad.set_value(x);
-                return f.value();
-              },
-              [&](const DenseVector& x) -> SparseVector {
-                x_ad.set_value(x);
-                return g.value();
-              },
-              [&](const DenseVector& x, const DenseVector& y,
-                  const DenseVector& z) -> SparseMatrix {
-                x_ad.set_value(x);
-                y_ad.set_value(y);
-                z_ad.set_value(z);
-                return H.value();
-              },
-              [&](const DenseVector& x) -> DenseVector {
-                x_ad.set_value(x);
-                return c_e_ad.value();
-              },
-              [&](const DenseVector& x) -> SparseMatrix {
-                x_ad.set_value(x);
-                return A_e.value();
-              },
-              [&](const DenseVector& x) -> DenseVector {
-                x_ad.set_value(x);
-                return c_i_ad.value();
-              },
-              [&](const DenseVector& x) -> SparseMatrix {
-                x_ad.set_value(x);
-                return A_i.value();
-              }},
-          callbacks, options,
+      status =
+          interior_point<Scalar>(matrix_callbacks, iteration_callbacks, options,
 #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
-          bound_constraint_mask,
+                                 bound_constraint_mask,
 #endif
-          x);
+                                 x);
    }

    if (options.diagnostics) {
@@ -657,11 +697,11 @@ class Problem {
    // Print problem structure
    slp::println("\nProblem structure:");
    slp::println("  ↳ {} cost function",
-                 types[static_cast<uint8_t>(cost_function_type())]);
+                 types[slp::to_underlying(cost_function_type())]);
    slp::println("  ↳ {} equality constraints",
-                 types[static_cast<uint8_t>(equality_constraint_type())]);
+                 types[slp::to_underlying(equality_constraint_type())]);
    slp::println("  ↳ {} inequality constraints",
-                 types[static_cast<uint8_t>(inequality_constraint_type())]);
+                 types[slp::to_underlying(inequality_constraint_type())]);

    if (m_decision_variables.size() == 1) {
      slp::print("\n1 decision variable\n");
@@ -673,7 +713,7 @@ class Problem {
        [](const gch::small_vector<Variable<Scalar>>& constraints) {
          std::array<size_t, 5> counts{};
          for (const auto& constraint : constraints) {
-            ++counts[static_cast<uint8_t>(constraint.type())];
+            ++counts[slp::to_underlying(constraint.type())];
          }
          for (size_t i = 0; i < counts.size(); ++i) {
            constexpr std::array names{"empty", "constant", "linear",
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/exit_status.hpp
@@ -25,11 +25,12 @@ enum class ExitStatus : int8_t {
  GLOBALLY_INFEASIBLE = -3,
  /// The linear system factorization failed.
  FACTORIZATION_FAILED = -4,
-  /// The backtracking line search failed, and the problem isn't locally
-  /// infeasible.
-  LINE_SEARCH_FAILED = -5,
-  /// The solver encountered nonfinite initial cost or constraints and gave up.
-  NONFINITE_INITIAL_COST_OR_CONSTRAINTS = -6,
+  /// The solver failed to reach the desired tolerance, and feasibility
+  /// restoration failed to converge.
+  FEASIBILITY_RESTORATION_FAILED = -5,
+  /// The solver encountered nonfinite initial cost, constraints, or derivatives
+  /// and gave up.
+  NONFINITE_INITIAL_GUESS = -6,
  /// The solver encountered diverging primal iterates xₖ and/or sₖ and gave up.
  DIVERGING_ITERATES = -7,
  /// The solver returned its solution so far after exceeding the maximum number
@@ -42,10 +43,12 @@ enum class ExitStatus : int8_t {

 }  // namespace slp

+// @cond Suppress Doxygen
+
 /// Formatter for ExitStatus.
 template <>
 struct fmt::formatter<slp::ExitStatus> {
-  /// Parse format string.
+  /// Parses format string.
  ///
  /// @param ctx Format parse context.
  /// @return Format parse context iterator.
@@ -53,14 +56,15 @@ struct fmt::formatter<slp::ExitStatus> {
    return m_underlying.parse(ctx);
  }

-  /// Format ExitStatus.
+  /// Formats ExitStatus.
  ///
  /// @tparam FmtContext Format context type.
  /// @param exit_status Exit status.
  /// @param ctx Format context.
  /// @return Format context iterator.
  template <typename FmtContext>
-  auto format(const slp::ExitStatus& exit_status, FmtContext& ctx) const {
+  constexpr auto format(const slp::ExitStatus& exit_status,
+                        FmtContext& ctx) const {
    using enum slp::ExitStatus;

    switch (exit_status) {
@@ -76,11 +80,11 @@ struct fmt::formatter<slp::ExitStatus> {
        return m_underlying.format("globally infeasible", ctx);
      case FACTORIZATION_FAILED:
        return m_underlying.format("factorization failed", ctx);
-      case LINE_SEARCH_FAILED:
-        return m_underlying.format("line search failed", ctx);
-      case NONFINITE_INITIAL_COST_OR_CONSTRAINTS:
-        return m_underlying.format("nonfinite initial cost or constraints",
-                                   ctx);
+      case FEASIBILITY_RESTORATION_FAILED:
+        return m_underlying.format("feasibility restoration failed", ctx);
+      case NONFINITE_INITIAL_GUESS:
+        return m_underlying.format(
+            "nonfinite initial cost, constraints, or derivatives", ctx);
      case DIVERGING_ITERATES:
        return m_underlying.format("diverging iterates", ctx);
      case MAX_ITERATIONS_EXCEEDED:
@@ -95,3 +99,5 @@ struct fmt::formatter<slp::ExitStatus> {
 private:
  fmt::formatter<const char*> m_underlying;
 };
+
+// @endcond
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp
@@ -17,7 +17,9 @@
 #include "sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp"
 #include "sleipnir/optimization/solver/iteration_info.hpp"
 #include "sleipnir/optimization/solver/options.hpp"
-#include "sleipnir/optimization/solver/util/error_estimate.hpp"
+#include "sleipnir/optimization/solver/util/all_finite.hpp"
+#include "sleipnir/optimization/solver/util/append_as_triplets.hpp"
+#include "sleipnir/optimization/solver/util/feasibility_restoration.hpp"
 #include "sleipnir/optimization/solver/util/filter.hpp"
 #include "sleipnir/optimization/solver/util/fraction_to_the_boundary_rule.hpp"
 #include "sleipnir/optimization/solver/util/is_locally_infeasible.hpp"
@@ -25,9 +27,8 @@
 #include "sleipnir/optimization/solver/util/regularized_ldlt.hpp"
 #include "sleipnir/util/assert.hpp"
 #include "sleipnir/util/print_diagnostics.hpp"
+#include "sleipnir/util/profiler.hpp"
 #include "sleipnir/util/scope_exit.hpp"
-#include "sleipnir/util/scoped_profiler.hpp"
-#include "sleipnir/util/solve_profiler.hpp"
 #include "sleipnir/util/symbol_exports.hpp"

 // See docs/algorithms.md#Works_cited for citation definitions.
@@ -70,17 +71,78 @@ ExitStatus interior_point(
 #endif
    Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  DenseVector s =
+      DenseVector::Ones(matrix_callbacks.num_inequality_constraints);
+  DenseVector y = DenseVector::Zero(matrix_callbacks.num_equality_constraints);
+  DenseVector z =
+      DenseVector::Ones(matrix_callbacks.num_inequality_constraints);
+  Scalar μ(0.1);
+
+  return interior_point(matrix_callbacks, iteration_callbacks, options, false,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+                        bound_constraint_mask,
+#endif
+                        x, s, y, z, μ);
+}
+
+/// Finds the optimal solution to a nonlinear program using the interior-point
+/// method.
+///
+/// A nonlinear program has the form:
+///
+/// ```
+///      min_x f(x)
+/// subject to cₑ(x) = 0
+///            cᵢ(x) ≥ 0
+/// ```
+///
+/// where f(x) is the cost function, cₑ(x) are the equality constraints, and
+/// cᵢ(x) are the inequality constraints.
+///
+/// @tparam Scalar Scalar type.
+/// @param[in] matrix_callbacks Matrix callbacks.
+/// @param[in] iteration_callbacks The list of callbacks to call at the
+///     beginning of each iteration.
+/// @param[in] options Solver options.
+/// @param[in] in_feasibility_restoration Whether solver is in feasibility
+///     restoration mode.
+/// @param[in,out] x The initial guess and output location for the decision
+///     variables.
+/// @param[in,out] s The initial guess and output location for the inequality
+///     constraint slack variables.
+/// @param[in,out] y The initial guess and output location for the equality
+///     constraint dual variables.
+/// @param[in,out] z The initial guess and output location for the inequality
+///     constraint dual variables.
+/// @param[in,out] μ The initial guess and output location for the barrier
+///     parameter.
+/// @return The exit status.
+template <typename Scalar>
+ExitStatus interior_point(
+    const InteriorPointMatrixCallbacks<Scalar>& matrix_callbacks,
+    std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
+        iteration_callbacks,
+    const Options& options, bool in_feasibility_restoration,
+#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& μ) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
  using SparseVector = Eigen::SparseVector<Scalar>;

  /// Interior-point method step direction.
  struct Step {
-    /// Primal step.
+    /// Decision variable primal step.
    DenseVector p_x;
+    /// Inequality constraint slack variable primal step.
+    DenseVector p_s;
    /// Equality constraint dual step.
    DenseVector p_y;
-    /// Inequality constraint slack variable step.
-    DenseVector p_s;
    /// Inequality constraint dual step.
    DenseVector p_z;
  };
@@ -104,6 +166,7 @@ ExitStatus interior_point(
  solve_profilers.emplace_back("    ↳ f(x)");
  solve_profilers.emplace_back("    ↳ ∇f(x)");
  solve_profilers.emplace_back("    ↳ ∇²ₓₓL");
+  solve_profilers.emplace_back("    ↳ ∇²ₓₓ(yᵀcₑ + zᵀcᵢ)");
  solve_profilers.emplace_back("    ↳ cₑ(x)");
  solve_profilers.emplace_back("    ↳ ∂cₑ/∂x");
  solve_profilers.emplace_back("    ↳ cᵢ(x)");
@@ -126,12 +189,16 @@ ExitStatus interior_point(
  auto& f_prof = solve_profilers[11];
  auto& g_prof = solve_profilers[12];
  auto& H_prof = solve_profilers[13];
-  auto& c_e_prof = solve_profilers[14];
-  auto& A_e_prof = solve_profilers[15];
-  auto& c_i_prof = solve_profilers[16];
-  auto& A_i_prof = solve_profilers[17];
+  auto& H_c_prof = solve_profilers[14];
+  auto& c_e_prof = solve_profilers[15];
+  auto& A_e_prof = solve_profilers[16];
+  auto& c_i_prof = solve_profilers[17];
+  auto& A_i_prof = solve_profilers[18];

  InteriorPointMatrixCallbacks<Scalar> matrices{
+      matrix_callbacks.num_decision_variables,
+      matrix_callbacks.num_equality_constraints,
+      matrix_callbacks.num_inequality_constraints,
      [&](const DenseVector& x) -> Scalar {
        ScopedProfiler prof{f_prof};
        return matrix_callbacks.f(x);
@@ -145,6 +212,11 @@ ExitStatus interior_point(
        ScopedProfiler prof{H_prof};
        return matrix_callbacks.H(x, y, z);
      },
+      [&](const DenseVector& x, const DenseVector& y,
+          const DenseVector& z) -> SparseMatrix {
+        ScopedProfiler prof{H_c_prof};
+        return matrix_callbacks.H_c(x, y, z);
+      },
      [&](const DenseVector& x) -> DenseVector {
        ScopedProfiler prof{c_e_prof};
        return matrix_callbacks.c_e(x);
@@ -169,15 +241,26 @@ ExitStatus interior_point(
  setup_prof.start();

  Scalar f = matrices.f(x);
+  SparseVector g = matrices.g(x);
+  SparseMatrix H = matrices.H(x, y, z);
  DenseVector c_e = matrices.c_e(x);
+  SparseMatrix A_e = matrices.A_e(x);
  DenseVector c_i = matrices.c_i(x);
+  SparseMatrix A_i = matrices.A_i(x);

-  int num_decision_variables = x.rows();
-  int num_equality_constraints = c_e.rows();
-  int num_inequality_constraints = c_i.rows();
+  // Ensure matrix callback dimensions are consistent
+  slp_assert(g.rows() == matrices.num_decision_variables);
+  slp_assert(H.rows() == matrices.num_decision_variables);
+  slp_assert(H.cols() == matrices.num_decision_variables);
+  slp_assert(c_e.rows() == matrices.num_equality_constraints);
+  slp_assert(A_e.rows() == matrices.num_equality_constraints);
+  slp_assert(A_e.cols() == matrices.num_decision_variables);
+  slp_assert(c_i.rows() == matrices.num_inequality_constraints);
+  slp_assert(A_i.rows() == matrices.num_inequality_constraints);
+  slp_assert(A_i.cols() == matrices.num_decision_variables);

  // Check for overconstrained problem
-  if (num_equality_constraints > num_decision_variables) {
+  if (matrices.num_equality_constraints > matrices.num_decision_variables) {
    if (options.diagnostics) {
      print_too_few_dofs_error(c_e);
    }
@@ -185,52 +268,33 @@ ExitStatus interior_point(
    return ExitStatus::TOO_FEW_DOFS;
  }

-  SparseVector g = matrices.g(x);
-  SparseMatrix A_e = matrices.A_e(x);
-  SparseMatrix A_i = matrices.A_i(x);
+  // Check whether initial guess has finite cost, constraints, and derivatives
+  if (!isfinite(f) || !all_finite(g) || !all_finite(H) || !c_e.allFinite() ||
+      !all_finite(A_e) || !c_i.allFinite() || !all_finite(A_i)) {
+    return ExitStatus::NONFINITE_INITIAL_GUESS;
+  }

-  DenseVector s = DenseVector::Ones(num_inequality_constraints);
 #ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
  // We set sʲ = cᵢʲ(x) for each bound inequality constraint index j
  s = bound_constraint_mask.select(c_i, s);
 #endif
-  DenseVector y = DenseVector::Zero(num_equality_constraints);
-  DenseVector z = DenseVector::Ones(num_inequality_constraints);
-
-  SparseMatrix H = matrices.H(x, y, z);
-
-  // Ensure matrix callback dimensions are consistent
-  slp_assert(g.rows() == num_decision_variables);
-  slp_assert(A_e.rows() == num_equality_constraints);
-  slp_assert(A_e.cols() == num_decision_variables);
-  slp_assert(A_i.rows() == num_inequality_constraints);
-  slp_assert(A_i.cols() == num_decision_variables);
-  slp_assert(H.rows() == num_decision_variables);
-  slp_assert(H.cols() == num_decision_variables);
-
-  // Check whether initial guess has finite f(xₖ), cₑ(xₖ), and cᵢ(xₖ)
-  if (!isfinite(f) || !c_e.allFinite() || !c_i.allFinite()) {
-    return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
-  }

  int iterations = 0;

  // Barrier parameter minimum
  const Scalar μ_min = Scalar(options.tolerance) / Scalar(10);

-  // Barrier parameter μ
-  Scalar μ(0.1);
-
  // Fraction-to-the-boundary rule scale factor minimum
  constexpr Scalar τ_min(0.99);

  // Fraction-to-the-boundary rule scale factor τ
  Scalar τ = τ_min;

-  Filter<Scalar> filter;
+  Filter<Scalar> filter{c_e.template lpNorm<1>() +
+                        (c_i - s).template lpNorm<1>()};

-  // This should be run when the error estimate is below a desired threshold for
-  // the current barrier parameter
+  // This should be run when the error is below a desired threshold for the
+  // current barrier parameter
  auto update_barrier_parameter_and_reset_filter = [&] {
    // Barrier parameter linear decrease power in "κ_μ μ". Range of (0, 1).
    constexpr Scalar κ_μ(0.2);
@@ -261,8 +325,11 @@ ExitStatus interior_point(
  // Kept outside the loop so its storage can be reused
  gch::small_vector<Eigen::Triplet<Scalar>> triplets;

-  RegularizedLDLT<Scalar> solver{num_decision_variables,
-                                 num_equality_constraints};
+  // Constraint regularization is forced to zero in feasibility restoration
+  // because the equality constraint Jacobian cannot be rank-deficient
+  RegularizedLDLT<Scalar> solver{
+      matrices.num_decision_variables, matrices.num_equality_constraints,
+      in_feasibility_restoration ? Scalar(0) : Scalar(1e-10)};

  // Variables for determining when a step is acceptable
  constexpr Scalar α_reduction_factor(0.5);
@@ -270,7 +337,7 @@ ExitStatus interior_point(

  int full_step_rejected_counter = 0;

-  // Error estimate
+  // Error
  Scalar E_0 = std::numeric_limits<Scalar>::infinity();

  setup_prof.stop();
@@ -279,6 +346,11 @@ ExitStatus interior_point(
  scope_exit exit{[&] {
    if (options.diagnostics) {
      solver_prof.stop();
+
+      if (in_feasibility_restoration) {
+        return;
+      }
+
      if (iterations > 0) {
        print_bottom_iteration_diagnostics();
      }
@@ -319,7 +391,7 @@ ExitStatus interior_point(

    // Call iteration callbacks
    for (const auto& callback : iteration_callbacks) {
-      if (callback({iterations, x, g, H, A_e, A_i})) {
+      if (callback({iterations, x, s, y, z, g, H, A_e, A_i})) {
        return ExitStatus::CALLBACK_REQUESTED_STOP;
      }
    }
@@ -340,18 +412,10 @@ ExitStatus interior_point(
        H + (A_i.transpose() * Σ * A_i).template triangularView<Eigen::Lower>();
    triplets.clear();
    triplets.reserve(top_left.nonZeros() + A_e.nonZeros());
-    for (int col = 0; col < H.cols(); ++col) {
-      // Append column of H + AᵢᵀΣAᵢ lower triangle in top-left quadrant
-      for (typename SparseMatrix::InnerIterator it{top_left, col}; it; ++it) {
-        triplets.emplace_back(it.row(), it.col(), it.value());
-      }
-      // Append column of Aₑ in bottom-left quadrant
-      for (typename SparseMatrix::InnerIterator it{A_e, col}; it; ++it) {
-        triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
-      }
-    }
-    SparseMatrix lhs(num_decision_variables + num_equality_constraints,
-                     num_decision_variables + num_equality_constraints);
+    append_as_triplets(triplets, 0, 0, {top_left, A_e});
+    SparseMatrix lhs(
+        matrices.num_decision_variables + matrices.num_equality_constraints,
+        matrices.num_decision_variables + matrices.num_equality_constraints);
    lhs.setFromSortedTriplets(triplets.begin(), triplets.end());

    // rhs = −[∇f − Aₑᵀy − Aᵢᵀ(−Σcᵢ + μS⁻¹e + z)]
@@ -369,6 +433,7 @@ ExitStatus interior_point(
    Scalar α_max(1);
    Scalar α(1);
    Scalar α_z(1);
+    bool call_feasibility_restoration = false;

    // Solve the Newton-KKT system
    //
@@ -402,14 +467,16 @@ ExitStatus interior_point(
    α_max = fraction_to_the_boundary_rule<Scalar>(s, step.p_s, τ);
    α = α_max;

-    // If maximum step size is below minimum, report line search failure
+    // If maximum step size is below minimum, invoke feasibility restoration
    if (α < α_min) {
-      return ExitStatus::LINE_SEARCH_FAILED;
+      call_feasibility_restoration = true;
    }

    // αₖᶻ = max(α ∈ (0, 1] : zₖ + αpₖᶻ ≥ (1−τⱼ)zₖ)
    α_z = fraction_to_the_boundary_rule<Scalar>(z, step.p_z, τ);

+    const FilterEntry<Scalar> current_entry{f, s, c_e, c_i, μ};
+
    // Loop until a step is accepted
    while (1) {
      DenseVector trial_x = x + α * step.p_x;
@@ -428,7 +495,8 @@ ExitStatus interior_point(
        α *= α_reduction_factor;

        if (α < α_min) {
-          return ExitStatus::LINE_SEARCH_FAILED;
+          call_feasibility_restoration = true;
+          break;
        }
        continue;
      }
@@ -445,8 +513,8 @@ ExitStatus interior_point(
      }

      // Check whether filter accepts trial iterate
-      if (filter.try_add(FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ},
-                         α)) {
+      FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
+      if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
        // Accept step
        break;
      }
@@ -484,7 +552,10 @@ ExitStatus interior_point(
                  step_acceptable ? IterationType::ACCEPTED_SOC
                                  : IterationType::REJECTED_SOC,
                  soc_profiler.current_duration(),
-                  error_estimate<Scalar>(g, A_e, trial_c_e, trial_y), trial_f,
+                  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)),
+                  trial_f,
                  trial_c_e.template lpNorm<1>() +
                      (trial_c_i - trial_s).template lpNorm<1>(),
                  trial_s.dot(trial_z), μ, solver.hessian_regularization(),
@@ -531,8 +602,8 @@ ExitStatus interior_point(
          }

          // Check whether filter accepts trial iterate
-          if (filter.try_add(
-                  FilterEntry{trial_f, trial_s, trial_c_e, trial_c_i, μ}, α)) {
+          FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
+          if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
            step = soc_step;
            α = α_soc;
            α_z = α_z_soc;
@@ -559,7 +630,8 @@ ExitStatus interior_point(
      // See section 3.2 case I of [2].
      if (full_step_rejected_counter >= 4 &&
          filter.max_constraint_violation >
-              filter.back().constraint_violation / Scalar(10)) {
+              current_entry.constraint_violation / Scalar(10) &&
+          filter.last_rejection_due_to_filter()) {
        filter.max_constraint_violation *= Scalar(0.1);
        filter.reset();
        continue;
@@ -569,10 +641,10 @@ ExitStatus interior_point(
      α *= α_reduction_factor;

      // If step size hit a minimum, check if the KKT error was reduced. If it
-      // wasn't, report line search failure.
+      // wasn't, invoke feasibility restoration.
      if (α < α_min) {
-        Scalar current_kkt_error =
-            kkt_error<Scalar>(g, A_e, c_e, A_i, c_i, s, y, z, μ);
+        Scalar current_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(
+            g, A_e, c_e, A_i, c_i, s, y, z, μ);

        trial_x = x + α_max * step.p_x;
        trial_s = s + α_max * step.p_s;
@@ -583,7 +655,7 @@ ExitStatus interior_point(
        trial_c_e = matrices.c_e(trial_x);
        trial_c_i = matrices.c_i(trial_x);

-        Scalar next_kkt_error = kkt_error<Scalar>(
+        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.A_i(trial_x), trial_c_i, trial_s, trial_y, trial_z, μ);

@@ -595,43 +667,84 @@ ExitStatus interior_point(
          break;
        }

-        return ExitStatus::LINE_SEARCH_FAILED;
+        call_feasibility_restoration = true;
+        break;
      }
    }

    line_search_profiler.stop();

-    // If full step was accepted, reset full-step rejected counter
-    if (α == α_max) {
-      full_step_rejected_counter = 0;
-    }
+    if (call_feasibility_restoration) {
+      // If already in feasibility restoration mode, running it again won't help
+      if (in_feasibility_restoration) {
+        return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
+      }

-    // xₖ₊₁ = xₖ + αₖpₖˣ
-    // sₖ₊₁ = sₖ + αₖpₖˢ
-    // yₖ₊₁ = yₖ + αₖᶻpₖʸ
-    // zₖ₊₁ = zₖ + αₖᶻpₖᶻ
-    x += α * step.p_x;
-    s += α * step.p_s;
-    y += α_z * step.p_y;
-    z += α_z * step.p_z;
+      FilterEntry initial_entry{matrices.f(x), s, c_e, c_i, μ};

-    // A requirement for the convergence proof is that the primal-dual barrier
-    // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal
-    // barrier term Hessian μSₖ₊₁⁻².
-    //
-    //   Σₖ₊₁ = μSₖ₊₁⁻²
-    //   Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻²
-    //   Zₖ₊₁ = μSₖ₊₁⁻¹
-    //
-    // We ensure this by resetting
-    //
-    //   zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁)
-    //
-    // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2].
-    for (int row = 0; row < z.rows(); ++row) {
-      constexpr Scalar κ_Σ(1e10);
-      z[row] =
-          std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]);
+      // Feasibility restoration phase
+      gch::small_vector<std::function<bool(const IterationInfo<Scalar>& info)>>
+          callbacks;
+      for (auto& callback : iteration_callbacks) {
+        callbacks.emplace_back(callback);
+      }
+      callbacks.emplace_back([&](const IterationInfo<Scalar>& info) {
+        DenseVector trial_x =
+            info.x.segment(0, matrices.num_decision_variables);
+        DenseVector trial_s =
+            info.s.segment(0, matrices.num_inequality_constraints);
+
+        DenseVector trial_c_e = matrices.c_e(trial_x);
+        DenseVector trial_c_i = matrices.c_i(trial_x);
+
+        // If the current iterate sufficiently reduces constraint violation and
+        // is accepted by the normal filter, stop feasibility restoration
+        FilterEntry trial_entry{matrices.f(trial_x), trial_s, trial_c_e,
+                                trial_c_i, μ};
+        return trial_entry.constraint_violation <
+                   Scalar(0.9) * initial_entry.constraint_violation &&
+               filter.try_add(initial_entry, trial_entry, trial_x - x, g, α);
+      });
+      auto status = feasibility_restoration<Scalar>(matrices, callbacks,
+                                                    options, x, s, y, z, μ);
+
+      if (status != ExitStatus::SUCCESS) {
+        // Report failure
+        return status;
+      }
+    } else {
+      // If full step was accepted, reset full-step rejected counter
+      if (α == α_max) {
+        full_step_rejected_counter = 0;
+      }
+
+      // xₖ₊₁ = xₖ + αₖpₖˣ
+      // sₖ₊₁ = sₖ + αₖpₖˢ
+      // yₖ₊₁ = yₖ + αₖᶻpₖʸ
+      // zₖ₊₁ = zₖ + αₖᶻpₖᶻ
+      x += α * step.p_x;
+      s += α * step.p_s;
+      y += α_z * step.p_y;
+      z += α_z * step.p_z;
+
+      // A requirement for the convergence proof is that the primal-dual barrier
+      // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal
+      // barrier term Hessian μSₖ₊₁⁻².
+      //
+      //   Σₖ₊₁ = μSₖ₊₁⁻²
+      //   Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻²
+      //   Zₖ₊₁ = μSₖ₊₁⁻¹
+      //
+      // We ensure this by resetting
+      //
+      //   zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁)
+      //
+      // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2].
+      for (int row = 0; row < z.rows(); ++row) {
+        constexpr Scalar κ_Σ(1e10);
+        z[row] =
+            std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]);
+      }
    }

    // Update autodiff for Jacobians and Hessian
@@ -646,20 +759,23 @@ ExitStatus interior_point(
    c_e = matrices.c_e(x);
    c_i = matrices.c_i(x);

-    // Update the error estimate
-    E_0 = error_estimate<Scalar>(g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
+    // 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));

    // Update the barrier parameter if necessary
    if (E_0 > Scalar(options.tolerance)) {
      // Barrier parameter scale factor for tolerance checks
      constexpr Scalar κ_ε(10);

-      // While the error estimate is below the desired threshold for this
-      // barrier parameter value, decrease the barrier parameter further
-      Scalar E_μ = error_estimate<Scalar>(g, A_e, c_e, A_i, c_i, s, y, z, μ);
+      // While the error is below the desired threshold for this barrier
+      // parameter value, decrease the barrier parameter further
+      Scalar E_μ = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
+          g, A_e, c_e, A_i, c_i, s, y, z, μ);
      while (μ > μ_min && E_μ <= κ_ε * μ) {
        update_barrier_parameter_and_reset_filter();
-        E_μ = error_estimate<Scalar>(g, A_e, c_e, A_i, c_i, s, y, z, μ);
+        E_μ = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g, A_e, c_e, A_i,
+                                                               c_i, s, y, z, μ);
      }
    }

@@ -668,7 +784,9 @@ ExitStatus interior_point(

    if (options.diagnostics) {
      print_iteration_diagnostics(
-          iterations, IterationType::NORMAL,
+          iterations,
+          in_feasibility_restoration ? IterationType::FEASIBILITY_RESTORATION
+                                     : IterationType::NORMAL,
          inner_iter_profiler.current_duration(), E_0, f,
          c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>(), s.dot(z),
          μ, solver.hessian_regularization(), α, α_max, α_reduction_factor,
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp
@@ -21,6 +21,15 @@ struct InteriorPointMatrixCallbacks {
  /// Type alias for sparse vector.
  using SparseVector = Eigen::SparseVector<Scalar>;

+  /// Number of decision variables.
+  int num_decision_variables = 0;
+
+  /// Number of equality constraints.
+  int num_equality_constraints = 0;
+
+  /// Number of inequality constraints.
+  int num_inequality_constraints = 0;
+
  /// Cost function value f(x) getter.
  ///
  /// <table>
@@ -65,7 +74,7 @@ struct InteriorPointMatrixCallbacks {

  /// Lagrangian Hessian ∇ₓₓ²L(x, y, z) getter.
  ///
-  /// L(xₖ, yₖ, zₖ) = f(xₖ) − yₖᵀcₑ(xₖ) − zₖᵀcᵢ(xₖ)
+  /// L(x, y, z) = f(x) − yᵀcₑ(x) − zᵀcᵢ(x)
  ///
  /// <table>
  ///   <tr>
@@ -98,6 +107,39 @@ struct InteriorPointMatrixCallbacks {
                             const DenseVector& z)>
      H;

+  /// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x) − zᵀcᵢ(x)) getter.
+  ///
+  /// <table>
+  ///   <tr>
+  ///     <th>Variable</th>
+  ///     <th>Rows</th>
+  ///     <th>Columns</th>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>x</td>
+  ///     <td>num_decision_variables</td>
+  ///     <td>1</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>y</td>
+  ///     <td>num_equality_constraints</td>
+  ///     <td>1</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>z</td>
+  ///     <td>num_inequality_constraints</td>
+  ///     <td>1</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>∇ₓₓ²(−yᵀcₑ(x) − zᵀcᵢ(x))</td>
+  ///     <td>num_decision_variables</td>
+  ///     <td>num_decision_variables</td>
+  ///   </tr>
+  /// </table>
+  std::function<SparseMatrix(const DenseVector& x, const DenseVector& y,
+                             const DenseVector& z)>
+      H_c;
+
  /// Equality constraint value cₑ(x) getter.
  ///
  /// <table>
@@ -122,10 +164,10 @@ struct InteriorPointMatrixCallbacks {
  /// Equality constraint Jacobian ∂cₑ/∂x getter.
  ///
  /// ```
-  ///         [∇ᵀcₑ₁(xₖ)]
-  /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
-  ///         [    ⋮    ]
-  ///         [∇ᵀcₑₘ(xₖ)]
+  ///         [∇ᵀcₑ₁(x)]
+  /// Aₑ(x) = [∇ᵀcₑ₂(x)]
+  ///         [   ⋮    ]
+  ///         [∇ᵀcₑₘ(x)]
  /// ```
  ///
  /// <table>
@@ -171,10 +213,10 @@ struct InteriorPointMatrixCallbacks {
  /// Inequality constraint Jacobian ∂cᵢ/∂x getter.
  ///
  /// ```
-  ///         [∇ᵀcᵢ₁(xₖ)]
-  /// Aᵢ(x) = [∇ᵀcᵢ₂(xₖ)]
-  ///         [    ⋮    ]
-  ///         [∇ᵀcᵢₘ(xₖ)]
+  ///         [∇ᵀcᵢ₁(x)]
+  /// Aᵢ(x) = [∇ᵀcᵢ₂(x)]
+  ///         [   ⋮    ]
+  ///         [∇ᵀcᵢₘ(x)]
  /// ```
  ///
  /// <table>
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/iteration_info.hpp
@@ -18,6 +18,15 @@ struct IterationInfo {
  /// The decision variables.
  const Eigen::Vector<Scalar, Eigen::Dynamic>& x;

+  /// The inequality constraint slack variables.
+  const Eigen::Vector<Scalar, Eigen::Dynamic>& s;
+
+  /// The equality constraint dual variables.
+  const Eigen::Vector<Scalar, Eigen::Dynamic>& y;
+
+  /// The inequality constraint dual variables.
+  const Eigen::Vector<Scalar, Eigen::Dynamic>& z;
+
  /// The gradient of the cost function.
  const Eigen::SparseVector<Scalar>& g;

--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp
@@ -16,15 +16,14 @@
 #include "sleipnir/optimization/solver/iteration_info.hpp"
 #include "sleipnir/optimization/solver/newton_matrix_callbacks.hpp"
 #include "sleipnir/optimization/solver/options.hpp"
-#include "sleipnir/optimization/solver/util/error_estimate.hpp"
+#include "sleipnir/optimization/solver/util/all_finite.hpp"
 #include "sleipnir/optimization/solver/util/filter.hpp"
 #include "sleipnir/optimization/solver/util/kkt_error.hpp"
 #include "sleipnir/optimization/solver/util/regularized_ldlt.hpp"
 #include "sleipnir/util/assert.hpp"
 #include "sleipnir/util/print_diagnostics.hpp"
+#include "sleipnir/util/profiler.hpp"
 #include "sleipnir/util/scope_exit.hpp"
-#include "sleipnir/util/scoped_profiler.hpp"
-#include "sleipnir/util/solve_profiler.hpp"
 #include "sleipnir/util/symbol_exports.hpp"

 // See docs/algorithms.md#Works_cited for citation definitions.
@@ -94,6 +93,7 @@ ExitStatus newton(
  auto& H_prof = solve_profilers[11];

  NewtonMatrixCallbacks<Scalar> matrices{
+      matrix_callbacks.num_decision_variables,
      [&](const DenseVector& x) -> Scalar {
        ScopedProfiler prof{f_prof};
        return matrix_callbacks.f(x);
@@ -114,33 +114,30 @@ ExitStatus newton(
  setup_prof.start();

  Scalar f = matrices.f(x);
-
-  int num_decision_variables = x.rows();
-
  SparseVector g = matrices.g(x);
  SparseMatrix H = matrices.H(x);

  // Ensure matrix callback dimensions are consistent
-  slp_assert(g.rows() == num_decision_variables);
-  slp_assert(H.rows() == num_decision_variables);
-  slp_assert(H.cols() == num_decision_variables);
+  slp_assert(g.rows() == matrices.num_decision_variables);
+  slp_assert(H.rows() == matrices.num_decision_variables);
+  slp_assert(H.cols() == matrices.num_decision_variables);

-  // Check whether initial guess has finite f(xₖ)
-  if (!isfinite(f)) {
-    return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
+  // Check whether initial guess has finite cost and derivatives
+  if (!isfinite(f) || !all_finite(g) || !all_finite(H)) {
+    return ExitStatus::NONFINITE_INITIAL_GUESS;
  }

  int iterations = 0;

  Filter<Scalar> filter;

-  RegularizedLDLT<Scalar> solver{num_decision_variables, 0};
+  RegularizedLDLT<Scalar> solver{matrices.num_decision_variables, 0};

  // Variables for determining when a step is acceptable
  constexpr Scalar α_reduction_factor(0.5);
  constexpr Scalar α_min(1e-20);

-  // Error estimate
+  // Error
  Scalar E_0 = std::numeric_limits<Scalar>::infinity();

  setup_prof.stop();
@@ -170,7 +167,7 @@ ExitStatus newton(

    // Call iteration callbacks
    for (const auto& callback : iteration_callbacks) {
-      if (callback({iterations, x, g, H, SparseMatrix{}, SparseMatrix{}})) {
+      if (callback({iterations, x, {}, {}, {}, g, H, {}, {}})) {
        return ExitStatus::CALLBACK_REQUESTED_STOP;
      }
    }
@@ -207,13 +204,13 @@ ExitStatus newton(
        α *= α_reduction_factor;

        if (α < α_min) {
-          return ExitStatus::LINE_SEARCH_FAILED;
+          return ExitStatus::LOCALLY_INFEASIBLE;
        }
        continue;
      }

      // Check whether filter accepts trial iterate
-      if (filter.try_add(FilterEntry{trial_f}, α)) {
+      if (filter.try_add(FilterEntry{f}, FilterEntry{trial_f}, p_x, g, α)) {
        // Accept step
        break;
      }
@@ -224,11 +221,12 @@ ExitStatus newton(
      // If step size hit a minimum, check if the KKT error was reduced. If it
      // wasn't, report bad line search.
      if (α < α_min) {
-        Scalar current_kkt_error = kkt_error<Scalar>(g);
+        Scalar current_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(g);

        DenseVector trial_x = x + α_max * p_x;

-        Scalar next_kkt_error = kkt_error<Scalar>(matrices.g(trial_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) {
@@ -238,7 +236,7 @@ ExitStatus newton(
          break;
        }

-        return ExitStatus::LINE_SEARCH_FAILED;
+        return ExitStatus::LOCALLY_INFEASIBLE;
      }
    }

@@ -254,8 +252,8 @@ ExitStatus newton(

    ScopedProfiler next_iter_prep_profiler{next_iter_prep_prof};

-    // Update the error estimate
-    E_0 = error_estimate<Scalar>(g);
+    // Update the error
+    E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g);

    next_iter_prep_profiler.stop();
    inner_iter_profiler.stop();
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton_matrix_callbacks.hpp
@@ -21,6 +21,9 @@ struct NewtonMatrixCallbacks {
  /// Type alias for sparse vector.
  using SparseVector = Eigen::SparseVector<Scalar>;

+  /// Number of decision variables.
+  int num_decision_variables = 0;
+
  /// Cost function value f(x) getter.
  ///
  /// <table>
@@ -65,7 +68,7 @@ struct NewtonMatrixCallbacks {

  /// Lagrangian Hessian ∇ₓₓ²L(x) getter.
  ///
-  /// L(xₖ) = f(xₖ)
+  /// L(x) = f(x)
  ///
  /// <table>
  ///   <tr>
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/options.hpp
@@ -21,71 +21,19 @@ struct SLEIPNIR_DLLEXPORT Options {
  std::chrono::duration<double> timeout{
      std::numeric_limits<double>::infinity()};

-  /// Enables the feasible interior-point method. When the inequality
-  /// constraints are all feasible, step sizes are reduced when necessary to
-  /// prevent them becoming infeasible again. This is useful when parts of the
-  /// problem are ill-conditioned in infeasible regions (e.g., square root of a
-  /// negative value). This can slow or prevent progress toward a solution
-  /// though, so only enable it if necessary.
+  /// Enables the feasible interior-point method.
+  ///
+  /// When the inequality constraints are all feasible, step sizes are reduced
+  /// when necessary to prevent them becoming infeasible again. This is useful
+  /// when parts of the problem are ill-conditioned in infeasible regions (e.g.,
+  /// square root of a negative value). This can slow or prevent progress toward
+  /// a solution though, so only enable it if necessary.
  bool feasible_ipm = false;

-  /// Enables diagnostic prints.
+  /// Enables diagnostic output.
  ///
-  /// <table>
-  ///   <tr>
-  ///     <th>Heading</th>
-  ///     <th>Description</th>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>iter</td>
-  ///     <td>Iteration number</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>type</td>
-  ///     <td>Iteration type (normal, accepted second-order correction, rejected
-  ///     second-order correction)</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>time (ms)</td>
-  ///     <td>Duration of iteration in milliseconds</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>error</td>
-  ///     <td>Error estimate</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>cost</td>
-  ///     <td>Cost function value at current iterate</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>infeas.</td>
-  ///     <td>Constraint infeasibility at current iterate</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>complement.</td>
-  ///     <td>Complementary slackness at current iterate (sᵀz)</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>μ</td>
-  ///     <td>Barrier parameter</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>reg</td>
-  ///     <td>Iteration matrix regularization</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>primal α</td>
-  ///     <td>Primal step size</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>dual α</td>
-  ///     <td>Dual step size</td>
-  ///   </tr>
-  ///   <tr>
-  ///     <td>↩</td>
-  ///     <td>Number of line search backtracks</td>
-  ///   </tr>
-  /// </table>
+  /// See https://sleipnirgroup.github.io/Sleipnir/md_usage.html#output for more
+  /// information.
  bool diagnostics = false;
 };

--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp
@@ -16,16 +16,17 @@
 #include "sleipnir/optimization/solver/iteration_info.hpp"
 #include "sleipnir/optimization/solver/options.hpp"
 #include "sleipnir/optimization/solver/sqp_matrix_callbacks.hpp"
-#include "sleipnir/optimization/solver/util/error_estimate.hpp"
+#include "sleipnir/optimization/solver/util/all_finite.hpp"
+#include "sleipnir/optimization/solver/util/append_as_triplets.hpp"
+#include "sleipnir/optimization/solver/util/feasibility_restoration.hpp"
 #include "sleipnir/optimization/solver/util/filter.hpp"
 #include "sleipnir/optimization/solver/util/is_locally_infeasible.hpp"
 #include "sleipnir/optimization/solver/util/kkt_error.hpp"
 #include "sleipnir/optimization/solver/util/regularized_ldlt.hpp"
 #include "sleipnir/util/assert.hpp"
 #include "sleipnir/util/print_diagnostics.hpp"
+#include "sleipnir/util/profiler.hpp"
 #include "sleipnir/util/scope_exit.hpp"
-#include "sleipnir/util/scoped_profiler.hpp"
-#include "sleipnir/util/solve_profiler.hpp"
 #include "sleipnir/util/symbol_exports.hpp"

 // See docs/algorithms.md#Works_cited for citation definitions.
@@ -59,14 +60,49 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
               const Options& options,
               Eigen::Vector<Scalar, Eigen::Dynamic>& x) {
  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  DenseVector y = DenseVector::Zero(matrix_callbacks.num_equality_constraints);
+
+  return sqp(matrix_callbacks, iteration_callbacks, options, x, y);
+}
+
+/// Finds the optimal solution to a nonlinear program using Sequential Quadratic
+/// Programming (SQP).
+///
+/// A nonlinear program has the form:
+///
+/// ```
+///      min_x f(x)
+/// subject to cₑ(x) = 0
+/// ```
+///
+/// where f(x) is the cost function and cₑ(x) are the equality constraints.
+///
+/// @tparam Scalar Scalar type.
+/// @param[in] matrix_callbacks Matrix callbacks.
+/// @param[in] iteration_callbacks The list of callbacks to call at the
+///     beginning of each iteration.
+/// @param[in] options Solver options.
+/// @param[in,out] x The initial guess and output location for the decision
+///     variables.
+/// @param[in,out] y The initial guess and output location for the equality
+///     constraint dual variables.
+/// @return The exit status.
+template <typename Scalar>
+ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
+               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) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
  using SparseVector = Eigen::SparseVector<Scalar>;

  /// SQP step direction.
  struct Step {
-    /// Primal step.
+    /// Decision variable primal step.
    DenseVector p_x;
-    /// Dual step.
+    /// Equality constraint dual step.
    DenseVector p_y;
  };

@@ -89,6 +125,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
  solve_profilers.emplace_back("    ↳ f(x)");
  solve_profilers.emplace_back("    ↳ ∇f(x)");
  solve_profilers.emplace_back("    ↳ ∇²ₓₓL");
+  solve_profilers.emplace_back("    ↳ ∇²ₓₓyᵀcₑ");
  solve_profilers.emplace_back("    ↳ cₑ(x)");
  solve_profilers.emplace_back("    ↳ ∂cₑ/∂x");

@@ -109,10 +146,13 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
  auto& f_prof = solve_profilers[11];
  auto& g_prof = solve_profilers[12];
  auto& H_prof = solve_profilers[13];
-  auto& c_e_prof = solve_profilers[14];
-  auto& A_e_prof = solve_profilers[15];
+  auto& H_c_prof = solve_profilers[14];
+  auto& c_e_prof = solve_profilers[15];
+  auto& A_e_prof = solve_profilers[16];

  SQPMatrixCallbacks<Scalar> matrices{
+      matrix_callbacks.num_decision_variables,
+      matrix_callbacks.num_equality_constraints,
      [&](const DenseVector& x) -> Scalar {
        ScopedProfiler prof{f_prof};
        return matrix_callbacks.f(x);
@@ -125,6 +165,10 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
        ScopedProfiler prof{H_prof};
        return matrix_callbacks.H(x, y);
      },
+      [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix {
+        ScopedProfiler prof{H_c_prof};
+        return matrix_callbacks.H_c(x, y);
+      },
      [&](const DenseVector& x) -> DenseVector {
        ScopedProfiler prof{c_e_prof};
        return matrix_callbacks.c_e(x);
@@ -141,13 +185,21 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
  setup_prof.start();

  Scalar f = matrices.f(x);
+  SparseVector g = matrices.g(x);
+  SparseMatrix H = matrices.H(x, y);
  DenseVector c_e = matrices.c_e(x);
+  SparseMatrix A_e = matrices.A_e(x);

-  int num_decision_variables = x.rows();
-  int num_equality_constraints = c_e.rows();
+  // Ensure matrix callback dimensions are consistent
+  slp_assert(g.rows() == matrices.num_decision_variables);
+  slp_assert(H.rows() == matrices.num_decision_variables);
+  slp_assert(H.cols() == matrices.num_decision_variables);
+  slp_assert(c_e.rows() == matrices.num_equality_constraints);
+  slp_assert(A_e.rows() == matrices.num_equality_constraints);
+  slp_assert(A_e.cols() == matrices.num_decision_variables);

  // Check for overconstrained problem
-  if (num_equality_constraints > num_decision_variables) {
+  if (matrices.num_equality_constraints > matrices.num_decision_variables) {
    if (options.diagnostics) {
      print_too_few_dofs_error(c_e);
    }
@@ -155,34 +207,21 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
    return ExitStatus::TOO_FEW_DOFS;
  }

-  SparseVector g = matrices.g(x);
-  SparseMatrix A_e = matrices.A_e(x);
-
-  DenseVector y = DenseVector::Zero(num_equality_constraints);
-
-  SparseMatrix H = matrices.H(x, y);
-
-  // Ensure matrix callback dimensions are consistent
-  slp_assert(g.rows() == num_decision_variables);
-  slp_assert(A_e.rows() == num_equality_constraints);
-  slp_assert(A_e.cols() == num_decision_variables);
-  slp_assert(H.rows() == num_decision_variables);
-  slp_assert(H.cols() == num_decision_variables);
-
-  // Check whether initial guess has finite f(xₖ) and cₑ(xₖ)
-  if (!isfinite(f) || !c_e.allFinite()) {
-    return ExitStatus::NONFINITE_INITIAL_COST_OR_CONSTRAINTS;
+  // Check whether initial guess has finite cost, constraints, and derivatives
+  if (!isfinite(f) || !all_finite(g) || !all_finite(H) || !c_e.allFinite() ||
+      !all_finite(A_e)) {
+    return ExitStatus::NONFINITE_INITIAL_GUESS;
  }

  int iterations = 0;

-  Filter<Scalar> filter;
+  Filter<Scalar> filter{c_e.template lpNorm<1>()};

  // Kept outside the loop so its storage can be reused
  gch::small_vector<Eigen::Triplet<Scalar>> triplets;

-  RegularizedLDLT<Scalar> solver{num_decision_variables,
-                                 num_equality_constraints};
+  RegularizedLDLT<Scalar> solver{matrices.num_decision_variables,
+                                 matrices.num_equality_constraints};

  // Variables for determining when a step is acceptable
  constexpr Scalar α_reduction_factor(0.5);
@@ -190,7 +229,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,

  int full_step_rejected_counter = 0;

-  // Error estimate
+  // Error
  Scalar E_0 = std::numeric_limits<Scalar>::infinity();

  setup_prof.stop();
@@ -229,7 +268,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,

    // Call iteration callbacks
    for (const auto& callback : iteration_callbacks) {
-      if (callback({iterations, x, g, H, A_e, SparseMatrix{}})) {
+      if (callback({iterations, x, {}, y, {}, g, H, A_e, {}})) {
        return ExitStatus::CALLBACK_REQUESTED_STOP;
      }
    }
@@ -243,18 +282,10 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
    // Don't assign upper triangle because solver only uses lower triangle.
    triplets.clear();
    triplets.reserve(H.nonZeros() + A_e.nonZeros());
-    for (int col = 0; col < H.cols(); ++col) {
-      // Append column of H lower triangle in top-left quadrant
-      for (typename SparseMatrix::InnerIterator it{H, col}; it; ++it) {
-        triplets.emplace_back(it.row(), it.col(), it.value());
-      }
-      // Append column of Aₑ in bottom-left quadrant
-      for (typename SparseMatrix::InnerIterator it{A_e, col}; it; ++it) {
-        triplets.emplace_back(H.rows() + it.row(), it.col(), it.value());
-      }
-    }
-    SparseMatrix lhs(num_decision_variables + num_equality_constraints,
-                     num_decision_variables + num_equality_constraints);
+    append_as_triplets(triplets, 0, 0, {H, A_e});
+    SparseMatrix lhs(
+        matrices.num_decision_variables + matrices.num_equality_constraints,
+        matrices.num_decision_variables + matrices.num_equality_constraints);
    lhs.setFromSortedTriplets(triplets.begin(), triplets.end());

    // rhs = −[∇f − Aₑᵀy]
@@ -269,6 +300,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
    Step step;
    constexpr Scalar α_max(1);
    Scalar α(1);
+    bool call_feasibility_restoration = false;

    // Solve the Newton-KKT system
    //
@@ -295,6 +327,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,

    α = α_max;

+    const FilterEntry<Scalar> current_entry{f, c_e};
+
    // Loop until a step is accepted
    while (1) {
      DenseVector trial_x = x + α * step.p_x;
@@ -310,13 +344,15 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
        α *= α_reduction_factor;

        if (α < α_min) {
-          return ExitStatus::LINE_SEARCH_FAILED;
+          call_feasibility_restoration = true;
+          break;
        }
        continue;
      }

      // Check whether filter accepts trial iterate
-      if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) {
+      FilterEntry trial_entry{trial_f, trial_c_e};
+      if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
        // Accept step
        break;
      }
@@ -350,8 +386,9 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
                  step_acceptable ? IterationType::ACCEPTED_SOC
                                  : IterationType::REJECTED_SOC,
                  soc_profiler.current_duration(),
-                  error_estimate<Scalar>(g, A_e, trial_c_e, trial_y), trial_f,
-                  trial_c_e.template lpNorm<1>(), Scalar(0), Scalar(0),
+                  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));
            }
@@ -386,7 +423,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
          }

          // Check whether filter accepts trial iterate
-          if (filter.try_add(FilterEntry{trial_f, trial_c_e}, α)) {
+          FilterEntry trial_entry{trial_f, trial_c_e};
+          if (filter.try_add(current_entry, trial_entry, step.p_x, g, α)) {
            step = soc_step;
            α = α_soc;
            step_acceptable = true;
@@ -412,7 +450,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
      // See section 3.2 case I of [2].
      if (full_step_rejected_counter >= 4 &&
          filter.max_constraint_violation >
-              filter.back().constraint_violation / Scalar(10)) {
+              current_entry.constraint_violation / Scalar(10) &&
+          filter.last_rejection_due_to_filter()) {
        filter.max_constraint_violation *= Scalar(0.1);
        filter.reset();
        continue;
@@ -422,16 +461,17 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
      α *= α_reduction_factor;

      // If step size hit a minimum, check if the KKT error was reduced. If it
-      // wasn't, report line search failure.
+      // wasn't, invoke feasibility restoration.
      if (α < α_min) {
-        Scalar current_kkt_error = kkt_error<Scalar>(g, A_e, c_e, y);
+        Scalar current_kkt_error =
+            kkt_error<Scalar, KKTErrorType::ONE_NORM>(g, A_e, c_e, y);

        trial_x = x + α_max * step.p_x;
        trial_y = y + α_max * step.p_y;

        trial_c_e = matrices.c_e(trial_x);

-        Scalar next_kkt_error = kkt_error<Scalar>(
+        Scalar next_kkt_error = kkt_error<Scalar, KKTErrorType::ONE_NORM>(
            matrices.g(trial_x), matrices.A_e(trial_x), trial_c_e, trial_y);

        // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
@@ -442,21 +482,54 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
          break;
        }

-        return ExitStatus::LINE_SEARCH_FAILED;
+        call_feasibility_restoration = true;
+        break;
      }
    }

    line_search_profiler.stop();

-    // If full step was accepted, reset full-step rejected counter
-    if (α == α_max) {
-      full_step_rejected_counter = 0;
-    }
+    if (call_feasibility_restoration) {
+      FilterEntry initial_entry{matrices.f(x), c_e};

-    // xₖ₊₁ = xₖ + αₖpₖˣ
-    // yₖ₊₁ = yₖ + αₖpₖʸ
-    x += α * step.p_x;
-    y += α * step.p_y;
+      // Feasibility restoration phase
+      gch::small_vector<std::function<bool(const IterationInfo<Scalar>& info)>>
+          callbacks;
+      for (auto& callback : iteration_callbacks) {
+        callbacks.emplace_back(callback);
+      }
+      callbacks.emplace_back([&](const IterationInfo<Scalar>& info) {
+        DenseVector trial_x =
+            info.x.segment(0, matrices.num_decision_variables);
+
+        DenseVector trial_c_e = matrices.c_e(trial_x);
+
+        FilterEntry trial_entry{matrices.f(trial_x), trial_c_e};
+
+        // If the current iterate sufficiently reduces constraint violation and
+        // is accepted by the normal filter, stop feasibility restoration
+        return trial_entry.constraint_violation <
+                   Scalar(0.9) * initial_entry.constraint_violation &&
+               filter.try_add(initial_entry, trial_entry, trial_x - x, g, α);
+      });
+      auto status =
+          feasibility_restoration<Scalar>(matrices, callbacks, options, x, y);
+
+      if (status != ExitStatus::SUCCESS) {
+        // Report failure
+        return status;
+      }
+    } else {
+      // If full step was accepted, reset full-step rejected counter
+      if (α == α_max) {
+        full_step_rejected_counter = 0;
+      }
+
+      // xₖ₊₁ = xₖ + αₖpₖˣ
+      // yₖ₊₁ = yₖ + αₖpₖʸ
+      x += α * step.p_x;
+      y += α * step.p_y;
+    }

    // Update autodiff for Jacobians and Hessian
    f = matrices.f(x);
@@ -468,8 +541,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,

    c_e = matrices.c_e(x);

-    // Update the error estimate
-    E_0 = error_estimate<Scalar>(g, A_e, c_e, y);
+    // Update the error
+    E_0 = kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(g, A_e, c_e, y);

    next_iter_prep_profiler.stop();
    inner_iter_profiler.stop();
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp
@@ -21,6 +21,12 @@ struct SQPMatrixCallbacks {
  /// Type alias for sparse vector.
  using SparseVector = Eigen::SparseVector<Scalar>;

+  /// Number of decision variables.
+  int num_decision_variables = 0;
+
+  /// Number of equality constraints.
+  int num_equality_constraints = 0;
+
  /// Cost function value f(x) getter.
  ///
  /// <table>
@@ -65,7 +71,7 @@ struct SQPMatrixCallbacks {

  /// Lagrangian Hessian ∇ₓₓ²L(x, y) getter.
  ///
-  /// L(xₖ, yₖ) = f(xₖ) − yₖᵀcₑ(xₖ)
+  /// L(x, y) = f(x) − yᵀcₑ(x)
  ///
  /// <table>
  ///   <tr>
@@ -91,6 +97,32 @@ struct SQPMatrixCallbacks {
  /// </table>
  std::function<SparseMatrix(const DenseVector& x, const DenseVector& y)> H;

+  /// Constraint part of Lagrangian Hessian ∇ₓₓ² −yᵀcₑ(x) getter.
+  ///
+  /// <table>
+  ///   <tr>
+  ///     <th>Variable</th>
+  ///     <th>Rows</th>
+  ///     <th>Columns</th>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>x</td>
+  ///     <td>num_decision_variables</td>
+  ///     <td>1</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>y</td>
+  ///     <td>num_equality_constraints</td>
+  ///     <td>1</td>
+  ///   </tr>
+  ///   <tr>
+  ///     <td>∇ₓₓ² −yᵀcₑ(x)</td>
+  ///     <td>num_decision_variables</td>
+  ///     <td>num_decision_variables</td>
+  ///   </tr>
+  /// </table>
+  std::function<SparseMatrix(const DenseVector& x, const DenseVector& y)> H_c;
+
  /// Equality constraint value cₑ(x) getter.
  ///
  /// <table>
@@ -115,10 +147,10 @@ struct SQPMatrixCallbacks {
  /// Equality constraint Jacobian ∂cₑ/∂x getter.
  ///
  /// ```
-  ///         [∇ᵀcₑ₁(xₖ)]
-  /// Aₑ(x) = [∇ᵀcₑ₂(xₖ)]
-  ///         [    ⋮    ]
-  ///         [∇ᵀcₑₘ(xₖ)]
+  ///         [∇ᵀcₑ₁(x)]
+  /// Aₑ(x) = [∇ᵀcₑ₂(x)]
+  ///         [   ⋮    ]
+  ///         [∇ᵀcₑₘ(x)]
  /// ```
  ///
  /// <table>
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/all_finite.hpp
@@ -0,0 +1,26 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <cmath>
+
+#include <Eigen/SparseCore>
+
+/// Returns true if elements of sparse matrix are all finite.
+///
+/// @param mat Sparse matrix.
+/// @return True if elements of sparse matrix are all finite.
+template <typename Derived>
+bool all_finite(const Eigen::SparseCompressedBase<Derived>& mat) {
+  using std::isfinite;
+
+  for (int k = 0; k < mat.outerSize(); ++k) {
+    for (typename Derived::InnerIterator it{mat, k}; it; ++it) {
+      if (!isfinite(it.value())) {
+        return false;
+      }
+    }
+  }
+
+  return true;
+}
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/append_as_triplets.hpp
@@ -0,0 +1,60 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <initializer_list>
+
+#include <Eigen/SparseCore>
+#include <gch/small_vector.hpp>
+
+/// Appends sparse matrices to list of triplets at the given offset.
+///
+/// The triplets are appended in column-major order (e.g., first column of mat1,
+/// first column of mat2 underneath first column of mat1, second column of mat1,
+/// second column of mat2 underneath second column of mat1).
+///
+/// @tparam Scalar Scalar type.
+/// @param triplets List of triplets.
+/// @param row_offset Row offset for first matrix.
+/// @param col_offset Column offset for first matrix.
+/// @param mats Sparse matrices to append.
+template <typename Scalar>
+void append_as_triplets(
+    gch::small_vector<Eigen::Triplet<Scalar>>& triplets, int row_offset,
+    int col_offset, std::initializer_list<Eigen::SparseMatrix<Scalar>> mats) {
+  // Compute row offset for each matrix
+  gch::small_vector<int> mat_row_offsets;
+  int mat_row_offset = 0;
+  for (const auto& mat : mats) {
+    mat_row_offsets.emplace_back(mat_row_offset);
+    mat_row_offset += mat.rows();
+  }
+
+  // Append elements in column-major order
+  for (int col = 0; col < mats.begin()[0].cols(); ++col) {
+    for (size_t i = 0; i < mats.size(); ++i) {
+      for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{
+               mats.begin()[i], col};
+           it; ++it) {
+        triplets.emplace_back(row_offset + mat_row_offsets[i] + it.row(),
+                              col_offset + it.col(), it.value());
+      }
+    }
+  }
+}
+
+/// Append diagonal matrix to list of triplets at the given offset.
+///
+/// @tparam Scalar Scalar type.
+/// @param triplets List of triplets.
+/// @param row_offset Row offset for first matrix.
+/// @param col_offset Column offset for first matrix.
+/// @param diag Diagonal of matrix.
+template <typename Scalar>
+void append_diagonal_as_triplets(
+    gch::small_vector<Eigen::Triplet<Scalar>>& triplets, int row_offset,
+    int col_offset, const Eigen::Vector<Scalar, Eigen::Dynamic>& diag) {
+  for (int row = 0; row < diag.rows(); ++row) {
+    triplets.emplace_back(row_offset + row, col_offset + row, diag[row]);
+  }
+}
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/error_estimate.hpp
@@ -1,130 +0,0 @@
-// Copyright (c) Sleipnir contributors
-
-#pragma once
-
-#include <algorithm>
-
-#include <Eigen/Core>
-#include <Eigen/SparseCore>
-
-// See docs/algorithms.md#Works_cited for citation definitions
-
-namespace slp {
-
-/// Returns the error estimate using the KKT conditions for Newton's method.
-///
-/// @tparam Scalar Scalar type.
-/// @param g Gradient of the cost function ∇f.
-template <typename Scalar>
-Scalar error_estimate(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
-  // Update the error estimate using the KKT conditions from equations (19.5a)
-  // through (19.5d) of [1].
-  //
-  //   ∇f = 0
-
-  return g.template lpNorm<Eigen::Infinity>();
-}
-
-/// Returns the error estimate using the KKT conditions for SQP.
-///
-/// @tparam Scalar Scalar type.
-/// @param g Gradient of the cost function ∇f.
-/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
-///     current iterate.
-/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
-///     iterate.
-/// @param y Equality constraint dual variables.
-template <typename Scalar>
-Scalar error_estimate(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) {
-  // Update the error estimate using the KKT conditions from equations (19.5a)
-  // through (19.5d) of [1].
-  //
-  //   ∇f − Aₑᵀy = 0
-  //   cₑ = 0
-  //
-  // The error tolerance is the max of the following infinity norms scaled by
-  // s_d (see equation (5) of [2]).
-  //
-  //   ‖∇f − Aₑᵀy‖_∞ / s_d
-  //   ‖cₑ‖_∞
-
-  // s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ
-  constexpr Scalar s_max(100);
-  Scalar s_d =
-      std::max(s_max, y.template lpNorm<1>() / Scalar(y.rows())) / s_max;
-
-  return std::max(
-      {(g - A_e.transpose() * y).template lpNorm<Eigen::Infinity>() / s_d,
-       c_e.template lpNorm<Eigen::Infinity>()});
-}
-
-/// Returns the error estimate using the KKT conditions for the interior-point
-/// method.
-///
-/// @tparam Scalar Scalar type.
-/// @param g Gradient of the cost function ∇f.
-/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
-///     current iterate.
-/// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
-///     iterate.
-/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
-///     the current iterate.
-/// @param c_i The problem's inequality constraints cᵢ(x) evaluated at the
-///     current iterate.
-/// @param s Inequality constraint slack variables.
-/// @param y Equality constraint dual variables.
-/// @param z Inequality constraint dual variables.
-/// @param μ Barrier parameter.
-template <typename Scalar>
-Scalar error_estimate(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 μ) {
-  // Update the error estimate using the KKT conditions from equations (19.5a)
-  // through (19.5d) of [1].
-  //
-  //   ∇f − Aₑᵀy − Aᵢᵀz = 0
-  //   Sz − μe = 0
-  //   cₑ = 0
-  //   cᵢ − s = 0
-  //
-  // The error tolerance is the max of the following infinity norms scaled by
-  // s_d and s_c (see equation (5) of [2]).
-  //
-  //   ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d
-  //   ‖Sz − μe‖_∞ / s_c
-  //   ‖cₑ‖_∞
-  //   ‖cᵢ − s‖_∞
-
-  // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ
-  constexpr Scalar s_max(100);
-  Scalar s_d =
-      std::max(s_max, (y.template lpNorm<1>() + z.template lpNorm<1>()) /
-                          Scalar(y.rows() + z.rows())) /
-      s_max;
-
-  // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ
-  Scalar s_c =
-      std::max(s_max, z.template lpNorm<1>() / Scalar(z.rows())) / s_max;
-
-  const auto S = s.asDiagonal();
-  const Eigen::Vector<Scalar, Eigen::Dynamic> μe =
-      Eigen::Vector<Scalar, Eigen::Dynamic>::Constant(s.rows(), μ);
-
-  return std::max({(g - A_e.transpose() * y - A_i.transpose() * z)
-                           .template lpNorm<Eigen::Infinity>() /
-                       s_d,
-                   (S * z - μe).template lpNorm<Eigen::Infinity>() / s_c,
-                   c_e.template lpNorm<Eigen::Infinity>(),
-                   (c_i - s).template lpNorm<Eigen::Infinity>()});
-}
-
-}  // namespace slp
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp
@@ -0,0 +1,601 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <span>
+#include <tuple>
+#include <utility>
+
+#include <Eigen/Core>
+#include <gch/small_vector.hpp>
+
+#include "sleipnir/optimization/solver/exit_status.hpp"
+#include "sleipnir/optimization/solver/interior_point_matrix_callbacks.hpp"
+#include "sleipnir/optimization/solver/iteration_info.hpp"
+#include "sleipnir/optimization/solver/options.hpp"
+#include "sleipnir/optimization/solver/sqp_matrix_callbacks.hpp"
+#include "sleipnir/optimization/solver/util/append_as_triplets.hpp"
+#include "sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp"
+
+namespace slp {
+
+template <typename Scalar>
+ExitStatus interior_point(
+    const InteriorPointMatrixCallbacks<Scalar>& matrix_callbacks,
+    std::span<std::function<bool(const IterationInfo<Scalar>& info)>>
+        iteration_callbacks,
+    const Options& options, bool in_feasibility_restoration,
+#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& μ);
+
+/// Computes initial values for p and n in feasibility restoration.
+///
+/// @tparam Scalar Scalar type.
+/// @param[in] c The constraint violation.
+/// @param[in] ρ Scaling parameter.
+/// @param[in] μ Barrier parameter.
+/// @return Tuple of positive and negative constraint relaxation slack variables
+///     respectively.
+template <typename Scalar>
+std::tuple<Eigen::Vector<Scalar, Eigen::Dynamic>,
+           Eigen::Vector<Scalar, Eigen::Dynamic>>
+compute_p_n(const Eigen::Vector<Scalar, Eigen::Dynamic>& c, Scalar ρ,
+            Scalar μ) {
+  // From equation (33) of [2]:
+  //                       ______________________
+  //       μ − ρ c(x)     /(μ − ρ c(x))²   μ c(x)
+  //   n = −−−−−−−−−− +  / (−−−−−−−−−−)  + −−−−−−     (1)
+  //           2ρ       √  (    2ρ    )      2ρ
+  //
+  // The quadratic formula:
+  //             ________
+  //       -b + √b² - 4ac
+  //   x = −−−−−−−−−−−−−−                             (2)
+  //             2a
+  //
+  // Rearrange (1) to fit the quadratic formula better:
+  //                     _________________________
+  //       μ - ρ c(x) + √(μ - ρ c(x))² + 2ρ μ c(x)
+  //   n = −−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−−
+  //                         2ρ
+  //
+  // Solve for coefficients:
+  //
+  //   a = ρ                                          (3)
+  //   b = ρ c(x) - μ                                 (4)
+  //
+  //   -4ac = 2ρ μ c(x)
+  //   -4(ρ)c = 2ρ μ c(x)
+  //   -4c = 2μ c(x)
+  //   c = -μ c(x)/2                                  (5)
+  //
+  //   p = c(x) + n                                   (6)
+
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  using std::sqrt;
+
+  DenseVector p{c.rows()};
+  DenseVector n{c.rows()};
+  for (int row = 0; row < p.rows(); ++row) {
+    Scalar _a = ρ;
+    Scalar _b = ρ * c[row] - μ;
+    Scalar _c = -μ * c[row] / Scalar(2);
+
+    n[row] = (-_b + sqrt(_b * _b - Scalar(4) * _a * _c)) / (Scalar(2) * _a);
+    p[row] = c[row] + n[row];
+  }
+
+  return {std::move(p), std::move(n)};
+}
+
+// @cond Suppress Doxygen
+
+/// Finds the iterate that minimizes the constraint violation while not
+/// deviating too far from the starting point. This is a fallback procedure when
+/// the normal Sequential Quadratic Programming method fails to converge to a
+/// feasible point.
+///
+/// @tparam Scalar Scalar type.
+/// @param[in] matrix_callbacks Matrix callbacks.
+/// @param[in] iteration_callbacks The list of callbacks to call at the
+///     beginning of each iteration.
+/// @param[in] options Solver options.
+/// @param[in,out] x The decision variables from the normal solve.
+/// @param[in,out] y The equality constraint dual variables from the normal
+///     solve.
+/// @return The exit status.
+template <typename Scalar>
+ExitStatus feasibility_restoration(
+    const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
+    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) {
+  // Feasibility restoration
+  //
+  //        min  ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
+  //         x
+  //       pₑ,nₑ
+  //
+  //   s.t. cₑ(x) - pₑ + nₑ = 0
+  //        pₑ ≥ 0
+  //        nₑ ≥ 0
+  //
+  // where ρ = 1000, ζ = √μ where μ is the barrier parameter, xᵣ is original
+  // iterate before feasibility restoration, and Dᵣ is a scaling matrix defined
+  // by
+  //
+  //   Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
+
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  using DiagonalMatrix = Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic>;
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+  using SparseVector = Eigen::SparseVector<Scalar>;
+
+  using std::sqrt;
+
+  const auto& matrices = matrix_callbacks;
+  const auto& num_vars = matrices.num_decision_variables;
+  const auto& num_eq = matrices.num_equality_constraints;
+
+  constexpr Scalar ρ(1e3);
+  const Scalar μ(options.tolerance / 10.0);
+  const Scalar ζ = sqrt(μ);
+
+  const DenseVector c_e = matrices.c_e(x);
+
+  const auto& x_r = x;
+  const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ);
+
+  // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
+  const DiagonalMatrix D_r =
+      x.cwiseSquare().cwiseInverse().cwiseMin(Scalar(1)).asDiagonal();
+
+  DenseVector fr_x{num_vars + 2 * num_eq};
+  fr_x << x, p_e_0, n_e_0;
+
+  DenseVector fr_s = DenseVector::Ones(2 * num_eq);
+
+  DenseVector fr_y = DenseVector::Zero(num_eq);
+
+  DenseVector fr_z{2 * num_eq};
+  fr_z << μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse();
+
+  Scalar fr_μ = std::max(μ, c_e.template lpNorm<Eigen::Infinity>());
+
+  InteriorPointMatrixCallbacks<Scalar> fr_matrix_callbacks{
+      static_cast<int>(fr_x.rows()),
+      static_cast<int>(fr_y.rows()),
+      static_cast<int>(fr_z.rows()),
+      [&](const DenseVector& x_p) -> Scalar {
+        auto x = x_p.segment(0, num_vars);
+
+        // Cost function
+        //
+        //   ρ Σ (pₑ + nₑ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
+
+        auto diff = x - x_r;
+        return ρ * x_p.segment(num_vars, 2 * num_eq).array().sum() +
+               ζ / Scalar(2) * diff.transpose() * D_r * diff;
+      },
+      [&](const DenseVector& x_p) -> SparseVector {
+        auto x = x_p.segment(0, num_vars);
+
+        // Cost function gradient
+        //
+        //   [ζDᵣ(x − xᵣ)]
+        //   [     ρ     ]
+        //   [     ρ     ]
+        DenseVector g{x_p.rows()};
+        g.segment(0, num_vars) = ζ * D_r * (x - x_r);
+        g.segment(num_vars, 2 * num_eq).setConstant(ρ);
+        return g.sparseView();
+      },
+      [&](const DenseVector& x_p, const DenseVector& y_p,
+          [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix {
+        auto x = x_p.segment(0, num_vars);
+        const auto& y = y_p;
+
+        // Cost function Hessian
+        //
+        //   [ζDᵣ  0  0]
+        //   [ 0   0  0]
+        //   [ 0   0  0]
+        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
+        triplets.reserve(x_p.rows());
+        append_as_triplets(triplets, 0, 0, {SparseMatrix{ζ * D_r}});
+        SparseMatrix d2f_dx2{x_p.rows(), x_p.rows()};
+        d2f_dx2.setFromSortedTriplets(triplets.begin(), triplets.end());
+
+        // Constraint part of original problem's Lagrangian Hessian
+        //
+        //   −∇ₓₓ²yᵀcₑ(x)
+        auto H_c = matrices.H_c(x, y);
+        H_c.resize(x_p.rows(), x_p.rows());
+
+        // Lagrangian Hessian
+        //
+        //   [ζDᵣ  0  0]
+        //   [ 0   0  0] − ∇ₓₓ²yᵀcₑ(x)
+        //   [ 0   0  0]
+        return d2f_dx2 + H_c;
+      },
+      [&](const DenseVector& x_p, [[maybe_unused]] const DenseVector& y_p,
+          [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix {
+        return SparseMatrix{x_p.rows(), x_p.rows()};
+      },
+      [&](const DenseVector& x_p) -> DenseVector {
+        auto x = x_p.segment(0, num_vars);
+        auto p_e = x_p.segment(num_vars, num_eq);
+        auto n_e = x_p.segment(num_vars + num_eq, num_eq);
+
+        // Equality constraints
+        //
+        //   cₑ(x) - pₑ + nₑ = 0
+        return matrices.c_e(x) - p_e + n_e;
+      },
+      [&](const DenseVector& x_p) -> SparseMatrix {
+        auto x = x_p.segment(0, num_vars);
+
+        // Equality constraint Jacobian
+        //
+        //   [Aₑ  −I  I]
+
+        SparseMatrix A_e = matrices.A_e(x);
+
+        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
+        triplets.reserve(A_e.nonZeros() + 2 * num_eq);
+
+        append_as_triplets(triplets, 0, 0, {A_e});
+        append_diagonal_as_triplets(
+            triplets, 0, num_vars,
+            DenseVector::Constant(num_eq, Scalar(-1)).eval());
+        append_diagonal_as_triplets(
+            triplets, 0, num_vars + num_eq,
+            DenseVector::Constant(num_eq, Scalar(1)).eval());
+
+        SparseMatrix A_e_p{A_e.rows(), x_p.rows()};
+        A_e_p.setFromSortedTriplets(triplets.begin(), triplets.end());
+        return A_e_p;
+      },
+      [&](const DenseVector& x_p) -> DenseVector {
+        // Inequality constraints
+        //
+        //   pₑ ≥ 0
+        //   nₑ ≥ 0
+        return x_p.segment(num_vars, 2 * num_eq);
+      },
+      [&](const DenseVector& x_p) -> SparseMatrix {
+        // Inequality constraint Jacobian
+        //
+        //   [0  I  0]
+        //   [0  0  I]
+
+        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
+        triplets.reserve(2 * num_eq);
+
+        append_diagonal_as_triplets(
+            triplets, 0, num_vars,
+            DenseVector::Constant(2 * num_eq, Scalar(1)).eval());
+
+        SparseMatrix A_i_p{2 * num_eq, x_p.rows()};
+        A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
+        return A_i_p;
+      }};
+
+  auto status = interior_point<Scalar>(fr_matrix_callbacks, iteration_callbacks,
+                                       options, true,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+                                       {},
+#endif
+                                       fr_x, fr_s, fr_y, fr_z, fr_μ);
+
+  x = fr_x.segment(0, x.rows());
+
+  if (status == ExitStatus::CALLBACK_REQUESTED_STOP) {
+    auto g = matrices.g(x);
+    auto A_e = matrices.A_e(x);
+
+    y = lagrange_multiplier_estimate(g, A_e);
+
+    return ExitStatus::SUCCESS;
+  } else if (status == ExitStatus::SUCCESS) {
+    return ExitStatus::LOCALLY_INFEASIBLE;
+  } else {
+    return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
+  }
+}
+
+/// Finds the iterate that minimizes the constraint violation while not
+/// deviating too far from the starting point. This is a fallback procedure when
+/// the normal interior-point method fails to converge to a feasible point.
+///
+/// @tparam Scalar Scalar type.
+/// @param[in] matrix_callbacks Matrix callbacks.
+/// @param[in] iteration_callbacks The list of callbacks to call at the
+///     beginning of each iteration.
+/// @param[in] options Solver options.
+/// @param[in,out] x The current decision variables from the normal solve.
+/// @param[in,out] s The current inequality constraint slack variables from the
+///     normal solve.
+/// @param[in,out] y The current equality constraint duals from the normal
+///     solve.
+/// @param[in,out] z The current inequality constraint duals from the normal
+///     solve.
+/// @param[in] μ Barrier parameter.
+/// @return The exit status.
+template <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,
+    Eigen::Vector<Scalar, Eigen::Dynamic>& s,
+    Eigen::Vector<Scalar, Eigen::Dynamic>& y,
+    Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar μ) {
+  // Feasibility restoration
+  //
+  //        min  ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
+  //         x
+  //       pₑ,nₑ
+  //       pᵢ,nᵢ
+  //
+  //   s.t. cₑ(x) - pₑ + nₑ = 0
+  //        cᵢ(x) - pᵢ + nᵢ ≥ 0
+  //        pₑ ≥ 0
+  //        nₑ ≥ 0
+  //        pᵢ ≥ 0
+  //        nᵢ ≥ 0
+  //
+  // where ρ = 1000, ζ = √μ where μ is the barrier parameter, xᵣ is original
+  // iterate before feasibility restoration, and Dᵣ is a scaling matrix defined
+  // by
+  //
+  //   Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
+
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  using DiagonalMatrix = Eigen::DiagonalMatrix<Scalar, Eigen::Dynamic>;
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+  using SparseVector = Eigen::SparseVector<Scalar>;
+
+  using std::sqrt;
+
+  const auto& matrices = matrix_callbacks;
+  const auto& num_vars = matrices.num_decision_variables;
+  const auto& num_eq = matrices.num_equality_constraints;
+  const auto& num_ineq = matrices.num_inequality_constraints;
+
+  constexpr Scalar ρ(1e3);
+  const Scalar ζ = sqrt(μ);
+
+  const DenseVector c_e = matrices.c_e(x);
+  const DenseVector c_i = matrices.c_i(x);
+
+  const auto& x_r = x;
+  const auto [p_e_0, n_e_0] = compute_p_n(c_e, ρ, μ);
+  const auto [p_i_0, n_i_0] = compute_p_n((c_i - s).eval(), ρ, μ);
+
+  // Dᵣ = diag(min(1, 1/xᵣ[i]²) for i in x.rows())
+  const DiagonalMatrix D_r =
+      x.cwiseSquare().cwiseInverse().cwiseMin(Scalar(1)).asDiagonal();
+
+  DenseVector fr_x{num_vars + 2 * num_eq + 2 * num_ineq};
+  fr_x << x, p_e_0, n_e_0, p_i_0, n_i_0;
+
+  DenseVector fr_s{s.rows() + 2 * num_eq + 2 * num_ineq};
+  fr_s.segment(0, s.rows()) = s;
+  fr_s.segment(s.rows(), 2 * num_eq + 2 * num_ineq).setOnes();
+
+  DenseVector fr_y = DenseVector::Zero(c_e.rows());
+
+  DenseVector fr_z{c_i.rows() + 2 * num_eq + 2 * num_ineq};
+  fr_z << z.cwiseMin(ρ), μ * p_e_0.cwiseInverse(), μ * n_e_0.cwiseInverse(),
+      μ * p_i_0.cwiseInverse(), μ * n_i_0.cwiseInverse();
+
+  Scalar fr_μ = std::max({μ, c_e.template lpNorm<Eigen::Infinity>(),
+                          (c_i - s).template lpNorm<Eigen::Infinity>()});
+
+  InteriorPointMatrixCallbacks<Scalar> fr_matrix_callbacks{
+      static_cast<int>(fr_x.rows()),
+      static_cast<int>(fr_y.rows()),
+      static_cast<int>(fr_z.rows()),
+      [&](const DenseVector& x_p) -> Scalar {
+        auto x = x_p.segment(0, num_vars);
+
+        // Cost function
+        //
+        //   ρ Σ (pₑ + nₑ + pᵢ + nᵢ) + ζ/2 (x - xᵣ)ᵀDᵣ(x - xᵣ)
+        auto diff = x - x_r;
+        return ρ * x_p.segment(num_vars, 2 * num_eq + 2 * num_ineq)
+                       .array()
+                       .sum() +
+               ζ / Scalar(2) * diff.transpose() * D_r * diff;
+      },
+      [&](const DenseVector& x_p) -> SparseVector {
+        auto x = x_p.segment(0, num_vars);
+
+        // Cost function gradient
+        //
+        //   [ζDᵣ(x − xᵣ)]
+        //   [     ρ     ]
+        //   [     ρ     ]
+        //   [     ρ     ]
+        //   [     ρ     ]
+        DenseVector g{x_p.rows()};
+        g.segment(0, num_vars) = ζ * D_r * (x - x_r);
+        g.segment(num_vars, 2 * num_eq + 2 * num_ineq).setConstant(ρ);
+        return g.sparseView();
+      },
+      [&](const DenseVector& x_p, const DenseVector& y_p,
+          const DenseVector& z_p) -> SparseMatrix {
+        auto x = x_p.segment(0, num_vars);
+        const auto& y = y_p;
+        auto z = z_p.segment(0, num_ineq);
+
+        // Cost function Hessian
+        //
+        //   [ζDᵣ  0  0  0  0]
+        //   [ 0   0  0  0  0]
+        //   [ 0   0  0  0  0]
+        //   [ 0   0  0  0  0]
+        //   [ 0   0  0  0  0]
+        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
+        triplets.reserve(x_p.rows());
+        append_as_triplets(triplets, 0, 0, {SparseMatrix{ζ * D_r}});
+        SparseMatrix d2f_dx2{x_p.rows(), x_p.rows()};
+        d2f_dx2.setFromSortedTriplets(triplets.begin(), triplets.end());
+
+        // Constraint part of original problem's Lagrangian Hessian
+        //
+        //   −∇ₓₓ²yᵀcₑ(x) − ∇ₓₓ²zᵀcᵢ(x)
+        auto H_c = matrices.H_c(x, y, z);
+        H_c.resize(x_p.rows(), x_p.rows());
+
+        // Lagrangian Hessian
+        //
+        //   [ζDᵣ  0  0  0  0]
+        //   [ 0   0  0  0  0]
+        //   [ 0   0  0  0  0] − ∇ₓₓ²yᵀcₑ(x) − ∇ₓₓ²zᵀcᵢ(x)
+        //   [ 0   0  0  0  0]
+        //   [ 0   0  0  0  0]
+        return d2f_dx2 + H_c;
+      },
+      [&](const DenseVector& x_p, [[maybe_unused]] const DenseVector& y_p,
+          [[maybe_unused]] const DenseVector& z_p) -> SparseMatrix {
+        return SparseMatrix{x_p.rows(), x_p.rows()};
+      },
+      [&](const DenseVector& x_p) -> DenseVector {
+        auto x = x_p.segment(0, num_vars);
+        auto p_e = x_p.segment(num_vars, num_eq);
+        auto n_e = x_p.segment(num_vars + num_eq, num_eq);
+
+        // Equality constraints
+        //
+        //   cₑ(x) - pₑ + nₑ = 0
+        return matrices.c_e(x) - p_e + n_e;
+      },
+      [&](const DenseVector& x_p) -> SparseMatrix {
+        auto x = x_p.segment(0, num_vars);
+
+        // Equality constraint Jacobian
+        //
+        //   [Aₑ  −I  I  0  0]
+
+        SparseMatrix A_e = matrices.A_e(x);
+
+        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
+        triplets.reserve(A_e.nonZeros() + 2 * num_eq);
+
+        append_as_triplets(triplets, 0, 0, {A_e});
+        append_diagonal_as_triplets(
+            triplets, 0, num_vars,
+            DenseVector::Constant(num_eq, Scalar(-1)).eval());
+        append_diagonal_as_triplets(
+            triplets, 0, num_vars + num_eq,
+            DenseVector::Constant(num_eq, Scalar(1)).eval());
+
+        SparseMatrix A_e_p{A_e.rows(), x_p.rows()};
+        A_e_p.setFromSortedTriplets(triplets.begin(), triplets.end());
+        return A_e_p;
+      },
+      [&](const DenseVector& x_p) -> DenseVector {
+        auto x = x_p.segment(0, num_vars);
+        auto p_i = x_p.segment(num_vars + 2 * num_eq, num_ineq);
+        auto n_i = x_p.segment(num_vars + 2 * num_eq + num_ineq, num_ineq);
+
+        // Inequality constraints
+        //
+        //   cᵢ(x) - pᵢ + nᵢ ≥ 0
+        //   pₑ ≥ 0
+        //   nₑ ≥ 0
+        //   pᵢ ≥ 0
+        //   nᵢ ≥ 0
+        DenseVector c_i_p{c_i.rows() + 2 * num_eq + 2 * num_ineq};
+        c_i_p.segment(0, num_ineq) = matrices.c_i(x) - p_i + n_i;
+        c_i_p.segment(p_i.rows(), 2 * num_eq + 2 * num_ineq) =
+            x_p.segment(num_vars, 2 * num_eq + 2 * num_ineq);
+        return c_i_p;
+      },
+      [&](const DenseVector& x_p) -> SparseMatrix {
+        auto x = x_p.segment(0, num_vars);
+
+        // Inequality constraint Jacobian
+        //
+        //   [Aᵢ  0  0  −I  I]
+        //   [0   I  0   0  0]
+        //   [0   0  I   0  0]
+        //   [0   0  0   I  0]
+        //   [0   0  0   0  I]
+
+        SparseMatrix A_i = matrices.A_i(x);
+
+        gch::small_vector<Eigen::Triplet<Scalar>> triplets;
+        triplets.reserve(A_i.nonZeros() + 2 * num_eq + 4 * num_ineq);
+
+        // Column 0
+        append_as_triplets(triplets, 0, 0, {A_i});
+
+        // Columns 1 and 2
+        append_diagonal_as_triplets(
+            triplets, num_ineq, num_vars,
+            DenseVector::Constant(2 * num_eq, Scalar(1)).eval());
+
+        SparseMatrix I_ineq{
+            DenseVector::Constant(num_ineq, Scalar(1)).asDiagonal()};
+
+        // Column 3
+        SparseMatrix Z_col3{2 * num_eq, num_ineq};
+        append_as_triplets(triplets, 0, num_vars + 2 * num_eq,
+                           {(-I_ineq).eval(), Z_col3, I_ineq});
+
+        // Column 4
+        SparseMatrix Z_col4{2 * num_eq + num_ineq, num_ineq};
+        append_as_triplets(triplets, 0, num_vars + 2 * num_eq + num_ineq,
+                           {I_ineq, Z_col4, I_ineq});
+
+        SparseMatrix A_i_p{2 * num_eq + 3 * num_ineq, x_p.rows()};
+        A_i_p.setFromSortedTriplets(triplets.begin(), triplets.end());
+        return A_i_p;
+      }};
+
+  auto status = interior_point<Scalar>(fr_matrix_callbacks, iteration_callbacks,
+                                       options, true,
+#ifdef SLEIPNIR_ENABLE_BOUND_PROJECTION
+                                       {},
+#endif
+                                       fr_x, fr_s, fr_y, fr_z, fr_μ);
+
+  x = fr_x.segment(0, x.rows());
+  s = fr_s.segment(0, s.rows());
+
+  if (status == ExitStatus::CALLBACK_REQUESTED_STOP) {
+    auto g = matrices.g(x);
+    auto A_e = matrices.A_e(x);
+    auto A_i = matrices.A_i(x);
+
+    auto [y_estimate, z_estimate] =
+        lagrange_multiplier_estimate(g, A_e, A_i, s, μ);
+    y = y_estimate;
+    z = z_estimate;
+
+    return ExitStatus::SUCCESS;
+  } else if (status == ExitStatus::SUCCESS) {
+    return ExitStatus::LOCALLY_INFEASIBLE;
+  } else {
+    return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
+  }
+}
+
+// @endcond
+
+}  // namespace slp
+
+#include "sleipnir/optimization/solver/interior_point.hpp"
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/filter.hpp
@@ -5,9 +5,9 @@
 #include <algorithm>
 #include <cmath>
 #include <limits>
-#include <utility>

 #include <Eigen/Core>
+#include <Eigen/SparseCore>
 #include <gch/small_vector.hpp>

 // See docs/algorithms.md#Works_cited for citation definitions.
@@ -34,14 +34,10 @@ struct FilterEntry {
  ///
  /// @param cost The cost function's value.
  /// @param constraint_violation The constraint violation.
-  constexpr FilterEntry(Scalar cost, Scalar constraint_violation)
+  explicit constexpr FilterEntry(Scalar cost,
+                                 Scalar constraint_violation = Scalar(0))
      : cost{cost}, constraint_violation{constraint_violation} {}

-  /// Constructs a Newton's method filter entry.
-  ///
-  /// @param f The cost function value.
-  explicit FilterEntry(Scalar f) : FilterEntry{f, Scalar(0)} {}
-
  /// Constructs a Sequential Quadratic Programming filter entry.
  ///
  /// @param f The cost function value.
@@ -61,6 +57,15 @@ struct FilterEntry {
      : FilterEntry{f - μ * s.array().log().sum(),
                    c_e.template lpNorm<1>() + (c_i - s).template lpNorm<1>()} {
  }
+
+  /// Returns true if this filter entry is dominated by another.
+  ///
+  /// @param entry The other entry.
+  /// @return True if this filter entry is dominated by another.
+  constexpr bool dominated_by(const FilterEntry<Scalar>& entry) const {
+    return entry.cost <= cost &&
+           entry.constraint_violation <= constraint_violation;
+  }
 };

 /// Step filter.
@@ -71,115 +76,151 @@ struct FilterEntry {
 template <typename Scalar>
 class Filter {
 public:
+  /// Type alias for dense vector.
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+
+  /// Type alias for sparse vector.
+  using SparseVector = Eigen::SparseVector<Scalar>;
+
+  /// The minimum constraint violation
+  Scalar min_constraint_violation;
+
  /// The maximum constraint violation
-  Scalar max_constraint_violation{1e4};
+  Scalar max_constraint_violation;

  /// Constructs an empty filter.
-  Filter() {
-    // Initial filter entry rejects constraint violations above max
-    m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
-                          max_constraint_violation);
+  ///
+  /// @param initial_constraint_violation The optimization problem's initial
+  ///     constraint violation.
+  explicit constexpr Filter(Scalar initial_constraint_violation = Scalar(0)) {
+    using std::max;
+
+    min_constraint_violation =
+        Scalar(1e-4) * max(Scalar(1), initial_constraint_violation);
+    max_constraint_violation =
+        Scalar(1e4) * max(Scalar(1), initial_constraint_violation);
  }

  /// Resets the filter.
  void reset() {
    m_filter.clear();
-
-    // Initial filter entry rejects constraint violations above max
-    m_filter.emplace_back(std::numeric_limits<Scalar>::infinity(),
-                          max_constraint_violation);
+    m_last_rejection_due_to_filter = false;
  }

-  /// Adds a new entry to the filter.
+  /// Returns true if the given trial entry is acceptable to the filter.
  ///
-  /// @param entry The entry to add to the filter.
-  void add(const FilterEntry<Scalar>& entry) {
-    // Remove dominated entries
-    erase_if(m_filter, [&](const auto& elem) {
-      return entry.cost <= elem.cost &&
-             entry.constraint_violation <= elem.constraint_violation;
-    });
-
-    m_filter.push_back(entry);
-  }
-
-  /// Adds a new entry to the filter.
-  ///
-  /// @param entry The entry to add to the filter.
-  void add(FilterEntry<Scalar>&& entry) {
-    // Remove dominated entries
-    erase_if(m_filter, [&](const auto& elem) {
-      return entry.cost <= elem.cost &&
-             entry.constraint_violation <= elem.constraint_violation;
-    });
-
-    m_filter.push_back(entry);
-  }
-
-  /// Returns true if the given iterate is accepted by the filter.
-  ///
-  /// @param entry The entry to attempt adding to the filter.
-  /// @param α The step size (0, 1].
-  /// @return True if the given iterate is accepted by the filter.
-  bool try_add(const FilterEntry<Scalar>& entry, Scalar α) {
-    if (is_acceptable(entry, α)) {
-      add(entry);
-      return true;
-    } else {
-      return false;
-    }
-  }
-
-  /// Returns true if the given iterate is accepted by the filter.
-  ///
-  /// @param entry The entry to attempt adding to the filter.
-  /// @param α The step size (0, 1].
-  /// @return True if the given iterate is accepted by the filter.
-  bool try_add(FilterEntry<Scalar>&& entry, Scalar α) {
-    if (is_acceptable(entry, α)) {
-      add(std::move(entry));
-      return true;
-    } else {
-      return false;
-    }
-  }
-
-  /// Returns true if the given entry is acceptable to the filter.
-  ///
-  /// @param entry The entry to check.
+  /// @param current_entry The entry corresponding to the current iterate.
+  /// @param trial_entry The entry corresponding to the trial iterate.
+  /// @param p_x Decision variable primal step.
+  /// @param g Cost function gradient.
  /// @param α The step size (0, 1].
  /// @return True if the given entry is acceptable to the filter.
-  bool is_acceptable(const FilterEntry<Scalar>& entry, Scalar α) {
+  bool try_add(const FilterEntry<Scalar>& current_entry,
+               const FilterEntry<Scalar>& trial_entry, const DenseVector& p_x,
+               const SparseVector& g, Scalar α) {
    using std::isfinite;
    using std::pow;

-    if (!isfinite(entry.cost) || !isfinite(entry.constraint_violation)) {
+    // Reject steps with nonfinite cost or constraint violation above maximum
+    if (!isfinite(trial_entry.cost) ||
+        trial_entry.constraint_violation > max_constraint_violation) {
      return false;
    }

-    Scalar ϕ = pow(α, Scalar(1.5));
+    Scalar g_p_x = g.transpose() * p_x;

-    // If current filter entry is better than all prior ones in some respect,
-    // accept it.
+    // Switching condition
+    constexpr Scalar s_ϕ(2.3);
+    constexpr Scalar s_θ(1.1);
+    bool switching_condition =
+        g_p_x < Scalar(0) &&
+        α * pow(-g_p_x, s_ϕ) > pow(current_entry.constraint_violation, s_θ);
+
+    // Armijo condition
+    constexpr Scalar η_ϕ(1e-8);
+    bool armijo_condition =
+        trial_entry.cost <= current_entry.cost + η_ϕ * α * g_p_x;
+
+    // Sufficient decrease condition
    //
    // See equation (2.13) of [4].
-    return std::ranges::all_of(m_filter, [&](const auto& elem) {
-      return entry.cost <= elem.cost - ϕ * γ_cost * elem.constraint_violation ||
-             entry.constraint_violation <=
-                 (Scalar(1) - ϕ * γ_constraint) * elem.constraint_violation;
-    });
+    Scalar ϕ = pow(α, Scalar(1.5));
+    bool sufficient_decrease =
+        trial_entry.cost <=
+            current_entry.cost -
+                ϕ * γ_cost * current_entry.constraint_violation ||
+        trial_entry.constraint_violation <=
+            (Scalar(1) - ϕ * γ_constraint) * current_entry.constraint_violation;
+
+    // If constraint violation is below threshold and switching condition is
+    // true, check Armijo condition for step rejection. Otherwise, check
+    // sufficient decrease condition.
+    if (current_entry.constraint_violation <= min_constraint_violation &&
+        switching_condition) {
+      if (!armijo_condition) {
+        m_last_rejection_due_to_filter = false;
+        return false;
+      }
+    } else if (!sufficient_decrease) {
+      m_last_rejection_due_to_filter = false;
+      return false;
+    }
+
+    // Reject steps in filter (i.e., dominated by any filter entry)
+    if (in_filter(trial_entry)) {
+      m_last_rejection_due_to_filter = true;
+      return false;
+    }
+
+    // Augment filter with accepted iterate if switching condition or Armijo
+    // condition are false
+    if (!switching_condition || !armijo_condition) {
+      add(FilterEntry{
+          current_entry.cost - ϕ * γ_cost * current_entry.constraint_violation,
+          (Scalar(1) - ϕ * γ_constraint) * current_entry.constraint_violation});
+    }
+
+    return true;
  }

-  /// Returns the most recently added filter entry.
+  /// Returns true if the most recent trial entry rejection was due to the
+  /// filter.
  ///
-  /// @return The most recently added filter entry.
-  const FilterEntry<Scalar>& back() const { return m_filter.back(); }
+  /// @return True if the most recent trial entry rejection was due to the
+  /// filter.
+  bool last_rejection_due_to_filter() const {
+    return m_last_rejection_due_to_filter;
+  }

 private:
  static constexpr Scalar γ_cost{1e-8};
  static constexpr Scalar γ_constraint{1e-5};

  gch::small_vector<FilterEntry<Scalar>> m_filter;
+
+  bool m_last_rejection_due_to_filter = false;
+
+  /// Adds a new entry to the filter.
+  ///
+  /// @param entry The entry to add to the filter.
+  void add(const FilterEntry<Scalar>& entry) {
+    // Remove dominated entries
+    erase_if(m_filter,
+             [&](const auto& elem) { return elem.dominated_by(entry); });
+
+    m_filter.push_back(entry);
+  }
+
+  /// Returns true if the given entry is in the filter.
+  ///
+  /// @param entry The entry.
+  /// @return True if the given entry is in the filter.
+  bool in_filter(const FilterEntry<Scalar>& entry) const {
+    // An entry is in the filter if it's dominated by any filter entry
+    return std::any_of(m_filter.begin(), m_filter.end(), [&](const auto& elem) {
+      return entry.dominated_by(elem);
+    });
+  }
 };

 }  // namespace slp
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/kkt_error.hpp
@@ -2,6 +2,8 @@

 #pragma once

+#include <algorithm>
+
 #include <Eigen/Core>
 #include <Eigen/SparseCore>

@@ -9,47 +11,75 @@

 namespace slp {

+/// Type of KKT error to compute.
+enum class KKTErrorType {
+  /// ∞-norm of scaled KKT condition errors.
+  INF_NORM_SCALED,
+  /// 1-norm of KKT condition errors.
+  ONE_NORM
+};
+
 /// Returns the KKT error for Newton's method.
 ///
 /// @tparam Scalar Scalar type.
+/// @tparam T The type of KKT error to compute.
 /// @param g Gradient of the cost function ∇f.
-template <typename Scalar>
+template <typename Scalar, KKTErrorType T>
 Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g) {
-  // Compute the KKT error as the 1-norm of the KKT conditions from equations
-  // (19.5a) through (19.5d) of [1].
+  // The KKT conditions from docs/algorithms.md:
  //
  //   ∇f = 0

-  return g.template lpNorm<1>();
+  if constexpr (T == KKTErrorType::INF_NORM_SCALED) {
+    return g.template lpNorm<Eigen::Infinity>();
+  } else if constexpr (T == KKTErrorType::ONE_NORM) {
+    return g.template lpNorm<1>();
+  }
 }

 /// Returns the KKT error for Sequential Quadratic Programming.
 ///
 /// @tparam Scalar Scalar type.
+/// @tparam T The type of KKT error to compute.
 /// @param g Gradient of the cost function ∇f.
 /// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
 ///     current iterate.
 /// @param c_e The problem's equality constraints cₑ(x) evaluated at the current
 ///     iterate.
 /// @param y Equality constraint dual variables.
-template <typename Scalar>
+template <typename Scalar, KKTErrorType T>
 Scalar kkt_error(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) {
-  // Compute the KKT error as the 1-norm of the KKT conditions from equations
-  // (19.5a) through (19.5d) of [1].
+  // The KKT conditions from docs/algorithms.md:
  //
  //   ∇f − Aₑᵀy = 0
  //   cₑ = 0

-  return (g - A_e.transpose() * y).template lpNorm<1>() +
-         c_e.template lpNorm<1>();
+  if constexpr (T == KKTErrorType::INF_NORM_SCALED) {
+    // See equation (5) of [2].
+
+    // s_d = max(sₘₐₓ, ‖y‖₁ / m) / sₘₐₓ
+    constexpr Scalar s_max(100);
+    Scalar s_d =
+        std::max(s_max, y.template lpNorm<1>() / Scalar(y.rows())) / s_max;
+
+    // ‖∇f − Aₑᵀy‖_∞ / s_d
+    // ‖cₑ‖_∞
+    return std::max(
+        {(g - A_e.transpose() * y).template lpNorm<Eigen::Infinity>() / s_d,
+         c_e.template lpNorm<Eigen::Infinity>()});
+  } else if constexpr (T == KKTErrorType::ONE_NORM) {
+    return (g - A_e.transpose() * y).template lpNorm<1>() +
+           c_e.template lpNorm<1>();
+  }
 }

 /// Returns the KKT error for the interior-point method.
 ///
 /// @tparam Scalar Scalar type.
+/// @tparam T The type of KKT error to compute.
 /// @param g Gradient of the cost function ∇f.
 /// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
 ///     current iterate.
@@ -63,7 +93,7 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
 /// @param y Equality constraint dual variables.
 /// @param z Inequality constraint dual variables.
 /// @param μ Barrier parameter.
-template <typename Scalar>
+template <typename Scalar, KKTErrorType T>
 Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
                 const Eigen::SparseMatrix<Scalar>& A_e,
                 const Eigen::Vector<Scalar, Eigen::Dynamic>& c_e,
@@ -72,21 +102,51 @@ Scalar kkt_error(const Eigen::Vector<Scalar, Eigen::Dynamic>& g,
                 const Eigen::Vector<Scalar, Eigen::Dynamic>& s,
                 const Eigen::Vector<Scalar, Eigen::Dynamic>& y,
                 const Eigen::Vector<Scalar, Eigen::Dynamic>& z, Scalar μ) {
-  // Compute the KKT error as the 1-norm of the KKT conditions from equations
-  // (19.5a) through (19.5d) of [1].
+  // The KKT conditions from docs/algorithms.md:
  //
  //   ∇f − Aₑᵀy − Aᵢᵀz = 0
  //   Sz − μe = 0
  //   cₑ = 0
  //   cᵢ − s = 0

-  const auto S = s.asDiagonal();
-  const Eigen::Vector<Scalar, Eigen::Dynamic> μe =
-      Eigen::Vector<Scalar, Eigen::Dynamic>::Constant(s.rows(), μ);
+  if constexpr (T == KKTErrorType::INF_NORM_SCALED) {
+    // See equation (5) of [2].

-  return (g - A_e.transpose() * y - A_i.transpose() * z).template lpNorm<1>() +
-         (S * z - μe).template lpNorm<1>() + c_e.template lpNorm<1>() +
-         (c_i - s).template lpNorm<1>();
+    // s_d = max(sₘₐₓ, (‖y‖₁ + ‖z‖₁) / (m + n)) / sₘₐₓ
+    constexpr Scalar s_max(100);
+    Scalar s_d =
+        std::max(s_max, (y.template lpNorm<1>() + z.template lpNorm<1>()) /
+                            Scalar(y.rows() + z.rows())) /
+        s_max;
+
+    // s_c = max(sₘₐₓ, ‖z‖₁ / n) / sₘₐₓ
+    Scalar s_c =
+        std::max(s_max, z.template lpNorm<1>() / Scalar(z.rows())) / s_max;
+
+    const auto S = s.asDiagonal();
+    const Eigen::Vector<Scalar, Eigen::Dynamic> μe =
+        Eigen::Vector<Scalar, Eigen::Dynamic>::Constant(s.rows(), μ);
+
+    // ‖∇f − Aₑᵀy − Aᵢᵀz‖_∞ / s_d
+    // ‖Sz − μe‖_∞ / s_c
+    // ‖cₑ‖_∞
+    // ‖cᵢ − s‖_∞
+    return std::max({(g - A_e.transpose() * y - A_i.transpose() * z)
+                             .template lpNorm<Eigen::Infinity>() /
+                         s_d,
+                     (S * z - μe).template lpNorm<Eigen::Infinity>() / s_c,
+                     c_e.template lpNorm<Eigen::Infinity>(),
+                     (c_i - s).template lpNorm<Eigen::Infinity>()});
+  } else if constexpr (T == KKTErrorType::ONE_NORM) {
+    const auto S = s.asDiagonal();
+    const Eigen::Vector<Scalar, Eigen::Dynamic> μe =
+        Eigen::Vector<Scalar, Eigen::Dynamic>::Constant(s.rows(), μ);
+
+    return (g - A_e.transpose() * y - A_i.transpose() * z)
+               .template lpNorm<1>() +
+           (S * z - μe).template lpNorm<1>() + c_e.template lpNorm<1>() +
+           (c_i - s).template lpNorm<1>();
+  }
 }

 }  // namespace slp
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp
@@ -0,0 +1,135 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <algorithm>
+#include <utility>
+
+#include <Eigen/SparseCholesky>
+#include <Eigen/SparseCore>
+#include <gch/small_vector.hpp>
+
+#include "sleipnir/optimization/solver/util/append_as_triplets.hpp"
+
+namespace slp {
+
+/// Lagrange multiplier estimates.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+struct LagrangeMultiplierEstimate {
+  /// Equality constraint dual estimate.
+  Eigen::Vector<Scalar, Eigen::Dynamic> y;
+  /// Inequality constraint dual estimate.
+  Eigen::Vector<Scalar, Eigen::Dynamic> z;
+};
+
+/// Estimates Lagrange multipliers for SQP.
+///
+/// @tparam Scalar Scalar type.
+/// @param g Gradient of the cost function ∇f.
+/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
+///     current iterate.
+template <typename Scalar>
+Eigen::Vector<Scalar, Eigen::Dynamic> lagrange_multiplier_estimate(
+    const Eigen::SparseVector<Scalar>& g,
+    const Eigen::SparseMatrix<Scalar>& A_e) {
+  // Lagrange multiplier estimates
+  //
+  //   ∇f − Aₑᵀy = 0
+  //   Aₑᵀy = ∇f
+  //   y = (AₑAₑᵀ)⁻¹Aₑ∇f
+  return Eigen::SimplicialLDLT<Eigen::SparseMatrix<Scalar>>{A_e *
+                                                            A_e.transpose()}
+      .solve(A_e * g);
+}
+
+/// Estimates Lagrange multipliers for interior-point method.
+///
+/// @tparam Scalar Scalar type.
+/// @param g Gradient of the cost function ∇f.
+/// @param A_e The problem's equality constraint Jacobian Aₑ(x) evaluated at the
+///     current iterate.
+/// @param A_i The problem's inequality constraint Jacobian Aᵢ(x) evaluated at
+///     the current iterate.
+/// @param s Inequality constraint slack variables.
+/// @param μ Barrier parameter.
+template <typename Scalar>
+LagrangeMultiplierEstimate<Scalar> lagrange_multiplier_estimate(
+    const Eigen::SparseVector<Scalar>& g,
+    const Eigen::SparseMatrix<Scalar>& A_e,
+    const Eigen::SparseMatrix<Scalar>& A_i,
+    const Eigen::Vector<Scalar, Eigen::Dynamic>& s, Scalar μ) {
+  using DenseVector = Eigen::Vector<Scalar, Eigen::Dynamic>;
+  using SparseMatrix = Eigen::SparseMatrix<Scalar>;
+
+  // Lagrange multiplier estimates
+  //
+  //   ∇f − Aₑᵀy − Aᵢᵀz = 0
+  //   Sz − μe = 0
+  //
+  //   Aₑᵀy + Aᵢᵀz = ∇f
+  //   −Sz = −μe
+  //
+  //   [Aₑᵀ  Aᵢᵀ][y] = [ ∇f]
+  //   [ 0   −S ][z]   [−μe]
+  //
+  //   [Aₑ   0]ᵀ[y] = [ ∇f]
+  //   [Aᵢ  −S] [z]   [−μe]
+  //
+  // Let Â = [Aₑ   0]
+  //         [Aᵢ  −S]
+  //
+  //   Âᵀ[y] = [ ∇f]
+  //     [z]   [−μe]
+  //
+  //   [y] = (ÂÂᵀ)⁻¹Â[ ∇f]
+  //   [z]           [−μe]
+
+  gch::small_vector<Eigen::Triplet<Scalar>> triplets;
+
+  // Â = [Aₑ   0]
+  //     [Aᵢ  −S]
+  triplets.reserve(A_e.nonZeros() + A_i.nonZeros() + s.rows());
+  append_as_triplets(triplets, 0, 0, {A_e, A_i});
+  append_diagonal_as_triplets(triplets, A_e.rows(), A_i.cols(), (-s).eval());
+  SparseMatrix A_hat{A_e.rows() + A_i.rows(), A_e.cols() + s.rows()};
+  A_hat.setFromSortedTriplets(triplets.begin(), triplets.end());
+
+  // lhs = ÂÂᵀ
+  SparseMatrix lhs = A_hat * A_hat.transpose();
+
+  // rhs = Â[ ∇f]
+  //        [−μe]
+  DenseVector rhs_temp{g.rows() + s.rows()};
+  rhs_temp.segment(0, g.rows()) = g;
+  rhs_temp.segment(g.rows(), s.rows()).setConstant(-μ);
+  DenseVector rhs = A_hat * rhs_temp;
+
+  Eigen::SimplicialLDLT<SparseMatrix> yz_estimator{lhs};
+  DenseVector sol = yz_estimator.solve(rhs);
+  DenseVector y = sol.segment(0, A_e.rows());
+  DenseVector z = sol.segment(A_e.rows(), s.rows());
+
+  // A requirement for the convergence proof is that the primal-dual barrier
+  // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal barrier
+  // term Hessian μSₖ₊₁⁻².
+  //
+  //   Σₖ₊₁ = μSₖ₊₁⁻²
+  //   Sₖ₊₁⁻¹Zₖ₊₁ = μSₖ₊₁⁻²
+  //   Zₖ₊₁ = μSₖ₊₁⁻¹
+  //
+  // We ensure this by resetting
+  //
+  //   zₖ₊₁ = clamp(zₖ₊₁, 1/κ_Σ μ/sₖ₊₁, κ_Σ μ/sₖ₊₁)
+  //
+  // for some fixed κ_Σ ≥ 1 after each step. See equation (16) of [2].
+  for (int row = 0; row < z.rows(); ++row) {
+    constexpr Scalar κ_Σ(1e10);
+    z[row] = std::clamp(z[row], Scalar(1) / κ_Σ * μ / s[row], κ_Σ * μ / s[row]);
+  }
+
+  return {std::move(y), std::move(z)};
+}
+
+}  // namespace slp
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/util/regularized_ldlt.hpp
@@ -36,6 +36,19 @@ class RegularizedLDLT {
      : m_num_decision_variables{num_decision_variables},
        m_num_equality_constraints{num_equality_constraints} {}

+  /// Constructs a RegularizedLDLT instance.
+  ///
+  /// @param num_decision_variables The number of decision variables in the
+  ///     system.
+  /// @param num_equality_constraints The number of equality constraints in the
+  ///     system.
+  /// @param γ_min The minimum constraint regularization.
+  RegularizedLDLT(int num_decision_variables, int num_equality_constraints,
+                  Scalar γ_min)
+      : m_num_decision_variables{num_decision_variables},
+        m_num_equality_constraints{num_equality_constraints},
+        m_γ_min{γ_min} {}
+
  /// Reports whether previous computation was successful.
  ///
  /// @return Whether previous computation was successful.
@@ -55,14 +68,14 @@ class RegularizedLDLT {
    m_info = m_is_sparse ? compute_sparse(lhs).info()
                         : m_dense_solver.compute(lhs).info();

-    Inertia inertia;
-
    if (m_info == Eigen::Success) {
-      inertia = m_is_sparse ? Inertia{m_sparse_solver.vectorD()}
-                            : Inertia{m_dense_solver.vectorD()};
+      auto D =
+          m_is_sparse ? m_sparse_solver.vectorD() : m_dense_solver.vectorD();

-      // If the inertia is ideal, don't regularize the system
-      if (inertia == ideal_inertia) {
+      // If the inertia is ideal and D from LDLT is sufficiently far from zero,
+      // don't regularize the system
+      if (Inertia{D} == ideal_inertia &&
+          (D.cwiseAbs().array() >= Scalar(1e-4)).all()) {
        m_prev_δ = Scalar(0);
        return *this;
      }
@@ -73,9 +86,11 @@ class RegularizedLDLT {
    // attempt a δ and γ half as big as the previous run so δ and γ can trend
    // downwards over time.
    Scalar δ = m_prev_δ == Scalar(0) ? Scalar(1e-4) : m_prev_δ / Scalar(2);
-    Scalar γ(1e-10);
+    Scalar γ = m_γ_min;

    while (true) {
+      Inertia inertia;
+
      // Regularize lhs by adding a multiple of the identity matrix
      //
      // lhs = [H + AᵢᵀΣAᵢ + δI  Aₑᵀ]
@@ -109,7 +124,7 @@ class RegularizedLDLT {
        } else if (inertia.positive > ideal_inertia.positive) {
          // If there's too many positive eigenvalues, increase γ by an order of
          // magnitude and try again
-          γ *= Scalar(10);
+          γ = γ == Scalar(0) ? Scalar(1e-10) : γ * Scalar(10);
        }
      } else {
        // If the decomposition failed, increase δ and γ by an order of
@@ -132,7 +147,7 @@ class RegularizedLDLT {
  /// @param rhs Right-hand side of the system.
  /// @return The solution.
  template <typename Rhs>
-  DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) {
+  DenseVector solve(const Eigen::MatrixBase<Rhs>& rhs) const {
    if (m_is_sparse) {
      return m_sparse_solver.solve(rhs);
    } else {
@@ -145,7 +160,7 @@ class RegularizedLDLT {
  /// @param rhs Right-hand side of the system.
  /// @return The solution.
  template <typename Rhs>
-  DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) {
+  DenseVector solve(const Eigen::SparseMatrixBase<Rhs>& rhs) const {
    if (m_is_sparse) {
      return m_sparse_solver.solve(rhs);
    } else {
@@ -174,6 +189,9 @@ class RegularizedLDLT {
  /// The number of equality constraints in the system.
  int m_num_equality_constraints = 0;

+  /// The minimum constraint regularization.
+  Scalar m_γ_min{1e-10};
+
  /// The ideal system inertia.
  Inertia ideal_inertia{m_num_decision_variables, m_num_equality_constraints,
                        0};
@@ -206,7 +224,7 @@ class RegularizedLDLT {
  /// @param δ The Hessian regularization factor.
  /// @param γ The equality constraint Jacobian regularization factor.
  /// @return Regularization matrix.
-  SparseMatrix regularization(Scalar δ, Scalar γ) {
+  SparseMatrix regularization(Scalar δ, Scalar γ) const {
    DenseVector vec{m_num_decision_variables + m_num_equality_constraints};
    vec.segment(0, m_num_decision_variables).setConstant(δ);
    vec.segment(m_num_decision_variables, m_num_equality_constraints)
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/assert.hpp
@@ -8,7 +8,7 @@

 #include <fmt/format.h>

-/// Throw an exception in Python.
+/// Throws an exception in Python.
 #define slp_assert(condition)                                        \
  do {                                                               \
    if (!(condition)) {                                              \
@@ -21,6 +21,6 @@
 #else
 #include <cassert>

-/// Abort in C++.
+/// Aborts in C++.
 #define slp_assert(condition) assert(condition)
 #endif
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/function_ref.hpp
@@ -75,7 +75,7 @@ class function_ref<R(Args...)> {
    std::swap(callback_, rhs.callback_);
  }

-  /// Call the stored callable with the given arguments.
+  /// Calls the stored callable with the given arguments.
  ///
  /// @param args The arguments.
  /// @return The return value of the callable.
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp
@@ -16,8 +16,8 @@
 #include <gch/small_vector.hpp>

 #include "sleipnir/util/print.hpp"
-#include "sleipnir/util/setup_profiler.hpp"
-#include "sleipnir/util/solve_profiler.hpp"
+#include "sleipnir/util/profiler.hpp"
+#include "sleipnir/util/to_underlying.hpp"

 namespace slp {

@@ -28,7 +28,9 @@ enum class IterationType : uint8_t {
  /// Accepted second-order correction iteration.
  ACCEPTED_SOC,
  /// Rejected second-order correction iteration.
-  REJECTED_SOC
+  REJECTED_SOC,
+  /// Feasibility restoration iteration.
+  FEASIBILITY_RESTORATION
 };

 /// Converts std::chrono::duration to a number of milliseconds rounded to three
@@ -234,13 +236,13 @@ void print_iteration_diagnostics(int iterations, IterationType type,
  int backtracks =
      static_cast<int>(log(primal_α / primal_α_max) / log(α_reduction_factor));

-  constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC"};
+  constexpr std::array ITERATION_TYPES = {"norm", "✓SOC", "XSOC", "rest"};
  slp::println(
      "│{:4} {:4} {:9.3f} {:12e} {:13e} {:12e} {:12e} {:.2e} {:<5} {:.2e} "
      "{:.2e} {:2d}│",
-      iterations, ITERATION_TYPES[static_cast<uint8_t>(type)], to_ms(time),
-      error, cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α,
-      dual_α, backtracks);
+      iterations, ITERATION_TYPES[slp::to_underlying(type)], to_ms(time), error,
+      cost, infeasibility, complementarity, μ, power_of_10(δ), primal_α, dual_α,
+      backtracks);
 }
 #else
 #define print_iteration_diagnostics(...)
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/profiler.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/profiler.hpp
@@ -0,0 +1,187 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <chrono>
+#include <concepts>
+#include <string>
+#include <string_view>
+#include <utility>
+
+namespace slp {
+
+/// Records the number of profiler measurements (start/stop pairs) and the
+/// average duration between each start and stop call.
+class SetupProfiler {
+ public:
+  /// Constructs a SetupProfiler.
+  ///
+  /// @param name Name of measurement to show in diagnostics.
+  explicit SetupProfiler(std::string_view name) : m_name{name} {}
+
+  /// Starts setup time measurement.
+  void start() {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    m_setup_start_time = std::chrono::steady_clock::now();
+#endif
+  }
+
+  /// Stops setup time measurement.
+  void stop() {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    m_setup_stop_time = std::chrono::steady_clock::now();
+    m_setup_duration = m_setup_stop_time - m_setup_start_time;
+#endif
+  }
+
+  /// Returns name of measurement to show in diagnostics.
+  ///
+  /// @return Name of measurement to show in diagnostics.
+  std::string_view name() const { return m_name; }
+
+  /// Returns the setup duration in milliseconds as a double.
+  ///
+  /// @return The setup duration in milliseconds as a double.
+  const std::chrono::duration<double>& duration() const {
+    return m_setup_duration;
+  }
+
+ private:
+  /// Name of measurement to show in diagnostics.
+  std::string m_name;
+
+  std::chrono::steady_clock::time_point m_setup_start_time;
+  std::chrono::steady_clock::time_point m_setup_stop_time;
+  std::chrono::duration<double> m_setup_duration{0.0};
+};
+
+/// Records the number of profiler measurements (start/stop pairs) and the
+/// average duration between each start and stop call.
+class SolveProfiler {
+ public:
+  /// Constructs a SolveProfiler.
+  ///
+  /// @param name Name of measurement to show in diagnostics.
+  explicit SolveProfiler(std::string_view name) : m_name{name} {}
+
+  /// Starts solve time measurement.
+  void start() {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    m_current_solve_start_time = std::chrono::steady_clock::now();
+#endif
+  }
+
+  /// Stops solve time measurement, increments the number of averages, and
+  /// incorporates the latest measurement into the average.
+  void stop() {
+#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
+    m_current_solve_stop_time = std::chrono::steady_clock::now();
+    m_current_solve_duration =
+        m_current_solve_stop_time - m_current_solve_start_time;
+    m_total_solve_duration += m_current_solve_duration;
+
+    ++m_num_solves;
+    m_average_solve_duration =
+        (m_num_solves - 1.0) / m_num_solves * m_average_solve_duration +
+        1.0 / m_num_solves * m_current_solve_duration;
+#endif
+  }
+
+  /// Returns name of measurement to show in diagnostics.
+  ///
+  /// @return Name of measurement to show in diagnostics.
+  std::string_view name() const { return m_name; }
+
+  /// Returns the number of solves.
+  ///
+  /// @return The number of solves.
+  int num_solves() const { return m_num_solves; }
+
+  /// Returns the most recent solve duration in seconds.
+  ///
+  /// @return The most recent solve duration in seconds.
+  const std::chrono::duration<double>& current_duration() const {
+    return m_current_solve_duration;
+  }
+
+  /// Returns the average solve duration in seconds.
+  ///
+  /// @return The average solve duration in seconds.
+  const std::chrono::duration<double>& average_duration() const {
+    return m_average_solve_duration;
+  }
+
+  /// Returns the sum of all solve durations in seconds.
+  ///
+  /// @return The sum of all solve durations in seconds.
+  const std::chrono::duration<double>& total_duration() const {
+    return m_total_solve_duration;
+  }
+
+ private:
+  /// Name of measurement to show in diagnostics.
+  std::string m_name;
+
+  std::chrono::steady_clock::time_point m_current_solve_start_time;
+  std::chrono::steady_clock::time_point m_current_solve_stop_time;
+  std::chrono::duration<double> m_current_solve_duration{0.0};
+  std::chrono::duration<double> m_total_solve_duration{0.0};
+
+  int m_num_solves = 0;
+  std::chrono::duration<double> m_average_solve_duration{0.0};
+};
+
+/// Starts a profiler in the constructor and stops it in the destructor.
+template <typename Profiler>
+  requires std::same_as<Profiler, SetupProfiler> ||
+           std::same_as<Profiler, SolveProfiler>
+class ScopedProfiler {
+ public:
+  /// Starts a profiler.
+  ///
+  /// @param profiler The profiler.
+  explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} {
+    m_profiler->start();
+  }
+
+  /// Stops a profiler.
+  ~ScopedProfiler() {
+    if (m_active) {
+      m_profiler->stop();
+    }
+  }
+
+  /// Move constructor.
+  ///
+  /// @param rhs The other ScopedProfiler.
+  ScopedProfiler(ScopedProfiler&& rhs) noexcept
+      : m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} {
+    rhs.m_active = false;
+  }
+
+  ScopedProfiler(const ScopedProfiler&) = delete;
+  ScopedProfiler& operator=(const ScopedProfiler&) = delete;
+
+  /// Stops the profiler.
+  ///
+  /// If this is called, the destructor is a no-op.
+  void stop() {
+    if (m_active) {
+      m_profiler->stop();
+      m_active = false;
+    }
+  }
+
+  /// Returns the most recent solve duration in milliseconds as a double.
+  ///
+  /// @return The most recent solve duration in milliseconds as a double.
+  const std::chrono::duration<double>& current_duration() const {
+    return m_profiler->current_duration();
+  }
+
+ private:
+  Profiler* m_profiler;
+  bool m_active = true;
+};
+
+}  // namespace slp
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/scoped_profiler.hpp
@@ -1,66 +0,0 @@
-// Copyright (c) Sleipnir contributors
-
-#pragma once
-
-#include <concepts>
-#include <utility>
-
-#include "sleipnir/util/setup_profiler.hpp"
-#include "sleipnir/util/solve_profiler.hpp"
-
-namespace slp {
-
-/// Starts a profiler in the constructor and stops it in the destructor.
-template <typename Profiler>
-  requires std::same_as<Profiler, SetupProfiler> ||
-           std::same_as<Profiler, SolveProfiler>
-class ScopedProfiler {
- public:
-  /// Starts a profiler.
-  ///
-  /// @param profiler The profiler.
-  explicit ScopedProfiler(Profiler& profiler) noexcept : m_profiler{&profiler} {
-    m_profiler->start();
-  }
-
-  /// Stops a profiler.
-  ~ScopedProfiler() {
-    if (m_active) {
-      m_profiler->stop();
-    }
-  }
-
-  /// Move constructor.
-  ///
-  /// @param rhs The other ScopedProfiler.
-  ScopedProfiler(ScopedProfiler&& rhs) noexcept
-      : m_profiler{std::move(rhs.m_profiler)}, m_active{rhs.m_active} {
-    rhs.m_active = false;
-  }
-
-  ScopedProfiler(const ScopedProfiler&) = delete;
-  ScopedProfiler& operator=(const ScopedProfiler&) = delete;
-
-  /// Stops the profiler.
-  ///
-  /// If this is called, the destructor is a no-op.
-  void stop() {
-    if (m_active) {
-      m_profiler->stop();
-      m_active = false;
-    }
-  }
-
-  /// Returns the most recent solve duration in milliseconds as a double.
-  ///
-  /// @return The most recent solve duration in milliseconds as a double.
-  const std::chrono::duration<double>& current_duration() const {
-    return m_profiler->current_duration();
-  }
-
- private:
-  Profiler* m_profiler;
-  bool m_active = true;
-};
-
-}  // namespace slp
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/setup_profiler.hpp
@@ -1,56 +0,0 @@
-// Copyright (c) Sleipnir contributors
-
-#pragma once
-
-#include <chrono>
-#include <string>
-#include <string_view>
-
-namespace slp {
-
-/// Records the number of profiler measurements (start/stop pairs) and the
-/// average duration between each start and stop call.
-class SetupProfiler {
- public:
-  /// Constructs a SetupProfiler.
-  ///
-  /// @param name Name of measurement to show in diagnostics.
-  explicit SetupProfiler(std::string_view name) : m_name{name} {}
-
-  /// Tell the profiler to start measuring setup time.
-  void start() {
-#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
-    m_setup_start_time = std::chrono::steady_clock::now();
-#endif
-  }
-
-  /// Tell the profiler to stop measuring setup time.
-  void stop() {
-#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
-    m_setup_stop_time = std::chrono::steady_clock::now();
-    m_setup_duration = m_setup_stop_time - m_setup_start_time;
-#endif
-  }
-
-  /// Returns name of measurement to show in diagnostics.
-  ///
-  /// @return Name of measurement to show in diagnostics.
-  std::string_view name() const { return m_name; }
-
-  /// Returns the setup duration in milliseconds as a double.
-  ///
-  /// @return The setup duration in milliseconds as a double.
-  const std::chrono::duration<double>& duration() const {
-    return m_setup_duration;
-  }
-
- private:
-  /// Name of measurement to show in diagnostics.
-  std::string m_name;
-
-  std::chrono::steady_clock::time_point m_setup_start_time;
-  std::chrono::steady_clock::time_point m_setup_stop_time;
-  std::chrono::duration<double> m_setup_duration{0.0};
-};
-
-}  // namespace slp
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/solve_profiler.hpp
@@ -1,87 +0,0 @@
-// Copyright (c) Sleipnir contributors
-
-#pragma once
-
-#include <chrono>
-#include <string>
-#include <string_view>
-
-namespace slp {
-
-/// Records the number of profiler measurements (start/stop pairs) and the
-/// average duration between each start and stop call.
-class SolveProfiler {
- public:
-  /// Constructs a SolveProfiler.
-  ///
-  /// @param name Name of measurement to show in diagnostics.
-  explicit SolveProfiler(std::string_view name) : m_name{name} {}
-
-  /// Tell the profiler to start measuring solve time.
-  void start() {
-#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
-    m_current_solve_start_time = std::chrono::steady_clock::now();
-#endif
-  }
-
-  /// Tell the profiler to stop measuring solve time, increment the number of
-  /// averages, and incorporate the latest measurement into the average.
-  void stop() {
-#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
-    m_current_solve_stop_time = std::chrono::steady_clock::now();
-    m_current_solve_duration =
-        m_current_solve_stop_time - m_current_solve_start_time;
-    m_total_solve_duration += m_current_solve_duration;
-
-    ++m_num_solves;
-    m_average_solve_duration =
-        (m_num_solves - 1.0) / m_num_solves * m_average_solve_duration +
-        1.0 / m_num_solves * m_current_solve_duration;
-#endif
-  }
-
-  /// Returns name of measurement to show in diagnostics.
-  ///
-  /// @return Name of measurement to show in diagnostics.
-  std::string_view name() const { return m_name; }
-
-  /// Returns the number of solves.
-  ///
-  /// @return The number of solves.
-  int num_solves() const { return m_num_solves; }
-
-  /// Returns the most recent solve duration in seconds.
-  ///
-  /// @return The most recent solve duration in seconds.
-  const std::chrono::duration<double>& current_duration() const {
-    return m_current_solve_duration;
-  }
-
-  /// Returns the average solve duration in seconds.
-  ///
-  /// @return The average solve duration in seconds.
-  const std::chrono::duration<double>& average_duration() const {
-    return m_average_solve_duration;
-  }
-
-  /// Returns the sum of all solve durations in seconds.
-  ///
-  /// @return The sum of all solve durations in seconds.
-  const std::chrono::duration<double>& total_duration() const {
-    return m_total_solve_duration;
-  }
-
- private:
-  /// Name of measurement to show in diagnostics.
-  std::string m_name;
-
-  std::chrono::steady_clock::time_point m_current_solve_start_time;
-  std::chrono::steady_clock::time_point m_current_solve_stop_time;
-  std::chrono::duration<double> m_current_solve_duration{0.0};
-  std::chrono::duration<double> m_total_solve_duration{0.0};
-
-  int m_num_solves = 0;
-  std::chrono::duration<double> m_average_solve_duration{0.0};
-};
-
-}  // namespace slp
--- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp
+++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/to_underlying.hpp
@@ -0,0 +1,14 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <type_traits>
+
+namespace slp {
+
+template <typename Enum>
+constexpr std::underlying_type_t<Enum> to_underlying(Enum e) noexcept {
+  return static_cast<std::underlying_type_t<Enum>>(e);
+}
+
+}  // namespace slp
--- a/wpimath/src/test/native/cpp/optimization/NonlinearProblemTest.cpp
+++ b/wpimath/src/test/native/cpp/optimization/NonlinearProblemTest.cpp
@@ -135,14 +135,7 @@ TEST(ProblemTest, Minimum2DDistanceWithLinearConstraint) {
  EXPECT_EQ(problem.equality_constraint_type(), slp::ExpressionType::LINEAR);
  EXPECT_EQ(problem.inequality_constraint_type(), slp::ExpressionType::NONE);

-#if defined(__linux__) && defined(__aarch64__)
-  // FIXME: Fails on Linux aarch64 with "line search failed"
-  EXPECT_EQ(problem.solve({.diagnostics = true}),
-            slp::ExitStatus::LINE_SEARCH_FAILED);
-  return;
-#else
  EXPECT_EQ(problem.solve({.diagnostics = true}), slp::ExitStatus::SUCCESS);
-#endif

  EXPECT_NEAR(x.value(), 2.5, 1e-2);
  EXPECT_NEAR(y.value(), 2.5, 1e-2);
@@ -208,13 +201,11 @@ TEST(ProblemTest, WachterAndBieglerLineSearchFailure) {
  EXPECT_EQ(problem.equality_constraint_type(), slp::ExpressionType::QUADRATIC);
  EXPECT_EQ(problem.inequality_constraint_type(), slp::ExpressionType::LINEAR);

-  // FIXME: Fails with "line search failed"
-  EXPECT_EQ(problem.solve({.diagnostics = true}),
-            slp::ExitStatus::LINE_SEARCH_FAILED);
+  EXPECT_EQ(problem.solve({.diagnostics = true}), slp::ExitStatus::SUCCESS);

-  // EXPECT_EQ(x.value(), 1.0);
-  // EXPECT_EQ(s1.value(), 0.0);
-  // EXPECT_EQ(s2.value(), 0.5);
+  EXPECT_NEAR(x.value(), 1.0, 1e-6);
+  EXPECT_NEAR(s1.value(), 0.0, 1e-6);
+  EXPECT_NEAR(s2.value(), 0.5, 1e-6);

  if (auto output = testing::internal::GetCapturedStdout(); HasFailure()) {
    fmt::println("{}", output);