[upstream_utils] Upgrade Sleipnir (#7512)

It now uses SQP for problems without inequality constraints, which is
faster.

main:
```
[ RUN      ] Ellipse2dTest.DistanceToPoint
0.203 ms
[       OK ] Ellipse2dTest.DistanceToPoint (0 ms)
[ RUN      ] Ellipse2dTest.FindNearestPoint
0.019 ms
[       OK ] Ellipse2dTest.FindNearestPoint (0 ms)
```

upgrade:
```
[ RUN      ] Ellipse2dTest.DistanceToPoint
0.197 ms
[       OK ] Ellipse2dTest.DistanceToPoint (0 ms)
[ RUN      ] Ellipse2dTest.FindNearestPoint
0.015 ms
[       OK ] Ellipse2dTest.FindNearestPoint (0 ms)
```

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

@@ -20,6 +20,7 @@
 #include "sleipnir/optimization/SolverIterationInfo.hpp"
 #include "sleipnir/optimization/SolverStatus.hpp"
 #include "sleipnir/optimization/solver/InteriorPoint.hpp"
+#include "sleipnir/optimization/solver/SQP.hpp"
 #include "sleipnir/util/Print.hpp"
 #include "sleipnir/util/SymbolExports.hpp"
 
@@ -305,10 +306,15 @@ class SLEIPNIR_DLLEXPORT OptimizationProblem {
     }
 
     // Solve the optimization problem
-    Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
-    InteriorPoint(m_decisionVariables, m_equalityConstraints,
-                  m_inequalityConstraints, m_f.value(), m_callback, config,
-                  false, x, s, &status);
+    if (m_inequalityConstraints.empty()) {
+      SQP(m_decisionVariables, m_equalityConstraints, m_f.value(), m_callback,
+          config, x, &status);
+    } else {
+      Eigen::VectorXd s = Eigen::VectorXd::Ones(m_inequalityConstraints.size());
+      InteriorPoint(m_decisionVariables, m_equalityConstraints,
+                    m_inequalityConstraints, m_f.value(), m_callback, config,
+                    false, x, s, &status);
+    }
 
     if (config.diagnostics) {
       sleipnir::println("Exit condition: {}", ToMessage(status.exitCondition));

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

@@ -0,0 +1,46 @@
+// Copyright (c) Sleipnir contributors
+
+#pragma once
+
+#include <span>
+
+#include <Eigen/Core>
+
+#include "sleipnir/autodiff/Variable.hpp"
+#include "sleipnir/optimization/SolverConfig.hpp"
+#include "sleipnir/optimization/SolverIterationInfo.hpp"
+#include "sleipnir/optimization/SolverStatus.hpp"
+#include "sleipnir/util/FunctionRef.hpp"
+#include "sleipnir/util/SymbolExports.hpp"
+
+namespace sleipnir {
+
+/**
+Finds the optimal solution to a nonlinear program using Sequential Quadratic
+Programming (SQP).
+
+A nonlinear program has the form:
+
+@verbatim
+     min_x f(x)
+subject to cₑ(x) = 0
+@endverbatim
+
+where f(x) is the cost function and cₑ(x) are the equality constraints.
+
+@param[in] decisionVariables The list of decision variables.
+@param[in] equalityConstraints The list of equality constraints.
+@param[in] f The cost function.
+@param[in] callback The user callback.
+@param[in] config Configuration options for the solver.
+@param[in,out] x The initial guess and output location for the decision
+  variables.
+@param[out] status The solver status.
+*/
+SLEIPNIR_DLLEXPORT void SQP(
+    std::span<Variable> decisionVariables,
+    std::span<Variable> equalityConstraints, Variable& f,
+    function_ref<bool(const SolverIterationInfo& info)> callback,
+    const SolverConfig& config, Eigen::VectorXd& x, SolverStatus* status);
+
+}  // namespace sleipnir