[wpimath] Move DrakeJNI to edu.wpi.first.math.WPIMathJNI

2026-06-21 01:01:43 +00:00 · 2020-08-07 09:38:13 -07:00
parent 42993b15c6
commit 8dc3d23831
6 changed files with 17 additions and 15 deletions
--- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/Drake.java
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+import org.ejml.simple.SimpleMatrix;
+
+import edu.wpi.first.wpiutil.math.Matrix;
+
+public final class Drake {
+  private Drake() {
+  }
+
+  /**
+   * Solves the discrete alegebraic Riccati equation.
+   *
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public static SimpleMatrix discreteAlgebraicRiccatiEquation(
+          SimpleMatrix A,
+          SimpleMatrix B,
+          SimpleMatrix Q,
+          SimpleMatrix R) {
+    var S = new SimpleMatrix(A.numRows(), A.numCols());
+    WPIMathJNI.discreteAlgebraicRiccatiEquation(A.getDDRM().getData(), B.getDDRM().getData(),
+            Q.getDDRM().getData(), R.getDDRM().getData(), A.numCols(), B.numCols(),
+            S.getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Solves the discrete alegebraic Riccati equation.
+   *
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   */
+  @SuppressWarnings("ParameterName")
+  public static SimpleMatrix discreteAlgebraicRiccatiEquation(
+          Matrix A,
+          Matrix B,
+          Matrix Q,
+          Matrix R) {
+    return discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(), Q.getStorage(),
+            R.getStorage());
+  }
+}
--- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.math;
+
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+import edu.wpi.first.wpiutil.RuntimeLoader;
+
+public final class WPIMathJNI {
+  static boolean libraryLoaded = false;
+  static RuntimeLoader<WPIMathJNI> loader = null;
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
+                WPIMathJNI.class);
+        loader.loadLibrary();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException If the library could not be loaded or found.
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
+            WPIMathJNI.class);
+    loader.loadLibrary();
+    libraryLoaded = true;
+  }
+
+  /**
+   * Solves the discrete alegebraic Riccati equation.
+   *
+   * @param A      Array containing elements of A in row-major order.
+   * @param B      Array containing elements of B in row-major order.
+   * @param Q      Array containing elements of Q in row-major order.
+   * @param R      Array containing elements of R in row-major order.
+   * @param states Number of states in A matrix.
+   * @param inputs Number of inputs in B matrix.
+   * @param S      Array storage for DARE solution.
+   */
+  public static native void discreteAlgebraicRiccatiEquation(
+          double[] A,
+          double[] B,
+          double[] Q,
+          double[] R,
+          int states,
+          int inputs,
+          double[] S);
+
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+  }
+}