sleipnirgroup-jormungandr 0.6.7.dev31__tar.gz → 0.6.7.dev44__tar.gz

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/CMakeLists.txt

@@ -206,17 +206,71 @@ if(PROJECT_IS_TOP_LEVEL AND BUILD_TESTING AND NOT CMAKE_CROSSCOMPILING)
     list(APPEND CMAKE_MODULE_PATH ${Catch2_SOURCE_DIR}/extras)
     include(Catch)
 
-    # Build Sleipnir tests
-    file(GLOB_RECURSE sleipnir_test_src test/src/*.cpp)
-    add_executable(sleipnir_test ${sleipnir_test_src})
-    compiler_flags(sleipnir_test)
+    # Build autodiff explicit instantiations shared by autodiff and
+    # optimization tests
+    file(
+        GLOB_RECURSE sleipnir_autodiff_explicit_double_src
+        test/src/autodiff/*_explicit_double.cpp
+    )
+    add_library(
+        sleipnir_autodiff_explicit_double
+        OBJECT
+        ${sleipnir_autodiff_explicit_double_src}
+    )
+    compiler_flags(sleipnir_autodiff_explicit_double)
+    target_include_directories(
+        sleipnir_autodiff_explicit_double
+        PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test/include
+    )
+    target_link_libraries(sleipnir_autodiff_explicit_double PUBLIC Sleipnir)
+
+    # Build Sleipnir autodiff tests
+    file(GLOB_RECURSE sleipnir_autodiff_test_src test/src/autodiff/*_test.cpp)
+    add_executable(sleipnir_autodiff_test ${sleipnir_autodiff_test_src})
+    compiler_flags(sleipnir_autodiff_test)
+    target_include_directories(
+        sleipnir_autodiff_test
+        PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test/include
+    )
+    target_link_libraries(
+        sleipnir_autodiff_test
+        PUBLIC Sleipnir sleipnir_autodiff_explicit_double Catch2::Catch2WithMain
+    )
+
+    # Build Sleipnir optimization tests
+    file(
+        GLOB_RECURSE sleipnir_optimization_test_src
+        test/src/optimization/*_explicit_double.cpp
+        test/src/optimization/*_test.cpp
+    )
+    add_executable(sleipnir_optimization_test ${sleipnir_optimization_test_src})
+    compiler_flags(sleipnir_optimization_test)
     target_include_directories(
-        sleipnir_test
+        sleipnir_optimization_test
         PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test/include
     )
-    target_link_libraries(sleipnir_test PUBLIC Sleipnir Catch2::Catch2WithMain)
+    target_link_libraries(
+        sleipnir_optimization_test
+        PUBLIC Sleipnir sleipnir_autodiff_explicit_double Catch2::Catch2WithMain
+    )
+
+    # Build Sleipnir util tests
+    file(GLOB_RECURSE sleipnir_util_test_src test/src/util/*_test.cpp)
+    add_executable(sleipnir_util_test ${sleipnir_util_test_src})
+    compiler_flags(sleipnir_util_test)
+    target_include_directories(
+        sleipnir_util_test
+        PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/test/include
+    )
+    target_link_libraries(
+        sleipnir_util_test
+        PUBLIC Sleipnir Catch2::Catch2WithMain
+    )
+
     if(NOT CMAKE_TOOLCHAIN_FILE)
-        catch_discover_tests(sleipnir_test)
+        catch_discover_tests(sleipnir_autodiff_test)
+        catch_discover_tests(sleipnir_optimization_test)
+        catch_discover_tests(sleipnir_util_test)
     endif()
 endif()
 

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: sleipnirgroup-jormungandr
-Version: 0.6.7.dev31
+Version: 0.6.7.dev44
 Summary: Reverse mode autodiff library and NLP solver DSL
 License-Expression: BSD-3-Clause
 License-File: LICENSE.txt

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/include/sleipnir/optimization/solver/ipm.hpp

@@ -353,6 +353,8 @@ ExitStatus ipm(const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
 
   int full_step_rejected_counter = 0;
 
+  StallDetector<Scalar> stall_detector;
+
   // Error
   Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
       matrices.scaling, g, A_e, c_e, A_i, c_i, s, y, z, Scalar(0));
@@ -504,7 +506,7 @@ ExitStatus ipm(const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
       } else {
         trial_s = s + α * step.p_s;
       }
-      trial_y = y + α_z * step.p_y;
+      trial_y = y + α * step.p_y;
       trial_z = z + α_z * step.p_z;
 
       trial_f = matrices.f(trial_x);
@@ -608,7 +610,7 @@ ExitStatus ipm(const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
 
           trial_x = x + α_soc * soc_step.p_x;
           trial_s = s + α_soc * soc_step.p_s;
-          trial_y = y + α_z_soc * soc_step.p_y;
+          trial_y = y + α_soc * soc_step.p_y;
           trial_z = z + α_z_soc * soc_step.p_z;
 
           trial_f = matrices.f(trial_x);
@@ -677,7 +679,7 @@ ExitStatus ipm(const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
 
         trial_x = x + α_max * step.p_x;
         trial_s = s + α_max * step.p_s;
-        trial_y = y + α_z * step.p_y;
+        trial_y = y + α_max * step.p_y;
         trial_z = z + α_z * step.p_z;
 
         trial_f = matrices.f(trial_x);
@@ -691,6 +693,7 @@ ExitStatus ipm(const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
         // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
         if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
           // Accept step
+          α = α_max;
           break;
         }
 
@@ -701,6 +704,15 @@ ExitStatus ipm(const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
 
     line_search_profiler.stop();
 
+    // If solver stalled, invoke feasibility restoration
+    if (!call_feasibility_restoration && !in_feasibility_restoration) {
+      FilterEntry trial_entry{trial_f, trial_s, trial_c_e, trial_c_i, μ};
+      if (stall_detector.update(current_entry, trial_entry, α,
+                                filter.min_constraint_violation)) {
+        call_feasibility_restoration = true;
+      }
+    }
+
     if (call_feasibility_restoration) {
       ScopedProfiler feasibility_restoration_profiler{
           feasibility_restoration_prof};
@@ -765,30 +777,30 @@ ExitStatus ipm(const IPMMatrixCallbacks<Scalar>& matrix_callbacks,
       y = trial_y;
       z = trial_z;
 
-      // A requirement for the convergence proof is that the primal-dual barrier
-      // term Hessian Σₖ₊₁ does not deviate arbitrarily much from the primal
-      // 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]);
-      }
-
       f = trial_f;
       c_e = trial_c_e;
       c_i = trial_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]);
+    }
+
     // Update autodiff for Jacobians and Hessian
     A_e = matrices.A_e(x);
     A_i = matrices.A_i(x);

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/include/sleipnir/optimization/solver/newton.hpp

@@ -232,14 +232,15 @@ ExitStatus newton(
 
         trial_x = x + α_max * p_x;
 
+        trial_f = matrices.f(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) {
-          trial_f = matrices.f(trial_x);
-
           // Accept step
+          α = α_max;
           break;
         }
 

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/include/sleipnir/optimization/solver/sqp.hpp

@@ -236,6 +236,8 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
 
   int full_step_rejected_counter = 0;
 
+  StallDetector<Scalar> stall_detector;
+
   // Error
   Scalar E_0 = unscaled_kkt_error<Scalar, KKTErrorType::INF_NORM_SCALED>(
       matrices.scaling, g, A_e, c_e, y);
@@ -488,6 +490,7 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
         // If the step using αᵐᵃˣ reduced the KKT error, accept it anyway
         if (next_kkt_error <= Scalar(0.999) * current_kkt_error) {
           // Accept step
+          α = α_max;
           break;
         }
 
@@ -498,6 +501,15 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
 
     line_search_profiler.stop();
 
+    // If solver stalled, invoke feasibility restoration
+    if (!call_feasibility_restoration) {
+      FilterEntry trial_entry{trial_f, trial_c_e};
+      if (stall_detector.update(current_entry, trial_entry, α,
+                                filter.min_constraint_violation)) {
+        call_feasibility_restoration = true;
+      }
+    }
+
     if (call_feasibility_restoration) {
       ScopedProfiler feasibility_restoration_profiler{
           feasibility_restoration_prof};

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/include/sleipnir/optimization/solver/util/bounds.hpp

@@ -225,7 +225,7 @@ void project_onto_bounds(
     // We assume that bound infeasibility is handled elsewhere.
     slp_assert(lower <= upper);
 
-    // See appendix B.2 of [4] and section 3.6 of [2]
+    // See section 3.6 of [2]
     if (isfinite(lower) && isfinite(upper)) {
       auto p_L = std::min(κ_1 * std::max(Scalar(1), abs(lower)),
                           κ_2 * (upper - lower));

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/include/sleipnir/optimization/solver/util/feasibility_restoration.hpp

@@ -19,7 +19,6 @@
 #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"
 #include "sleipnir/optimization/solver/util/problem_scaling.hpp"
 #include "sleipnir/util/print_diagnostics.hpp"
 
@@ -317,7 +316,9 @@ ExitStatus feasibility_restoration(
     auto g = matrices.g(x);
     auto A_e = matrices.A_e(x);
 
-    y = lagrange_multiplier_estimate(g, A_e);
+    // Reset the equality constraint duals since feasibility restoration didn't
+    // account for the cost function
+    y.setZero();
 
     return ExitStatus::SUCCESS;
   } else if (status == ExitStatus::SUCCESS) {
@@ -634,17 +635,39 @@ ExitStatus feasibility_restoration(
                   fr_x, fr_s, fr_y, fr_z, fr_μ, iterations);
 
   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;
+    const DenseVector s_old = s;
+
+    // Re-center the slacks on the original constraints cᵢ(x) if they're
+    // sufficiently feasible, and on 1 otherwise.
+    //
+    // We can't use the slacks of cᵢ(x) − pᵢ + nᵢ ≥ 0 because feasibility
+    // restoration's barrier parameter fr_μ drives them toward 0 independently
+    // of the original problem's barrier parameter μ. Slacks much smaller than μ
+    // make Σ = S⁻¹Z and μS⁻¹e very large, which causes very large dual steps
+    // after returning.
+    const DenseVector c_i = matrices.c_i(x);
+    s = (c_i.array() >= Scalar(1e-2)).select(c_i, Scalar(1));
+
+    // Reset the equality constraint duals since feasibility restoration didn't
+    // account for the cost function
+    y.setZero();
+
+    // Update the inequality constraint duals with a Newton step for
+    // complementarity, taking the change in slacks during the restoration
+    // phase as the primal step.
+    //
+    //   pᶻ = μS⁻¹e − z − Σpˢ
+    //
+    // If the result isn't strictly positive or is too large, reset all of them
+    // to 1.
+    const DenseVector Σ = z.cwiseQuotient(s_old);
+    const DenseVector p_s = s - s_old;
+    z += μ * s_old.cwiseInverse() - z - Σ.cwiseProduct(p_s);
+    if ((z.array() <= Scalar(0)).any() || (z.array() > Scalar(1e3)).any()) {
+      z.setOnes();
+    }
 
     return ExitStatus::SUCCESS;
   } else if (status == ExitStatus::SUCCESS) {
@@ -654,6 +677,9 @@ ExitStatus feasibility_restoration(
     // infeasibility anywhere else risks false positives (e.g., a
     // point-in-time test can reject iterates the solver would otherwise
     // escape). See section 3.3, p. 14 of [2].
+
+    s = fr_s.segment(0, s.rows());
+
     DenseVector c_e = matrices.c_e(x);
     if (matrices.scaling.c_e.size() > 0) {
       c_e = matrices.scaling.c_e.cwiseInverse().cwiseProduct(c_e);
@@ -688,6 +714,7 @@ ExitStatus feasibility_restoration(
 
     return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
   } else {
+    s = fr_s.segment(0, s.rows());
     return ExitStatus::FEASIBILITY_RESTORATION_FAILED;
   }
 }

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/include/sleipnir/optimization/solver/util/filter.hpp

@@ -130,15 +130,11 @@ class Filter {
         trial_entry.cost <= current_entry.cost + η_ϕ * α * D_ϕ;
 
     // Sufficient decrease condition
-    //
-    // See equation (2.13) of [3].
-    Scalar ϕ = pow(α, Scalar(1.5));
     bool sufficient_decrease =
         trial_entry.cost <=
-            current_entry.cost -
-                ϕ * γ_cost * current_entry.constraint_violation ||
+            current_entry.cost - γ_cost * current_entry.constraint_violation ||
         trial_entry.constraint_violation <=
-            (Scalar(1) - ϕ * γ_constraint) * current_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
@@ -164,8 +160,8 @@ class Filter {
     // 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});
+          current_entry.cost - γ_cost * current_entry.constraint_violation,
+          (Scalar(1) - γ_constraint) * current_entry.constraint_violation});
     }
 
     return true;
@@ -211,4 +207,47 @@ class Filter {
   }
 };
 
+/// Detects a solver stall.
+///
+/// @tparam Scalar Scalar type.
+template <typename Scalar>
+class StallDetector {
+ public:
+  /// Returns true if the solver stalled.
+  ///
+  /// @param current_entry The entry corresponding to the current iterate.
+  /// @param trial_entry The entry corresponding to the accepted trial iterate.
+  /// @param α The accepted step size.
+  /// @param min_constraint_violation The minimum constraint violation from the
+  ///     filter.
+  /// @return True if the solver stalled.
+  bool update(const FilterEntry<Scalar>& current_entry,
+              const FilterEntry<Scalar>& trial_entry, Scalar α,
+              Scalar min_constraint_violation) {
+    // If step is tiny, current iterate is infeasible, and accepted trial
+    // iterate made insufficient progress toward feasibility, increment stalled
+    // step counter
+    if (α < Scalar(1e-3) &&
+        current_entry.constraint_violation > min_constraint_violation &&
+        trial_entry.constraint_violation >
+            Scalar(0.99) * current_entry.constraint_violation) {
+      ++m_stalled_step_counter;
+
+      // If there were too many consecutive tiny steps, report a stall
+      if (m_stalled_step_counter >= 5) {
+        m_stalled_step_counter = 0;
+        return true;
+      }
+    } else {
+      // A break in stalled steps resets the counter
+      m_stalled_step_counter = 0;
+    }
+
+    return false;
+  }
+
+ private:
+  int m_stalled_step_counter = 0;
+};
+
 }  // namespace slp

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/python/src/sleipnir/__init__.py

@@ -2,4 +2,4 @@
 
 from ._sleipnir import *
 
-__version__ = "0.6.7.dev31"
+__version__ = "0.6.7.dev44"

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/python/test/optimization/cart_pole_problem_test.py

@@ -15,9 +15,11 @@ def lerp(a, b, t):
     return a + t * (b - a)
 
 
-def test_cart_pole_problem():
+# dt = 0.25 s is sensitive to small solver changes, so a failure here may just
+# mean the iterates found a different basin rather than a regression.
+@pytest.mark.parametrize("dt", [0.05, 0.125, 0.25])
+def test_cart_pole_problem(dt):
     TOTAL_TIME = 5.0  # s
-    dt = 0.05  # s
     N = int(TOTAL_TIME / dt)
 
     u_max = 20.0  # N

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/python/test/optimization/flywheel_ocp_test.py

@@ -96,7 +96,7 @@ def flywheel_test(
                 # steady-states.
                 assert problem.U().value(0, k) == pytest.approx(u, abs=2.0)
             else:
-                assert problem.U().value(0, k) == pytest.approx(u, abs=1e-4)
+                assert problem.U().value(0, k) == pytest.approx(u, abs=2e-4)
 
         # Project state forward
         x = A_discrete * x + B_discrete * u

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/python/test/optimization/flywheel_problem_test.py

@@ -83,7 +83,7 @@ def test_flywheel_problem():
             assert U.value(0, k) >= u_ss
             assert U.value(0, k) <= 12.0
         else:
-            assert U.value(0, k) == pytest.approx(u, abs=1e-4)
+            assert U.value(0, k) == pytest.approx(u, abs=2e-4)
 
         # Project state forward
         x = A * x + B * u

{sleipnirgroup_jormungandr-0.6.7.dev31 → sleipnirgroup_jormungandr-0.6.7.dev44}/python/test/optimization/nonlinear_problem_test.py

@@ -160,3 +160,35 @@ def test_wachter_and_biegler_line_search_failure():
         assert x.value() == pytest.approx(-1.0, abs=1e-6)
         assert s1.value() == pytest.approx(0.0, abs=1e-6)
         assert s2.value() == pytest.approx(0.0, abs=3e-5)
+
+
+def test_hock_schittkowski_13():
+    # Problem 13 of [1] (see
+    # https://vanderbei.princeton.edu/ampl/nlmodels/cute/hs013.mod) has
+    # constraint gradients that are linearly dependent at the solution.
+    #
+    # [1] Hock, W. and Schittkowski, K. "Test Examples for Nonlinear Programming
+    #     Codes", Springer, 1981.
+
+    problem = Problem()
+
+    x, y = problem.decision_variable(2)
+    x.set_value(-2)
+    y.set_value(-2)
+
+    problem.minimize((x - 2) ** 2 + y**2)
+
+    problem.subject_to((1 - x) ** 3 >= y)
+    problem.subject_to(x >= 0)
+    problem.subject_to(y >= 0)
+
+    assert problem.cost_function_type() == ExpressionType.QUADRATIC
+    assert problem.equality_constraint_type() == ExpressionType.NONE
+    assert problem.inequality_constraint_type() == ExpressionType.NONLINEAR
+
+    assert problem.solve(diagnostics=True) == ExitStatus.SUCCESS
+
+    # The solution has no finite Lagrange multipliers, so the iterates only
+    # approach it to within the square root of the tolerance
+    assert x.value() == pytest.approx(1.0, abs=1e-4)
+    assert y.value() == pytest.approx(0.0, abs=1e-4)

sleipnirgroup_jormungandr-0.6.7.dev31/include/sleipnir/optimization/solver/util/lagrange_multiplier_estimate.hpp

@@ -1,133 +0,0 @@
-// Copyright (c) Sleipnir contributors
-
-#pragma once
-
-#include <algorithm>
-#include <utility>
-
-#include <Eigen/Core>
-#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 Cost function gradient ∇f.
-/// @param A_e Equality constraint Jacobian Aₑ(x).
-template <typename Scalar>
-Eigen::Vector<Scalar, Eigen::Dynamic> lagrange_multiplier_estimate(
-    const Eigen::SparseVector<Scalar>& g,
-    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 Cost function gradient ∇f.
-/// @param A_e Equality constraint Jacobian Aₑ(x).
-/// @param A_i Inequality constraint Jacobian Aᵢ(x).
-/// @param s Inequality constraint slack variables.
-/// @param μ Barrier parameter.
-template <typename Scalar>
-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ₑ   0]
-  //         [Aᵢ  −S]
-  //
-  //   Âᵀ[y] = [ ∇f]
-  //     [z]   [−μe]
-  //
-  //   [y] = (ÂÂᵀ)⁻¹Â[ ∇f]
-  //   [z]           [−μe]
-
-  gch::small_vector<Eigen::Triplet<Scalar>> triplets;
-
-  // Â = [Aₑ   0]
-  //     [Aᵢ  −S]
-  triplets.reserve(A_e.nonZeros() + A_i.nonZeros() + s.rows());
-  append_as_triplets(triplets, 0, 0, {A_e, A_i});
-  append_diagonal_as_triplets(triplets, A_e.rows(), A_i.cols(), (-s).eval());
-  SparseMatrix A_hat{A_e.rows() + A_i.rows(), A_e.cols() + s.rows()};
-  A_hat.setFromSortedTriplets(triplets.begin(), triplets.end());
-
-  // lhs = ÂÂᵀ
-  SparseMatrix lhs = A_hat * A_hat.transpose();
-
-  // rhs = Â[ ∇f]
-  //        [−μe]
-  DenseVector rhs_temp{g.rows() + s.rows()};
-  rhs_temp.segment(0, g.rows()) = g;
-  rhs_temp.segment(g.rows(), s.rows()).setConstant(-μ);
-  DenseVector rhs = A_hat * rhs_temp;
-
-  Eigen::SimplicialLDLT<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