Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[upstream_utils] Upgrade to Sleipnir 0.5.1 (#8726)

Browse Source 
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.
This commit is contained in:
Tyler Veness
2026-04-07 23:52:30 -07:00
committed by GitHub
parent 719e9dddc0
commit 5b4769ea0a
12 changed files with 184 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
wpimath/src/main/java/org/wpilib/math/optimization/solver/Options.java
View File

@@ -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.
* <p>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) {

146
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/problem.hpp vendored
View File

@@ -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<SetupProfiler> ad_setup_profilers;
ad_setup_profilers.emplace_back("setup").start();
VariableMatrix<Scalar> 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<SetupProfiler> 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<Scalar, Eigen::Lower> 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<Spy<Scalar>> H_spy;
@@ -393,24 +401,48 @@ class Problem {
}
VariableMatrix<Scalar> c_e_ad{m_equality_constraints};
VariableMatrix<Scalar> y_ad(num_equality_constraints);
gch::small_vector<SetupProfiler> 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<Scalar, Eigen::Lower> H_f{f, x_ad};
ad_setup_profilers[3].stop();
// Set up constraint part of Lagrangian Hessian autodiff
ad_setup_profilers[4].start();
Hessian<Scalar, Eigen::Lower> 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<Scalar> 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<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[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<Spy<Scalar>> 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<Scalar>(matrix_callbacks, iteration_callbacks, options, x);
} else {
if (options.diagnostics) {
slp::println("\nInvoking IPM solver...\n");
slp::println("\nInvoking IPM solver\n");
}
VariableMatrix<Scalar> c_e_ad{m_equality_constraints};
VariableMatrix<Scalar> 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<Scalar> y_ad(num_equality_constraints);
VariableMatrix<Scalar> 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<Scalar, Eigen::Lower> H{L, x_ad};
gch::small_vector<SetupProfiler> 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<Scalar, Eigen::Lower> H_f{f, x_ad};
ad_setup_profilers[3].stop();
// Set up constraint part of Lagrangian Hessian autodiff
ad_setup_profilers[4].start();
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[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<Spy<Scalar>> 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);
}

36
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/interior_point.hpp vendored
View File

@@ -153,24 +153,24 @@ ExitStatus interior_point(
gch::small_vector<SolveProfiler> 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];

22
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/newton.hpp vendored
View File

@@ -64,17 +64,17 @@ ExitStatus newton(
gch::small_vector<SolveProfiler> 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];

32
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp.hpp vendored
View File

@@ -112,22 +112,22 @@ ExitStatus sqp(const SQPMatrixCallbacks<Scalar>& matrix_callbacks,
gch::small_vector<SolveProfiler> 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];

4
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/optimization/solver/sqp_matrix_callbacks.hpp vendored
View File

@@ -97,7 +97,7 @@ struct SQPMatrixCallbacks {
/// </table>
std::function<SparseMatrix(const DenseVector& x, const DenseVector& y)> H;
/// Constraint part of Lagrangian Hessian ∇ₓₓ² −yᵀcₑ(x) getter.
/// Constraint part of Lagrangian Hessian ∇ₓₓ²(−yᵀcₑ(x)) getter.
///
/// <table>
/// <tr>
@@ -116,7 +116,7 @@ struct SQPMatrixCallbacks {
/// <td>1</td>
/// </tr>
/// <tr>
/// <td>∇ₓₓ² −yᵀcₑ(x)</td>
/// <td>∇ₓₓ²(−yᵀcₑ(x))</td>
/// <td>num_decision_variables</td>
/// <td>num_decision_variables</td>
/// </tr>

30
wpimath/src/main/native/thirdparty/sleipnir/include/sleipnir/util/print_diagnostics.hpp vendored
View File

@@ -297,55 +297,55 @@ inline void print_solver_diagnostics(
const gch::small_vector<SolveProfiler>& 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<SetupProfiler>& 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