[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/main/native/cpp/jni/ArmFeedforwardJNI.cpp

@@ -1,37 +0,0 @@
-// 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 <jni.h>
-
-#include "org_wpilib_math_jni_ArmFeedforwardJNI.h"
-#include "wpi/math/controller/ArmFeedforward.hpp"
-#include "wpi/util/jni_util.hpp"
-
-using namespace wpi::util::java;
-
-extern "C" {
-
-/*
- * Class:     org_wpilib_math_jni_ArmFeedforwardJNI
- * Method:    calculate
- * Signature: (DDDDDDDD)D
- */
-JNIEXPORT jdouble JNICALL
-Java_org_wpilib_math_jni_ArmFeedforwardJNI_calculate
-  (JNIEnv* env, jclass, jdouble ks, jdouble kv, jdouble ka, jdouble kg,
-   jdouble currentAngle, jdouble currentVelocity, jdouble nextVelocity,
-   jdouble dt)
-{
-  return wpi::math::ArmFeedforward{
-      wpi::units::volt_t{ks}, wpi::units::volt_t{kg},
-      wpi::units::unit_t<wpi::math::ArmFeedforward::kv_unit>{kv},
-      wpi::units::unit_t<wpi::math::ArmFeedforward::ka_unit>{ka},
-      wpi::units::second_t{dt}}
-      .Calculate(wpi::units::radian_t{currentAngle},
-                 wpi::units::radians_per_second_t{currentVelocity},
-                 wpi::units::radians_per_second_t{nextVelocity})
-      .value();
-}
-
-}  // extern "C"

wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp

@@ -1,39 +0,0 @@
-// 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 <jni.h>
-
-#include "org_wpilib_math_jni_Ellipse2dJNI.h"
-#include "wpi/math/geometry/Ellipse2d.hpp"
-#include "wpi/util/array.hpp"
-#include "wpi/util/jni_util.hpp"
-
-using namespace wpi::util::java;
-
-extern "C" {
-
-/*
- * Class:     org_wpilib_math_jni_Ellipse2dJNI
- * Method:    nearest
- * Signature: (DDDDDDD[D)V
- */
-JNIEXPORT void JNICALL
-Java_org_wpilib_math_jni_Ellipse2dJNI_nearest
-  (JNIEnv* env, jclass, jdouble centerX, jdouble centerY, jdouble centerHeading,
-   jdouble xSemiAxis, jdouble ySemiAxis, jdouble pointX, jdouble pointY,
-   jdoubleArray nearestPoint)
-{
-  auto point =
-      wpi::math::Ellipse2d{
-          wpi::math::Pose2d{wpi::units::meter_t{centerX},
-                            wpi::units::meter_t{centerY},
-                            wpi::units::radian_t{centerHeading}},
-          wpi::units::meter_t{xSemiAxis}, wpi::units::meter_t{ySemiAxis}}
-          .Nearest({wpi::units::meter_t{pointX}, wpi::units::meter_t{pointY}});
-
-  wpi::util::array buf{point.X().value(), point.Y().value()};
-  env->SetDoubleArrayRegion(nearestPoint, 0, 2, buf.data());
-}
-
-}  // extern "C"

wpimath/src/main/native/cpp/jni/SleipnirJNIUtil.hpp

@@ -0,0 +1,65 @@
+// 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 <jni.h>
+
+#include <concepts>
+#include <vector>
+
+#include <Eigen/SparseCore>
+
+#include "wpi/util/jni_util.hpp"
+
+namespace wpi::math::detail {
+
+/**
+ * Converts Eigen sparse matrix to triplets.
+ *
+ * @param env JNI environment.
+ * @param mat Eigen sparse matrix to convert.
+ * @return NativeSparseTriplets instance.
+ */
+template <typename Derived>
+  requires std::derived_from<Derived, Eigen::SparseCompressedBase<Derived>>
+jobject GetTriplets(JNIEnv* env, const Derived& mat) {
+  const int nonZeros = mat.nonZeros();
+
+  std::vector<int> rows;
+  rows.reserve(nonZeros);
+
+  std::vector<int> cols;
+  cols.reserve(nonZeros);
+
+  std::vector<double> values;
+  values.reserve(nonZeros);
+
+  for (int k = 0; k < mat.outerSize(); ++k) {
+    for (typename Derived::InnerIterator it{mat, k}; it; ++it) {
+      rows.emplace_back(it.row());
+      cols.emplace_back(it.col());
+      values.emplace_back(it.value());
+    }
+  }
+
+  // Find NativeSparseTriplets class
+  static wpi::util::java::JClass cls{
+      env, "org/wpilib/math/autodiff/NativeSparseTriplets"};
+  if (!cls) {
+    return nullptr;
+  }
+
+  // Find NativeSparseTriplets constructor
+  static jmethodID ctor = env->GetMethodID(cls, "<init>", "([I[I[D)V");
+  if (!ctor) {
+    return nullptr;
+  }
+
+  return env->NewObject(cls, ctor, wpi::util::java::MakeJIntArray(env, rows),
+                        wpi::util::java::MakeJIntArray(env, cols),
+                        wpi::util::java::MakeJDoubleArray(env, values));
+}
+
+}  // namespace wpi::math::detail

wpimath/src/main/native/cpp/jni/autodiff/GradientJNI.cpp

@@ -0,0 +1,90 @@
+// 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 <jni.h>
+
+#include <utility>
+#include <vector>
+
+#include <sleipnir/autodiff/gradient.hpp>
+#include <sleipnir/autodiff/variable.hpp>
+#include <sleipnir/autodiff/variable_matrix.hpp>
+
+#include "../SleipnirJNIUtil.hpp"
+#include "org_wpilib_math_autodiff_GradientJNI.h"
+#include "wpi/util/jni_util.hpp"
+
+using namespace wpi::util::java;
+
+extern "C" {
+
+/*
+ * Class:     org_wpilib_math_autodiff_GradientJNI
+ * Method:    create
+ * Signature: (J[J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_GradientJNI_create
+  (JNIEnv* env, jclass, jlong variable, jlongArray wrt)
+{
+  auto& variableObj = *reinterpret_cast<slp::Variable<double>*>(variable);
+
+  JSpan<const jlong> wrtSpan{env, wrt};
+  slp::VariableMatrix<double> wrtObj(slp::detail::empty, wrtSpan.size(), 1);
+  for (size_t i = 0; i < wrtSpan.size(); ++i) {
+    wrtObj[i] = *reinterpret_cast<slp::Variable<double>*>(wrtSpan[i]);
+  }
+
+  return reinterpret_cast<jlong>(
+      new slp::Gradient{variableObj, std::move(wrtObj)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_GradientJNI
+ * Method:    destroy
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_autodiff_GradientJNI_destroy
+  (JNIEnv* env, jclass, jlong handle)
+{
+  delete reinterpret_cast<slp::Gradient<double>*>(handle);
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_GradientJNI
+ * Method:    get
+ * Signature: (J)[J
+ */
+JNIEXPORT jlongArray JNICALL
+Java_org_wpilib_math_autodiff_GradientJNI_get
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& gradient = *reinterpret_cast<slp::Gradient<double>*>(handle);
+  auto g = gradient.get();
+
+  std::vector<jlong> varHandles;
+  varHandles.reserve(g.size());
+  for (auto& var : g) {
+    varHandles.emplace_back(
+        reinterpret_cast<jlong>(new slp::Variable<double>{var}));
+  }
+
+  return MakeJLongArray(env, varHandles);
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_GradientJNI
+ * Method:    value
+ * Signature: (J)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_org_wpilib_math_autodiff_GradientJNI_value
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& gradient = *reinterpret_cast<slp::Gradient<double>*>(handle);
+  return wpi::math::detail::GetTriplets(env, gradient.value());
+}
+
+}  // extern "C"

