From 5b4769ea0a557f919d7ffdc53ae08967ec090a8a Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 7 Apr 2026 23:52:30 -0700 Subject: [PATCH] [upstream_utils] Upgrade to Sleipnir 0.5.1 (#8726) There's changes to the diagnostic output and a performance improvement for autodiff setup. I also updated Java's Options docs to more closely match upstream. --- upstream_utils/sleipnir.py | 2 +- .../sleipnir_patches/0001-Use-fmtlib.patch | 2 +- .../0004-Replace-std-to_underlying.patch | 8 +- .../0005-Replace-std-views-zip.patch | 4 +- ...tead-of-multidimensional-array-subsc.patch | 2 +- .../math/optimization/solver/Options.java | 23 +-- .../include/sleipnir/optimization/problem.hpp | 146 ++++++++++++------ .../optimization/solver/interior_point.hpp | 36 ++--- .../sleipnir/optimization/solver/newton.hpp | 22 +-- .../sleipnir/optimization/solver/sqp.hpp | 32 ++-- .../solver/sqp_matrix_callbacks.hpp | 4 +- .../sleipnir/util/print_diagnostics.hpp | 30 ++-- 12 files changed, 184 insertions(+), 127 deletions(-) diff --git a/upstream_utils/sleipnir.py b/upstream_utils/sleipnir.py index b4bd6ddbc8..4b7287cb92 100755 --- a/upstream_utils/sleipnir.py +++ b/upstream_utils/sleipnir.py @@ -46,7 +46,7 @@ using small_vector = wpi::util::SmallVector; def main(): name = "sleipnir" url = "https://github.com/SleipnirGroup/Sleipnir" - tag = "v0.5.0" + tag = "v0.5.1" sleipnir = Lib(name, url, tag, copy_upstream_src) sleipnir.main() diff --git a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch index 4b8fc3c136..226de84d9e 100644 --- a/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch +++ b/upstream_utils/sleipnir_patches/0001-Use-fmtlib.patch @@ -66,7 +66,7 @@ index 12d0568f628d5b97c0c2f5291851d4b20921f9d3..d06d32dac6c7b6faeedefeaa107cedac + +// @endcond diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 42e572b6c13bba6d5f7592d2b7ffbe1879b1d99b..bb0bb21bb932f77f95c7779241784c4c54d6fe93 100644 +index a9553ffbcfed568c48f7d789d8b127790dfddb91..3de6d5bf89a65fe07784350c3b1a4691dfc0c822 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp @@ -15,6 +15,7 @@ diff --git a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch index 39ba8a6681..4411ecafa0 100644 --- a/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch +++ b/upstream_utils/sleipnir_patches/0004-Replace-std-to_underlying.patch @@ -11,7 +11,7 @@ Subject: [PATCH 04/10] Replace std::to_underlying() create mode 100644 include/sleipnir/util/to_underlying.hpp diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index bb0bb21bb932f77f95c7779241784c4c54d6fe93..61c5de59d8c5f4d582eff4f650c66c8d50e181a0 100644 +index 3de6d5bf89a65fe07784350c3b1a4691dfc0c822..273363db2cf75c84d81f2fef36dcb2094303ee2e 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp @@ -37,6 +37,7 @@ @@ -22,7 +22,7 @@ index bb0bb21bb932f77f95c7779241784c4c54d6fe93..61c5de59d8c5f4d582eff4f650c66c8d namespace slp { -@@ -696,11 +697,11 @@ class Problem { +@@ -752,11 +753,11 @@ class Problem { // Print problem structure slp::println("\nProblem structure:"); slp::println(" ↳ {} cost function", @@ -37,7 +37,7 @@ index bb0bb21bb932f77f95c7779241784c4c54d6fe93..61c5de59d8c5f4d582eff4f650c66c8d if (m_decision_variables.size() == 1) { slp::print("\n1 decision variable\n"); -@@ -712,7 +713,7 @@ class Problem { +@@ -768,7 +769,7 @@ class Problem { [](const gch::small_vector>& constraints) { std::array counts{}; for (const auto& constraint : constraints) { @@ -47,7 +47,7 @@ index bb0bb21bb932f77f95c7779241784c4c54d6fe93..61c5de59d8c5f4d582eff4f650c66c8d for (const auto& [count, name] : std::views::zip(counts, std::array{"empty", "constant", "linear", diff --git a/include/sleipnir/util/print_diagnostics.hpp b/include/sleipnir/util/print_diagnostics.hpp -index f44663c6d283e6b1658a330c79e39de0dc9b428e..698a8df43d4236bb6740e8cf7872d2e1f5df8c31 100644 +index e16ef1b03e675b7ccb8db6781568eec7a44aace7..69cb99957cc7da5abc4162c53a229f7f642bbe4b 100644 --- a/include/sleipnir/util/print_diagnostics.hpp +++ b/include/sleipnir/util/print_diagnostics.hpp @@ -17,6 +17,7 @@ diff --git a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch index 4559bea611..9ea42ad62c 100644 --- a/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch +++ b/upstream_utils/sleipnir_patches/0005-Replace-std-views-zip.patch @@ -25,10 +25,10 @@ index 275c30b76d34efe7ee1608cd4eedfa54ab2dc1ec..c0e3c161343175837abcb25fb22da0c6 if (col != -1 && node->adjoint != Scalar(0)) { triplets.emplace_back(row, col, node->adjoint); diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 61c5de59d8c5f4d582eff4f650c66c8d50e181a0..6acd2f655a4b35d087ff365c2a2917b015b03b32 100644 +index 273363db2cf75c84d81f2fef36dcb2094303ee2e..c4a45f7f954c9e028b1c85c45d5034bde284474f 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp -@@ -715,9 +715,11 @@ class Problem { +@@ -771,9 +771,11 @@ class Problem { for (const auto& constraint : constraints) { ++counts[slp::to_underlying(constraint.type())]; } diff --git a/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch b/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch index d0e46a8fe9..6035ba27b7 100644 --- a/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch +++ b/upstream_utils/sleipnir_patches/0010-Use-operator-instead-of-multidimensional-array-subsc.patch @@ -959,7 +959,7 @@ index 5e20b9b8debc13611d5d719b589d8fd896c35a90..e5163f9c2dbbfa0a580d029e61c741e6 if (m_dynamics_type == DynamicsType::EXPLICIT_ODE) { x_end = rk4, diff --git a/include/sleipnir/optimization/problem.hpp b/include/sleipnir/optimization/problem.hpp -index 6acd2f655a4b35d087ff365c2a2917b015b03b32..e05ef004e999787a923860b2d173702e3ca4045a 100644 +index c4a45f7f954c9e028b1c85c45d5034bde284474f..4e38a082855c07c47f1aa191613114a5a392a4e4 100644 --- a/include/sleipnir/optimization/problem.hpp +++ b/include/sleipnir/optimization/problem.hpp @@ -97,7 +97,7 @@ class Problem { diff --git a/wpimath/src/main/java/org/wpilib/math/optimization/solver/Options.java b/wpimath/src/main/java/org/wpilib/math/optimization/solver/Options.java index a83f099871..7b6f85d012 100644 --- a/wpimath/src/main/java/org/wpilib/math/optimization/solver/Options.java +++ b/wpimath/src/main/java/org/wpilib/math/optimization/solver/Options.java @@ -38,7 +38,7 @@ public class Options { public Options() {} /** - * Set tolerance. + * Sets the tolerance. * * @param tolerance The solver will stop once the error is below this tolerance. * @return This Options object. @@ -49,7 +49,7 @@ public class Options { } /** - * Set max iterations. + * Sets the max iterations. * * @param maxIterations The maximum number of solver iterations before returning a solution. * @return This Options object. @@ -60,7 +60,7 @@ public class Options { } /** - * Set timeout. + * Sets the timeout. * * @param timeout The maximum elapsed wall clock time in seconds before returning a solution. * @return This Options object. @@ -71,13 +71,14 @@ public class Options { } /** - * Enable or disable feasible IPM. + * Enables or disables the feasible interior-point method. * - * @param feasibleIPM 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. + *

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. + * + * @param feasibleIPM Enables or disables the feasible interior-point method. * @return This Options object. */ public Options withFeasibleIPM(boolean feasibleIPM) { @@ -86,9 +87,9 @@ public class Options { } /** - * Enable or disable diagnostics. + * Enables or disables diagnostic output. * - * @param diagnostics Enables diagnostic prints. + * @param diagnostics Enables or disables diagnostic output. * @return This Options object. */ public Options withDiagnostics(boolean diagnostics) { diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp index e05ef004e9..4e38a08285 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp @@ -307,25 +307,17 @@ class Problem { c_i_type <= ExpressionType::CONSTANT) { #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS if (options.diagnostics) { - slp::println("\nInvoking no-op solver...\n"); + slp::println("\nInvoking no-op solver\n"); } #endif return ExitStatus::SUCCESS; } - gch::small_vector ad_setup_profilers; - ad_setup_profilers.emplace_back("setup").start(); - VariableMatrix x_ad{m_decision_variables}; // Set up cost function Variable f = m_f.value_or(Scalar(0)); - // Set up gradient autodiff - ad_setup_profilers.emplace_back(" ↳ ∇f(x)").start(); - Gradient g{f, x_ad}; - ad_setup_profilers.back().stop(); - int num_decision_variables = m_decision_variables.size(); int num_equality_constraints = m_equality_constraints.size(); int num_inequality_constraints = m_inequality_constraints.size(); @@ -343,16 +335,32 @@ class Problem { ExitStatus status; if (m_equality_constraints.empty() && m_inequality_constraints.empty()) { if (options.diagnostics) { - slp::println("\nInvoking Newton solver...\n"); + slp::println("\nInvoking Newton solver\n"); } + gch::small_vector ad_setup_profilers; + ad_setup_profilers.emplace_back("setup"); + ad_setup_profilers.emplace_back("↳ ∇f(x)"); + ad_setup_profilers.emplace_back("↳ ∇²ₓₓL"); + + ad_setup_profilers[0].start(); + + // Set up gradient autodiff + ad_setup_profilers[1].start(); + Gradient g{f, x_ad}; + ad_setup_profilers[1].stop(); + // Set up Lagrangian Hessian autodiff - ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); + ad_setup_profilers[2].start(); Hessian H{f, x_ad}; - ad_setup_profilers.back().stop(); + ad_setup_profilers[2].stop(); ad_setup_profilers[0].stop(); + if (options.diagnostics) { + print_setup_diagnostics(ad_setup_profilers); + } + #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS // Sparsity pattern files written when spy flag is set std::unique_ptr> H_spy; @@ -393,24 +401,48 @@ class Problem { } VariableMatrix c_e_ad{m_equality_constraints}; + VariableMatrix y_ad(num_equality_constraints); + + gch::small_vector ad_setup_profilers; + ad_setup_profilers.emplace_back("setup"); + ad_setup_profilers.emplace_back("↳ ∇f(x)"); + ad_setup_profilers.emplace_back("↳ ∇²ₓₓL"); + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL_f"); + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL_c"); + ad_setup_profilers.emplace_back("↳ ∂cₑ/∂x"); + + ad_setup_profilers[0].start(); + + // Set up gradient autodiff + ad_setup_profilers[1].start(); + Gradient g{f, x_ad}; + ad_setup_profilers[1].stop(); + + ad_setup_profilers[2].start(); + + // Set up cost part of Lagrangian Hessian autodiff + ad_setup_profilers[3].start(); + Hessian H_f{f, x_ad}; + ad_setup_profilers[3].stop(); + + // Set up constraint part of Lagrangian Hessian autodiff + ad_setup_profilers[4].start(); + Hessian H_c{-y_ad.T() * c_e_ad, x_ad}; + ad_setup_profilers[4].stop(); + + ad_setup_profilers[2].stop(); // Set up equality constraint Jacobian autodiff - ad_setup_profilers.emplace_back(" ↳ ∂cₑ/∂x").start(); + ad_setup_profilers[5].start(); Jacobian A_e{c_e_ad, x_ad}; - ad_setup_profilers.back().stop(); - - // Set up Lagrangian - VariableMatrix y_ad(num_equality_constraints); - Variable L = f - y_ad.T() * c_e_ad; - - // Set up Lagrangian Hessian autodiff - ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); - Hessian H{L, x_ad}; - Hessian H_c{-y_ad.T() * c_e_ad, x_ad}; - ad_setup_profilers.back().stop(); + ad_setup_profilers[5].stop(); ad_setup_profilers[0].stop(); + if (options.diagnostics) { + print_setup_diagnostics(ad_setup_profilers); + } + #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS // Sparsity pattern files written when spy flag is set std::unique_ptr> H_spy; @@ -447,7 +479,7 @@ class Problem { [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { x_ad.set_value(x); y_ad.set_value(y); - return H.value(); + return H_f.value() + H_c.value(); }, [&](const DenseVector& x, const DenseVector& y) -> SparseMatrix { x_ad.set_value(x); @@ -467,36 +499,61 @@ class Problem { status = sqp(matrix_callbacks, iteration_callbacks, options, x); } else { if (options.diagnostics) { - slp::println("\nInvoking IPM solver...\n"); + slp::println("\nInvoking IPM solver\n"); } VariableMatrix c_e_ad{m_equality_constraints}; VariableMatrix c_i_ad{m_inequality_constraints}; - - // Set up equality constraint Jacobian autodiff - ad_setup_profilers.emplace_back(" ↳ ∂cₑ/∂x").start(); - Jacobian A_e{c_e_ad, x_ad}; - ad_setup_profilers.back().stop(); - - // Set up inequality constraint Jacobian autodiff - ad_setup_profilers.emplace_back(" ↳ ∂cᵢ/∂x").start(); - Jacobian A_i{c_i_ad, x_ad}; - ad_setup_profilers.back().stop(); - - // Set up Lagrangian VariableMatrix y_ad(num_equality_constraints); VariableMatrix z_ad(num_inequality_constraints); - Variable L = f - y_ad.T() * c_e_ad - z_ad.T() * c_i_ad; - // Set up Lagrangian Hessian autodiff - ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL").start(); - Hessian H{L, x_ad}; + gch::small_vector ad_setup_profilers; + ad_setup_profilers.emplace_back("setup"); + ad_setup_profilers.emplace_back("↳ ∇f(x)"); + ad_setup_profilers.emplace_back("↳ ∇²ₓₓL"); + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL_f"); + ad_setup_profilers.emplace_back(" ↳ ∇²ₓₓL_c"); + ad_setup_profilers.emplace_back("↳ ∂cₑ/∂x"); + ad_setup_profilers.emplace_back("↳ ∂cᵢ/∂x"); + + ad_setup_profilers[0].start(); + + // Set up gradient autodiff + ad_setup_profilers[1].start(); + Gradient g{f, x_ad}; + ad_setup_profilers[1].stop(); + + ad_setup_profilers[2].start(); + + // Set up cost part of Lagrangian Hessian autodiff + ad_setup_profilers[3].start(); + Hessian H_f{f, x_ad}; + ad_setup_profilers[3].stop(); + + // Set up constraint part of Lagrangian Hessian autodiff + ad_setup_profilers[4].start(); Hessian H_c{-y_ad.T() * c_e_ad - z_ad.T() * c_i_ad, x_ad}; - ad_setup_profilers.back().stop(); + ad_setup_profilers[4].stop(); + + ad_setup_profilers[2].stop(); + + // Set up equality constraint Jacobian autodiff + ad_setup_profilers[5].start(); + Jacobian A_e{c_e_ad, x_ad}; + ad_setup_profilers[5].stop(); + + // Set up inequality constraint Jacobian autodiff + ad_setup_profilers[6].start(); + Jacobian A_i{c_i_ad, x_ad}; + ad_setup_profilers[6].stop(); ad_setup_profilers[0].stop(); + if (options.diagnostics) { + print_setup_diagnostics(ad_setup_profilers); + } + #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS // Sparsity pattern files written when spy flag is set std::unique_ptr> H_spy; @@ -557,7 +614,7 @@ class Problem { x_ad.set_value(x); y_ad.set_value(y); z_ad.set_value(z); - return H.value(); + return H_f.value() + H_c.value(); }, [&](const DenseVector& x, const DenseVector& y, const DenseVector& z) -> SparseMatrix { @@ -593,7 +650,6 @@ class Problem { } if (options.diagnostics) { - print_autodiff_diagnostics(ad_setup_profilers); slp::println("\nExit: {}", status); } diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp index 9f9cdc2560..034edc1691 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp @@ -153,24 +153,24 @@ ExitStatus interior_point( gch::small_vector solve_profilers; solve_profilers.emplace_back("solver"); - solve_profilers.emplace_back(" ↳ setup"); - solve_profilers.emplace_back(" ↳ iteration"); - solve_profilers.emplace_back(" ↳ feasibility ✓"); - solve_profilers.emplace_back(" ↳ iter callbacks"); - solve_profilers.emplace_back(" ↳ KKT matrix build"); - solve_profilers.emplace_back(" ↳ KKT matrix decomp"); - solve_profilers.emplace_back(" ↳ KKT system solve"); - solve_profilers.emplace_back(" ↳ line search"); - solve_profilers.emplace_back(" ↳ SOC"); - solve_profilers.emplace_back(" ↳ next iter prep"); - solve_profilers.emplace_back(" ↳ 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)"); - solve_profilers.emplace_back(" ↳ ∂cᵢ/∂x"); + solve_profilers.emplace_back("↳ setup"); + solve_profilers.emplace_back("↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility check"); + solve_profilers.emplace_back(" ↳ callbacks"); + solve_profilers.emplace_back(" ↳ KKT matrix build"); + solve_profilers.emplace_back(" ↳ KKT matrix decomp"); + solve_profilers.emplace_back(" ↳ KKT system solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ SOC"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL_c"); + solve_profilers.emplace_back(" ↳ cₑ(x)"); + solve_profilers.emplace_back(" ↳ ∂cₑ/∂x"); + solve_profilers.emplace_back(" ↳ cᵢ(x)"); + solve_profilers.emplace_back(" ↳ ∂cᵢ/∂x"); auto& solver_prof = solve_profilers[0]; auto& setup_prof = solve_profilers[1]; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp index 875a46ce9e..31acf4fe33 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp @@ -64,17 +64,17 @@ ExitStatus newton( gch::small_vector solve_profilers; solve_profilers.emplace_back("solver"); - solve_profilers.emplace_back(" ↳ setup"); - solve_profilers.emplace_back(" ↳ iteration"); - solve_profilers.emplace_back(" ↳ feasibility ✓"); - solve_profilers.emplace_back(" ↳ iter callbacks"); - solve_profilers.emplace_back(" ↳ KKT matrix decomp"); - solve_profilers.emplace_back(" ↳ KKT system solve"); - solve_profilers.emplace_back(" ↳ line search"); - solve_profilers.emplace_back(" ↳ next iter prep"); - solve_profilers.emplace_back(" ↳ f(x)"); - solve_profilers.emplace_back(" ↳ ∇f(x)"); - solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back("↳ setup"); + solve_profilers.emplace_back("↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility check"); + solve_profilers.emplace_back(" ↳ callbacks"); + solve_profilers.emplace_back(" ↳ KKT matrix decomp"); + solve_profilers.emplace_back(" ↳ KKT system solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); auto& solver_prof = solve_profilers[0]; auto& setup_prof = solve_profilers[1]; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp index 335245136b..3a48e2e59a 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp @@ -112,22 +112,22 @@ ExitStatus sqp(const SQPMatrixCallbacks& matrix_callbacks, gch::small_vector solve_profilers; solve_profilers.emplace_back("solver"); - solve_profilers.emplace_back(" ↳ setup"); - solve_profilers.emplace_back(" ↳ iteration"); - solve_profilers.emplace_back(" ↳ feasibility ✓"); - solve_profilers.emplace_back(" ↳ iter callbacks"); - solve_profilers.emplace_back(" ↳ KKT matrix build"); - solve_profilers.emplace_back(" ↳ KKT matrix decomp"); - solve_profilers.emplace_back(" ↳ KKT system solve"); - solve_profilers.emplace_back(" ↳ line search"); - solve_profilers.emplace_back(" ↳ SOC"); - solve_profilers.emplace_back(" ↳ next iter prep"); - solve_profilers.emplace_back(" ↳ 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"); + solve_profilers.emplace_back("↳ setup"); + solve_profilers.emplace_back("↳ iteration"); + solve_profilers.emplace_back(" ↳ feasibility check"); + solve_profilers.emplace_back(" ↳ callbacks"); + solve_profilers.emplace_back(" ↳ KKT matrix build"); + solve_profilers.emplace_back(" ↳ KKT matrix decomp"); + solve_profilers.emplace_back(" ↳ KKT system solve"); + solve_profilers.emplace_back(" ↳ line search"); + solve_profilers.emplace_back(" ↳ SOC"); + solve_profilers.emplace_back(" ↳ next iter prep"); + solve_profilers.emplace_back(" ↳ f(x)"); + solve_profilers.emplace_back(" ↳ ∇f(x)"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL"); + solve_profilers.emplace_back(" ↳ ∇²ₓₓL_c"); + solve_profilers.emplace_back(" ↳ cₑ(x)"); + solve_profilers.emplace_back(" ↳ ∂cₑ/∂x"); auto& solver_prof = solve_profilers[0]; auto& setup_prof = solve_profilers[1]; diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp index ee44c36d67..de2f2ed884 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp @@ -97,7 +97,7 @@ struct SQPMatrixCallbacks { /// std::function H; - /// Constraint part of Lagrangian Hessian ∇ₓₓ² −yᵀcₑ(x) getter. + /// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x)) getter. /// /// /// @@ -116,7 +116,7 @@ struct SQPMatrixCallbacks { /// /// /// - /// + /// /// /// /// diff --git a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp index 698a8df43d..69cb99957c 100644 --- a/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp +++ b/wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp @@ -297,55 +297,55 @@ inline void print_solver_diagnostics( const gch::small_vector& solve_profilers) { auto solve_duration = to_ms(solve_profilers[0].total_duration()); - slp::println("┏{:━^23}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); - slp::println("┃{:^23}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent", + slp::println("┏{:━^21}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); + slp::println("┃{:^21}│{:^18}│{:^10}│{:^9}│{:^4}┃", "solver trace", "percent", "total (ms)", "each (ms)", "runs"); - slp::println("┡{:━^23}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); + slp::println("┡{:━^21}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); for (auto& profiler : solve_profilers) { double norm = solve_duration == 0.0 ? (&profiler == &solve_profilers[0] ? 1.0 : 0.0) : to_ms(profiler.total_duration()) / solve_duration; - slp::println("│{:<23} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", + slp::println("│{:<21} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", profiler.name(), norm * 100.0, histogram<9>(norm), to_ms(profiler.total_duration()), to_ms(profiler.average_duration()), profiler.num_solves()); } - slp::println("└{:─^68}┘", ""); + slp::println("└{:─^66}┘", ""); } #else #define print_solver_diagnostics(...) #endif #ifndef SLEIPNIR_DISABLE_DIAGNOSTICS -/// Prints autodiff diagnostics. +/// Prints setup diagnostics. /// -/// @param setup_profilers Autodiff setup profilers. -inline void print_autodiff_diagnostics( +/// @param setup_profilers Setup profilers. +inline void print_setup_diagnostics( const gch::small_vector& setup_profilers) { auto setup_duration = to_ms(setup_profilers[0].duration()); // Print heading - slp::println("┏{:━^23}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); - slp::println("┃{:^23}│{:^18}│{:^10}│{:^9}│{:^4}┃", "autodiff trace", - "percent", "total (ms)", "each (ms)", "runs"); - slp::println("┡{:━^23}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); + slp::println("┏{:━^21}┯{:━^18}┯{:━^10}┯{:━^9}┯{:━^4}┓", "", "", "", "", ""); + slp::println("┃{:^21}│{:^18}│{:^10}│{:^9}│{:^4}┃", "setup trace", "percent", + "total (ms)", "each (ms)", "runs"); + slp::println("┡{:━^21}┷{:━^18}┷{:━^10}┷{:━^9}┷{:━^4}┩", "", "", "", "", ""); // Print setup profilers for (auto& profiler : setup_profilers) { double norm = setup_duration == 0.0 ? (&profiler == &setup_profilers[0] ? 1.0 : 0.0) : to_ms(profiler.duration()) / setup_duration; - slp::println("│{:<23} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", + slp::println("│{:<21} {:>6.2f}%▕{}▏ {:>10.3f} {:>9.3f} {:>4}│", profiler.name(), norm * 100.0, histogram<9>(norm), to_ms(profiler.duration()), to_ms(profiler.duration()), "1"); } - slp::println("└{:─^68}┘", ""); + slp::println("└{:─^66}┘", ""); } #else -#define print_autodiff_diagnostics(...) +#define print_setup_diagnostics(...) #endif } // namespace slp
1
∇ₓₓ² −yᵀcₑ(x)∇ₓₓ²(−yᵀcₑ(x))num_decision_variablesnum_decision_variables