[wpimath] Add Sleipnir Java bindings (#8236)

The wrapper includes reverse mode autodiff, the Problem DSL, and the
optimal control problem API. I wrote it by directly translating the
upstream
[API](https://github.com/SleipnirGroup/Sleipnir/tree/main/include/sleipnir)
and [tests](https://github.com/SleipnirGroup/Sleipnir/tree/main/test) to
Java (i.e., copy-paste-modify).

I replaced the ArmFeedforward and Ellipse2d JNIs with implementations
using the Sleipnir Java bindings. Switching dev binary JNIs to release
by default sped up wpimath test runs from several minutes to 7 seconds.

wpimath/src/test/native/cpp/optimization/CurrentManagerTest.cpp

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/math/optimization/CurrentManager.hpp"
+
+#include <array>
+
+#include <gtest/gtest.h>
+
+TEST(CurrentManagerTest, EnoughCurrent) {
+  wpi::math::CurrentManager manager{std::array{1.0, 5.0, 10.0, 5.0}, 40.0};
+
+  auto currents = manager.calculate(std::array{25.0, 10.0, 5.0, 0.0});
+
+  EXPECT_NEAR(currents[0], 25.0, 1e-3);
+  EXPECT_NEAR(currents[1], 10.0, 1e-3);
+  EXPECT_NEAR(currents[2], 5.0, 1e-3);
+  EXPECT_NEAR(currents[3], 0.0, 1e-3);
+}
+
+TEST(CurrentManagerTest, NotEnoughCurrent) {
+  wpi::math::CurrentManager manager{std::array{1.0, 5.0, 10.0, 5.0}, 40.0};
+
+  auto currents = manager.calculate(std::array{30.0, 10.0, 5.0, 0.0});
+
+  // Expected values are from the following program:
+  //
+  // #!/usr/bin/env python3
+  //
+  // from scipy.optimize import minimize
+  //
+  // r = [30.0, 10.0, 5.0, 0.0]
+  // q = [1.0, 5.0, 10.0, 5.0]
+  //
+  // result = minimize(
+  //     lambda x: sum((r[i] - x[i]) ** 2 / q[i] ** 2 for i in range(4)),
+  //     [0.0, 0.0, 0.0, 0.0],
+  //     constraints=[
+  //         {"type": "ineq", "fun": lambda x: x},
+  //         {"type": "ineq", "fun": lambda x: 40.0 - sum(x)},
+  //     ],
+  // )
+  // print(result.x)
+  EXPECT_NEAR(currents[0], 29.960, 1e-3);
+  EXPECT_NEAR(currents[1], 9.008, 1e-3);
+  EXPECT_NEAR(currents[2], 1.032, 1e-3);
+  EXPECT_NEAR(currents[3], 0.0, 1e-3);
+}

wpimath/src/test/native/include/wpi/math/optimization/CurrentManager.hpp

@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <limits>
+#include <span>
+#include <stdexcept>
+#include <vector>
+
+#include <sleipnir/optimization/problem.hpp>
+
+namespace wpi::math {
+
+/**
+ * This class computes the optimal current allocation for a list of subsystems
+ * given a list of their desired currents and current tolerances that determine
+ * which subsystem gets less current if the current budget is exceeded.
+ * Subsystems with a smaller tolerance are given higher priority.
+ */
+class CurrentManager {
+ public:
+  /**
+   * Constructs a CurrentManager.
+   *
+   * @param currentTolerances The relative current tolerance of each subsystem.
+   * @param maxCurrent The current budget to allocate between subsystems.
+   */
+  CurrentManager(std::span<const double> currentTolerances, double maxCurrent)
+      : m_desiredCurrents{static_cast<int>(currentTolerances.size()), 1},
+        m_allocatedCurrents{
+            m_problem.decision_variable(currentTolerances.size())} {
+    // Ensure m_desiredCurrents contains initialized Variables
+    for (int row = 0; row < m_desiredCurrents.rows(); ++row) {
+      // Don't initialize to 0 or 1, because those will get folded by Sleipnir
+      m_desiredCurrents[row] = std::numeric_limits<double>::infinity();
+    }
+
+    slp::Variable J = 0.0;
+    slp::Variable current_sum = 0.0;
+    for (size_t i = 0; i < currentTolerances.size(); ++i) {
+      // The weight is 1/tolᵢ² where tolᵢ is the tolerance between the desired
+      // and allocated current for subsystem i
+      auto error = m_desiredCurrents[i] - m_allocatedCurrents[i];
+      J += error * error / (currentTolerances[i] * currentTolerances[i]);
+
+      current_sum += m_allocatedCurrents[i];
+
+      // Currents must be nonnegative
+      m_problem.subject_to(m_allocatedCurrents[i] >= 0.0);
+    }
+    m_problem.minimize(J);
+
+    // Keep total current below maximum
+    m_problem.subject_to(current_sum <= maxCurrent);
+  }
+
+  /**
+   * Returns the optimal current allocation for a list of subsystems given a
+   * list of their desired currents and current tolerances that determine which
+   * subsystem gets less current if the current budget is exceeded. Subsystems
+   * with a smaller tolerance are given higher priority.
+   *
+   * @param desiredCurrents The desired current for each subsystem.
+   * @throws std::runtime_error if the number of desired currents doesn't equal
+   *         the number of tolerances passed in the constructor.
+   */
+  std::vector<double> calculate(std::span<const double> desiredCurrents) {
+    if (m_desiredCurrents.rows() != static_cast<int>(desiredCurrents.size())) {
+      throw std::runtime_error(
+          "Number of desired currents must equal the number of tolerances "
+          "passed in the constructor.");
+    }
+
+    for (size_t i = 0; i < desiredCurrents.size(); ++i) {
+      m_desiredCurrents[i].set_value(desiredCurrents[i]);
+    }
+
+    m_problem.solve();
+
+    std::vector<double> result;
+    for (size_t i = 0; i < desiredCurrents.size(); ++i) {
+      result.emplace_back(std::max(m_allocatedCurrents.value(i), 0.0));
+    }
+
+    return result;
+  }
+
+ private:
+  slp::Problem<double> m_problem;
+  slp::VariableMatrix<double> m_desiredCurrents;
+  slp::VariableMatrix<double> m_allocatedCurrents;
+};
+
+}  // namespace wpi::math