wpimath/src/main/native/cpp/jni/autodiff/HessianJNI.cpp

@@ -0,0 +1,96 @@
+// 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 <jni.h>
+
+#include <utility>
+#include <vector>
+
+#include <sleipnir/autodiff/hessian.hpp>
+#include <sleipnir/autodiff/variable.hpp>
+#include <sleipnir/autodiff/variable_matrix.hpp>
+
+#include "../SleipnirJNIUtil.hpp"
+#include "org_wpilib_math_autodiff_HessianJNI.h"
+#include "wpi/util/jni_util.hpp"
+
+using namespace wpi::util::java;
+
+extern "C" {
+
+/*
+ * Class:     org_wpilib_math_autodiff_HessianJNI
+ * Method:    create
+ * Signature: (J[J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_HessianJNI_create
+  (JNIEnv* env, jclass, jlong variable, jlongArray wrt)
+{
+  auto& variableObj = *reinterpret_cast<slp::Variable<double>*>(variable);
+
+  JSpan<const jlong> wrtSpan{env, wrt};
+  slp::VariableMatrix<double> wrtObj(slp::detail::empty, wrtSpan.size(), 1);
+  for (size_t i = 0; i < wrtSpan.size(); ++i) {
+    wrtObj[i] = *reinterpret_cast<slp::Variable<double>*>(wrtSpan[i]);
+  }
+
+  return reinterpret_cast<jlong>(
+      new slp::Hessian<double, Eigen::Lower | Eigen::Upper>{variableObj,
+                                                            std::move(wrtObj)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_HessianJNI
+ * Method:    destroy
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_autodiff_HessianJNI_destroy
+  (JNIEnv* env, jclass, jlong handle)
+{
+  delete reinterpret_cast<slp::Hessian<double, Eigen::Lower | Eigen::Upper>*>(
+      handle);
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_HessianJNI
+ * Method:    get
+ * Signature: (J)[J
+ */
+JNIEXPORT jlongArray JNICALL
+Java_org_wpilib_math_autodiff_HessianJNI_get
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& hessian =
+      *reinterpret_cast<slp::Hessian<double, Eigen::Lower | Eigen::Upper>*>(
+          handle);
+  auto H = hessian.get();
+
+  std::vector<jlong> varHandles;
+  varHandles.reserve(H.size());
+  for (auto& var : H) {
+    varHandles.emplace_back(
+        reinterpret_cast<jlong>(new slp::Variable<double>{var}));
+  }
+
+  return MakeJLongArray(env, varHandles);
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_HessianJNI
+ * Method:    value
+ * Signature: (J)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_org_wpilib_math_autodiff_HessianJNI_value
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& hessian =
+      *reinterpret_cast<slp::Hessian<double, Eigen::Lower | Eigen::Upper>*>(
+          handle);
+  return wpi::math::detail::GetTriplets(env, hessian.value());
+}
+
+}  // extern "C"

