[wpimath] Add Sleipnir Java bindings

Author: calcmogul

build.gradle

@@ -20,7 +20,7 @@ plugins {
     id 'net.ltgt.errorprone' version '4.3.0' apply false
     id 'com.gradleup.shadow' version '9.0.0' apply false
     id 'com.diffplug.spotless' version '7.2.1' apply false
-    id 'com.github.spotbugs' version '6.2.3' apply false
+    id 'com.github.spotbugs' version '6.4.2' apply false
 }
 
 wpilibVersioning.buildServerMode = project.hasProperty('buildServer')

wpimath/BUILD.bazel

@@ -81,6 +81,9 @@ cc_library(
 java_library(
     name = "wpimath-java",
     srcs = [":generated_java"] + glob(["src/main/java/**/*.java"]),
+    javacopts = [
+        "-Xep:UnicodeInCode:OFF",
+    ],
     visibility = ["//visibility:public"],
     deps = [
         "//wpiunits",

wpimath/CMakeLists.txt

@@ -7,14 +7,18 @@ include(DownloadAndCheck)
 
 file(
     GLOB wpimath_jni_src
-    src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
     src/main/native/cpp/jni/DAREJNI.cpp
     src/main/native/cpp/jni/EigenJNI.cpp
-    src/main/native/cpp/jni/Ellipse2dJNI.cpp
     src/main/native/cpp/jni/Exceptions.cpp
     src/main/native/cpp/jni/Pose3dJNI.cpp
     src/main/native/cpp/jni/StateSpaceUtilJNI.cpp
     src/main/native/cpp/jni/TrajectoryUtilJNI.cpp
+    src/main/native/cpp/jni/autodiff/GradientJNI.cpp
+    src/main/native/cpp/jni/autodiff/HessianJNI.cpp
+    src/main/native/cpp/jni/autodiff/JacobianJNI.cpp
+    src/main/native/cpp/jni/autodiff/VariableJNI.cpp
+    src/main/native/cpp/jni/autodiff/VariableMatrixJNI.cpp
+    src/main/native/cpp/jni/optimization/ProblemJNI.cpp
 )
 
 # Java bindings

wpimath/src/main/java/edu/wpi/first/math/autodiff/ExpressionType.java

@@ -0,0 +1,47 @@
+// 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.
+
+package edu.wpi.first.math.autodiff;
+
+/**
+ * Expression type.
+ *
+ * <p>Used for autodiff caching.
+ */
+public enum ExpressionType {
+  /** There is no expression. */
+  NONE(0),
+  /** The expression is a constant. */
+  CONSTANT(1),
+  /** The expression is composed of linear and lower-order operators. */
+  LINEAR(2),
+  /** The expression is composed of quadratic and lower-order operators. */
+  QUADRATIC(3),
+  /** The expression is composed of nonlinear and lower-order operators. */
+  NONLINEAR(4);
+
+  /** ExpressionType value. */
+  public final int value;
+
+  ExpressionType(int value) {
+    this.value = value;
+  }
+
+  /**
+   * Converts integer to its corresponding enum value.
+   *
+   * @param x The integer.
+   * @return The enum value.
+   */
+  public static ExpressionType fromInteger(int x) {
+    return switch (x) {
+      case 0 -> ExpressionType.NONE;
+      case 1 -> ExpressionType.CONSTANT;
+      case 2 -> ExpressionType.LINEAR;
+      case 3 -> ExpressionType.QUADRATIC;
+      case 4 -> ExpressionType.NONLINEAR;
+      default -> null;
+    };
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/autodiff/Gradient.java

@@ -0,0 +1,81 @@
+// 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.
+
+package edu.wpi.first.math.autodiff;
+
+import org.ejml.data.DMatrixSparseCSC;
+import org.ejml.ops.DConvertMatrixStruct;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * This class calculates the gradient of a variable with respect to a vector of variables.
+ *
+ * <p>The gradient is only recomputed if the variable expression is quadratic or higher order.
+ */
+public class Gradient implements AutoCloseable {
+  private long m_handle;
+  private int m_rows;
+
+  /**
+   * Constructs a Gradient object.
+   *
+   * @param variable Variable of which to compute the gradient.
+   * @param wrt Variable with respect to which to compute the gradient.
+   */
+  public Gradient(Variable variable, Variable wrt) {
+    this(variable, new VariableMatrix(wrt));
+  }
+
+  /**
+   * Constructs a Gradient object.
+   *
+   * @param variable Variable of which to compute the gradient.
+   * @param wrt Vector of variables with respect to which to compute the gradient.
+   */
+  public Gradient(Variable variable, VariableMatrix wrt) {
+    assert wrt.cols() == 1;
+
+    m_handle = GradientJNI.create(variable.getHandle(), wrt.getHandles());
+    m_rows = wrt.rows();
+  }
+
+  /**
+   * Constructs a Gradient object.
+   *
+   * @param variable Variable of which to compute the gradient.
+   * @param wrt Vector of variables with respect to which to compute the gradient.
+   */
+  public Gradient(Variable variable, VariableBlock wrt) {
+    this(variable, new VariableMatrix(wrt));
+  }
+
+  @Override
+  public void close() {
+    if (m_handle != 0) {
+      GradientJNI.destroy(m_handle);
+      m_handle = 0;
+    }
+  }
+
+  /**
+   * Returns the gradient as a VariableMatrix.
+   *
+   * <p>This is useful when constructing optimization problems with derivatives in them.
+   *
+   * @return The gradient as a VariableMatrix.
+   */
+  public VariableMatrix get() {
+    return VariableMatrix.fromHandles(m_rows, 1, GradientJNI.get(m_handle));
+  }
+
+  /**
+   * Evaluates the gradient at wrt's value.
+   *
+   * @return The gradient at wrt's value.
+   */
+  public SimpleMatrix value() {
+    var triplet = GradientJNI.value(m_handle);
+    return new SimpleMatrix(DConvertMatrixStruct.convert(triplet, (DMatrixSparseCSC) null));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/autodiff/GradientJNI.java

@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.autodiff;
+
+import edu.wpi.first.math.jni.WPIMathJNI;
+import org.ejml.data.DMatrixSparseTriplet;
+
+/** Gradient JNI functions. */
+public final class GradientJNI extends WPIMathJNI {
+  private GradientJNI() {
+    // Utility class.
+  }
+
+  /**
+   * Constructs a Gradient object.
+   *
+   * @param variable Variable of which to compute the Gradient.
+   * @param wrt Vector of variables with respect to which to compute the Gradient.
+   */
+  static native long create(long variable, long[] wrt);
+
+  /**
+   * Destructs a Gradient.
+   *
+   * @param handle Gradient handle.
+   */
+  static native void destroy(long handle);
+
+  /**
+   * Returns the Gradient as an array of Variable handles.
+   *
+   * <p>This is useful when constructing optimization problems with derivatives in them.
+   *
+   * @param handle Gradient handle.
+   * @return The Gradient as an array of Variable handles.
+   */
+  static native long[] get(long handle);
+
+  /**
+   * Evaluates the Gradient at wrt's value.
+   *
+   * @param handle Gradient handle.
+   * @return The Gradient at wrt's value.
+   */
+  static native DMatrixSparseTriplet value(long handle);
+}

wpimath/src/main/java/edu/wpi/first/math/autodiff/Hessian.java

@@ -0,0 +1,82 @@
+// 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.
+
+package edu.wpi.first.math.autodiff;
+
+import org.ejml.data.DMatrixSparseCSC;
+import org.ejml.ops.DConvertMatrixStruct;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * This class calculates the Hessian of a variable with respect to a vector of variables.
+ *
+ * <p>The gradient tree is cached so subsequent Hessian calculations are faster, and the Hessian is
+ * only recomputed if the variable expression is nonlinear.
+ */
+public class Hessian implements AutoCloseable {
+  private long m_handle;
+  private int m_rows;
+
+  /**
+   * Constructs a Hessian object.
+   *
+   * @param variable Variable of which to compute the Hessian.
+   * @param wrt Variable with respect to which to compute the Hessian.
+   */
+  public Hessian(Variable variable, Variable wrt) {
+    this(variable, new VariableMatrix(wrt));
+  }
+
+  /**
+   * Constructs a Hessian object.
+   *
+   * @param variable Variable of which to compute the Hessian.
+   * @param wrt Vector of variables with respect to which to compute the Hessian.
+   */
+  public Hessian(Variable variable, VariableMatrix wrt) {
+    assert wrt.cols() == 1;
+
+    m_handle = HessianJNI.create(variable.getHandle(), wrt.getHandles());
+    m_rows = wrt.rows();
+  }
+
+  /**
+   * Constructs a Hessian object.
+   *
+   * @param variable Variable of which to compute the Hessian.
+   * @param wrt Vector of variables with respect to which to compute the Hessian.
+   */
+  public Hessian(Variable variable, VariableBlock wrt) {
+    this(variable, new VariableMatrix(wrt));
+  }
+
+  @Override
+  public void close() {
+    if (m_handle != 0) {
+      HessianJNI.destroy(m_handle);
+      m_handle = 0;
+    }
+  }
+
+  /**
+   * Returns the Hessian as a VariableMatrix.
+   *
+   * <p>This is useful when constructing optimization problems with derivatives in them.
+   *
+   * @return The Hessian as a VariableMatrix.
+   */
+  public VariableMatrix get() {
+    return VariableMatrix.fromHandles(m_rows, m_rows, HessianJNI.get(m_handle));
+  }
+
+  /**
+   * Evaluates the Hessian at wrt's value.
+   *
+   * @return The Hessian at wrt's value.
+   */
+  public SimpleMatrix value() {
+    var triplet = HessianJNI.value(m_handle);
+    return new SimpleMatrix(DConvertMatrixStruct.convert(triplet, (DMatrixSparseCSC) null));
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/autodiff/HessianJNI.java

@@ -0,0 +1,48 @@
+// 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.
+
+package edu.wpi.first.math.autodiff;
+
+import edu.wpi.first.math.jni.WPIMathJNI;
+import org.ejml.data.DMatrixSparseTriplet;
+
+/** Hessian JNI functions. */
+public final class HessianJNI extends WPIMathJNI {
+  private HessianJNI() {
+    // Utility class.
+  }
+
+  /**
+   * Constructs a Hessian object.
+   *
+   * @param variable Variable of which to compute the Hessian.
+   * @param wrt Vector of variables with respect to which to compute the Hessian.
+   */
+  static native long create(long variable, long[] wrt);
+
+  /**
+   * Destructs a Hessian.
+   *
+   * @param handle Hessian handle.
+   */
+  static native void destroy(long handle);
+
+  /**
+   * Returns the Hessian as an array of Variable handles.
+   *
+   * <p>This is useful when constructing optimization problems with derivatives in them.
+   *
+   * @param handle Hessian handle.
+   * @return The Hessian as an array of Variable handles.
+   */
+  static native long[] get(long handle);
+
+  /**
+   * Evaluates the Hessian at wrt's value.
+   *
+   * @param handle Hessian handle.
+   * @return The Hessian at wrt's value.
+   */
+  static native DMatrixSparseTriplet value(long handle);
+}