wpimath/src/main/native/cpp/jni/autodiff/JacobianJNI.cpp

@@ -0,0 +1,96 @@
+// 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 <jni.h>
+
+#include <utility>
+#include <vector>
+
+#include <sleipnir/autodiff/jacobian.hpp>
+#include <sleipnir/autodiff/variable.hpp>
+#include <sleipnir/autodiff/variable_matrix.hpp>
+
+#include "../SleipnirJNIUtil.hpp"
+#include "org_wpilib_math_autodiff_JacobianJNI.h"
+#include "wpi/util/jni_util.hpp"
+
+using namespace wpi::util::java;
+
+extern "C" {
+
+/*
+ * Class:     org_wpilib_math_autodiff_JacobianJNI
+ * Method:    create
+ * Signature: ([J[J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_JacobianJNI_create
+  (JNIEnv* env, jclass, jlongArray variables, jlongArray wrt)
+{
+  JSpan<const jlong> variablesSpan{env, variables};
+  slp::VariableMatrix<double> variablesObj(slp::detail::empty,
+                                           variablesSpan.size(), 1);
+  for (size_t i = 0; i < variablesSpan.size(); ++i) {
+    variablesObj[i] =
+        *reinterpret_cast<slp::Variable<double>*>(variablesSpan[i]);
+  }
+
+  JSpan<const jlong> wrtSpan{env, wrt};
+  slp::VariableMatrix<double> wrtObj(slp::detail::empty, wrtSpan.size(), 1);
+  for (size_t i = 0; i < wrtSpan.size(); ++i) {
+    wrtObj[i] = *reinterpret_cast<slp::Variable<double>*>(wrtSpan[i]);
+  }
+
+  return reinterpret_cast<jlong>(
+      new slp::Jacobian{std::move(variablesObj), std::move(wrtObj)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_JacobianJNI
+ * Method:    destroy
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_autodiff_JacobianJNI_destroy
+  (JNIEnv* env, jclass, jlong handle)
+{
+  delete reinterpret_cast<slp::Jacobian<double>*>(handle);
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_JacobianJNI
+ * Method:    get
+ * Signature: (J)[J
+ */
+JNIEXPORT jlongArray JNICALL
+Java_org_wpilib_math_autodiff_JacobianJNI_get
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& jacobian = *reinterpret_cast<slp::Jacobian<double>*>(handle);
+  auto J = jacobian.get();
+
+  std::vector<jlong> varHandles;
+  varHandles.reserve(J.size());
+  for (auto& var : J) {
+    varHandles.emplace_back(
+        reinterpret_cast<jlong>(new slp::Variable<double>{var}));
+  }
+
+  return MakeJLongArray(env, varHandles);
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_JacobianJNI
+ * Method:    value
+ * Signature: (J)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_org_wpilib_math_autodiff_JacobianJNI_value
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& jacobian = *reinterpret_cast<slp::Jacobian<double>*>(handle);
+  return wpi::math::detail::GetTriplets(env, jacobian.value());
+}
+
+}  // extern "C"

wpimath/src/main/native/cpp/jni/autodiff/VariableJNI.cpp

@@ -0,0 +1,463 @@
+// 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 <jni.h>
+
+#include <sleipnir/autodiff/variable.hpp>
+
+#include "org_wpilib_math_autodiff_VariableJNI.h"
+
+extern "C" {
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    createDefault
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_createDefault
+  (JNIEnv* env, jclass)
+{
+  return reinterpret_cast<jlong>(new slp::Variable<double>{});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    createDouble
+ * Signature: (D)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_createDouble
+  (JNIEnv* env, jclass, jdouble value)
+{
+  return reinterpret_cast<jlong>(new slp::Variable<double>{value});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    createInt
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_createInt
+  (JNIEnv* env, jclass, jint value)
+{
+  return reinterpret_cast<jlong>(new slp::Variable<double>{value});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    destroy
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_destroy
+  (JNIEnv* env, jclass, jlong handle)
+{
+  delete reinterpret_cast<slp::Variable<double>*>(handle);
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    setValue
+ * Signature: (JD)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_setValue
+  (JNIEnv* env, jclass, jlong handle, jdouble value)
+{
+  auto& lhsVar = *reinterpret_cast<slp::Variable<double>*>(handle);
+  lhsVar.set_value(value);
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    times
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_times
+  (JNIEnv* env, jclass, jlong handle, jlong rhs)
+{
+  auto& lhsVar = *reinterpret_cast<slp::Variable<double>*>(handle);
+  auto& rhsVar = *reinterpret_cast<slp::Variable<double>*>(rhs);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{lhsVar * rhsVar});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    div
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_div
+  (JNIEnv* env, jclass, jlong handle, jlong rhs)
+{
+  auto& lhsVar = *reinterpret_cast<slp::Variable<double>*>(handle);
+  auto& rhsVar = *reinterpret_cast<slp::Variable<double>*>(rhs);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{lhsVar / rhsVar});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    plus
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_plus
+  (JNIEnv* env, jclass, jlong handle, jlong rhs)
+{
+  auto& lhsVar = *reinterpret_cast<slp::Variable<double>*>(handle);
+  auto& rhsVar = *reinterpret_cast<slp::Variable<double>*>(rhs);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{lhsVar + rhsVar});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    minus
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_minus
+  (JNIEnv* env, jclass, jlong handle, jlong rhs)
+{
+  auto& lhsVar = *reinterpret_cast<slp::Variable<double>*>(handle);
+  auto& rhsVar = *reinterpret_cast<slp::Variable<double>*>(rhs);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{lhsVar - rhsVar});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    unaryMinus
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_unaryMinus
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& lhsVar = *reinterpret_cast<slp::Variable<double>*>(handle);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{-lhsVar});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    value
+ * Signature: (J)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_value
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& lhsVar = *reinterpret_cast<slp::Variable<double>*>(handle);
+  return lhsVar.value();
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    type
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_type
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& lhsVar = *reinterpret_cast<slp::Variable<double>*>(handle);
+  return static_cast<jint>(lhsVar.type());
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    abs
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_abs
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{abs(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    acos
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_acos
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{acos(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    asin
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_asin
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{asin(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    atan
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_atan
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{atan(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    atan2
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_atan2
+  (JNIEnv* env, jclass, jlong y, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  auto& yVar = *reinterpret_cast<slp::Variable<double>*>(y);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{atan2(yVar, xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    cbrt
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_cbrt
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{cbrt(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    cos
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_cos
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{cos(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    cosh
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_cosh
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{cosh(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    exp
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_exp
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{exp(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    hypot
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_hypot
+  (JNIEnv* env, jclass, jlong x, jlong y)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  auto& yVar = *reinterpret_cast<slp::Variable<double>*>(y);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{hypot(xVar, yVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    log
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_log
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{log(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    log10
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_log10
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{log10(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    max
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_max
+  (JNIEnv* env, jclass, jlong a, jlong b)
+{
+  auto& aVar = *reinterpret_cast<slp::Variable<double>*>(a);
+  auto& bVar = *reinterpret_cast<slp::Variable<double>*>(b);
+  return reinterpret_cast<jlong>(
+      new slp::Variable<double>{(slp::max)(aVar, bVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    min
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_min
+  (JNIEnv* env, jclass, jlong a, jlong b)
+{
+  auto& aVar = *reinterpret_cast<slp::Variable<double>*>(a);
+  auto& bVar = *reinterpret_cast<slp::Variable<double>*>(b);
+  return reinterpret_cast<jlong>(
+      new slp::Variable<double>{(slp::min)(aVar, bVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    pow
+ * Signature: (JJ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_pow
+  (JNIEnv* env, jclass, jlong base, jlong power)
+{
+  auto& baseVar = *reinterpret_cast<slp::Variable<double>*>(base);
+  auto& powerVar = *reinterpret_cast<slp::Variable<double>*>(power);
+  return reinterpret_cast<jlong>(
+      new slp::Variable<double>{pow(baseVar, powerVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    signum
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_signum
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{sign(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    sin
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_sin
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{sin(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    sinh
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_sinh
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{sinh(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    sqrt
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_sqrt
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{sqrt(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    tan
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_tan
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{tan(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    tanh
+ * Signature: (J)J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_tanh
+  (JNIEnv* env, jclass, jlong x)
+{
+  auto& xVar = *reinterpret_cast<slp::Variable<double>*>(x);
+  return reinterpret_cast<jlong>(new slp::Variable<double>{tanh(xVar)});
+}
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableJNI
+ * Method:    totalNativeMemoryUsage
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_autodiff_VariableJNI_totalNativeMemoryUsage
+  (JNIEnv* env, jclass)
+{
+  return slp::global_pool_resource().blocks_in_use() *
+         sizeof(slp::detail::Expression<double>);
+}
+
+}  // extern "C"

wpimath/src/main/native/cpp/jni/autodiff/VariableMatrixJNI.cpp

@@ -0,0 +1,53 @@
+// 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 <jni.h>
+
+#include <vector>
+
+#include <sleipnir/autodiff/variable.hpp>
+#include <sleipnir/autodiff/variable_matrix.hpp>
+
+#include "org_wpilib_math_autodiff_VariableMatrixJNI.h"
+#include "wpi/util/jni_util.hpp"
+
+using namespace wpi::util::java;
+
+extern "C" {
+
+/*
+ * Class:     org_wpilib_math_autodiff_VariableMatrixJNI
+ * Method:    solve
+ * Signature: ([JI[JI)[J
+ */
+JNIEXPORT jlongArray JNICALL
+Java_org_wpilib_math_autodiff_VariableMatrixJNI_solve
+  (JNIEnv* env, jclass, jlongArray A, jint Acols, jlongArray B, jint Bcols)
+{
+  JSpan<const jlong> ASpan{env, A};
+  slp::VariableMatrix<double> AObj(slp::detail::empty, ASpan.size() / Acols,
+                                   Acols);
+  for (size_t i = 0; i < ASpan.size(); ++i) {
+    AObj[i] = *reinterpret_cast<slp::Variable<double>*>(ASpan[i]);
+  }
+
+  JSpan<const jlong> BSpan{env, B};
+  slp::VariableMatrix<double> BObj(slp::detail::empty, BSpan.size() / Bcols,
+                                   Bcols);
+  for (size_t i = 0; i < BSpan.size(); ++i) {
+    BObj[i] = *reinterpret_cast<slp::Variable<double>*>(BSpan[i]);
+  }
+
+  auto X = slp::solve(AObj, BObj);
+
+  std::vector<jlong> varHandles;
+  varHandles.reserve(X.size());
+  for (auto& var : X) {
+    varHandles.emplace_back(
+        reinterpret_cast<jlong>(new slp::Variable<double>{var}));
+  }
+  return MakeJLongArray(env, varHandles);
+}
+
+}  // extern "C"

wpimath/src/main/native/cpp/jni/optimization/ProblemJNI.cpp

@@ -0,0 +1,255 @@
+// 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 <jni.h>
+
+#include <vector>
+
+#include <sleipnir/optimization/problem.hpp>
+
+#include "../SleipnirJNIUtil.hpp"
+#include "org_wpilib_math_optimization_ProblemJNI.h"
+#include "wpi/util/jni_util.hpp"
+
+using namespace wpi::util::java;
+
+extern "C" {
+
+namespace {
+
+// ProblemJNI_solve() sets these before calling Problem::solve() so the Java
+// callback has a valid JNIEnv and object on which to call
+// Problem.runCallbacks()
+thread_local JNIEnv* callbackEnv;
+thread_local jobject callbackObj;
+
+}  // namespace
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    create
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_create
+  (JNIEnv* env, jclass)
+{
+  auto problem = new slp::Problem<double>;
+
+  // Configure Java iteration callbacks
+  problem->add_persistent_callback(
+      [](const slp::IterationInfo<double>& info) -> bool {
+        // Find Problem class
+        static JClass cls{callbackEnv, "org/wpilib/math/optimization/Problem"};
+        if (!cls) {
+          return true;
+        }
+
+        // Find Problem.runCallbacks()
+        static jmethodID runCallbacks = callbackEnv->GetMethodID(
+            cls, "runCallbacks",
+            "(III[DLorg/wpilib/math/autodiff/NativeSparseTriplets;"
+            "Lorg/wpilib/math/autodiff/NativeSparseTriplets;"
+            "Lorg/wpilib/math/autodiff/NativeSparseTriplets;"
+            "Lorg/wpilib/math/autodiff/NativeSparseTriplets;)Z");
+        if (!runCallbacks) {
+          return true;
+        }
+
+        // Run Java callbacks
+        return callbackEnv->CallBooleanMethod(
+            callbackObj, runCallbacks, info.A_e.rows(), info.A_i.rows(),
+            info.iteration, MakeJDoubleArray(callbackEnv, info.x),
+            wpi::math::detail::GetTriplets(callbackEnv, info.g),
+            wpi::math::detail::GetTriplets(callbackEnv, info.H),
+            wpi::math::detail::GetTriplets(callbackEnv, info.A_e),
+            wpi::math::detail::GetTriplets(callbackEnv, info.A_i));
+      });
+
+  return reinterpret_cast<jlong>(problem);
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    destroy
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_destroy
+  (JNIEnv* env, jclass, jlong handle)
+{
+  delete reinterpret_cast<slp::Problem<double>*>(handle);
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    decisionVariable
+ * Signature: (JII)[J
+ */
+JNIEXPORT jlongArray JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_decisionVariable
+  (JNIEnv* env, jclass, jlong handle, jint rows, jint cols)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  auto vars = problem.decision_variable(rows, cols);
+
+  std::vector<jlong> varHandles;
+  varHandles.reserve(vars.size());
+  for (auto& var : vars) {
+    varHandles.emplace_back(
+        reinterpret_cast<jlong>(new slp::Variable<double>{var}));
+  }
+  return MakeJLongArray(env, varHandles);
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    symmetricDecisionVariable
+ * Signature: (JI)[J
+ */
+JNIEXPORT jlongArray JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_symmetricDecisionVariable
+  (JNIEnv* env, jclass, jlong handle, jint rows)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  auto vars = problem.symmetric_decision_variable(rows);
+
+  std::vector<jlong> varHandles;
+  varHandles.reserve(vars.size());
+  for (auto& var : vars) {
+    varHandles.emplace_back(
+        reinterpret_cast<jlong>(new slp::Variable<double>{var}));
+  }
+  return MakeJLongArray(env, varHandles);
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    minimize
+ * Signature: (JJ)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_minimize
+  (JNIEnv* env, jclass, jlong handle, jlong costHandle)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  auto& costVar = *reinterpret_cast<slp::Variable<double>*>(costHandle);
+  problem.minimize(costVar);
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    maximize
+ * Signature: (JJ)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_maximize
+  (JNIEnv* env, jclass, jlong handle, jlong objectiveHandle)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  auto& objectiveVar =
+      *reinterpret_cast<slp::Variable<double>*>(objectiveHandle);
+  problem.maximize(objectiveVar);
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    subjectToEq
+ * Signature: (J[J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_subjectToEq
+  (JNIEnv* env, jclass, jlong handle, jlongArray constraintHandles)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  JSpan<const jlong> constraintHandlesSpan{env, constraintHandles};
+
+  for (const auto& constraintHandle : constraintHandlesSpan) {
+    const auto& constraint =
+        *reinterpret_cast<slp::Variable<double>*>(constraintHandle);
+    problem.subject_to(constraint == 0.0);
+  }
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    subjectToIneq
+ * Signature: (J[J)V
+ */
+JNIEXPORT void JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_subjectToIneq
+  (JNIEnv* env, jclass, jlong handle, jlongArray constraintHandles)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  JSpan<const jlong> constraintHandlesSpan{env, constraintHandles};
+
+  for (const auto& constraintHandle : constraintHandlesSpan) {
+    const auto& constraint =
+        *reinterpret_cast<slp::Variable<double>*>(constraintHandle);
+    problem.subject_to(constraint >= 0.0);
+  }
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    costFunctionType
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_costFunctionType
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  return static_cast<jint>(problem.cost_function_type());
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    equalityConstraintType
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_equalityConstraintType
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  return static_cast<jint>(problem.equality_constraint_type());
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    inequalityConstraintType
+ * Signature: (J)I
+ */
+JNIEXPORT jint JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_inequalityConstraintType
+  (JNIEnv* env, jclass, jlong handle)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+  return static_cast<jint>(problem.inequality_constraint_type());
+}
+
+/*
+ * Class:     org_wpilib_math_optimization_ProblemJNI
+ * Method:    solve
+ * Signature: (Ljava/lang/Object;JDIDZZ)I
+ */
+JNIEXPORT jint JNICALL
+Java_org_wpilib_math_optimization_ProblemJNI_solve
+  (JNIEnv* env, jclass, jobject obj, jlong handle, jdouble tolerance,
+   jint maxIterations, jdouble timeout, jboolean feasibleIPM,
+   jboolean diagnostics)
+{
+  auto& problem = *reinterpret_cast<slp::Problem<double>*>(handle);
+
+  callbackEnv = env;
+  callbackObj = obj;
+
+  slp::Options options{
+      tolerance, maxIterations, std::chrono::duration<double>{timeout},
+      static_cast<bool>(feasibleIPM), static_cast<bool>(diagnostics)};
+  return static_cast<int>(problem.solve(options));
+}
+
+}  // extern "C"