Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Disambiguate wpimath JNI functions (#6695)

Browse Source 
Each collection of JNI functions now has its own class.
This commit is contained in:
Tyler Veness
2024-06-05 12:26:58 -07:00
committed by GitHub
parent df4694c9df
commit 5221069bcc
32 changed files with 1038 additions and 543 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
wpimath/CMakeLists.txt
View File

@@ -19,14 +19,14 @@ protobuf_generate_cpp(
file(
GLOB wpimath_jni_src
src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp
src/main/native/cpp/jni/WPIMathJNI_DARE.cpp
src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp
src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp
src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp
src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp
src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp
src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp
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
)
# Java bindings

9
wpimath/src/main/java/edu/wpi/first/math/DARE.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math;
import edu.wpi.first.math.jni.DAREJNI;
import org.ejml.simple.SimpleMatrix;
/** DARE solver utility functions. */
@@ -43,7 +44,7 @@ public final class DARE {
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R) {
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
WPIMathJNI.dareDetailABQR(
DAREJNI.dareDetailABQR(
A.getStorage().getDDRM().getData(),
B.getStorage().getDDRM().getData(),
Q.getStorage().getDDRM().getData(),
@@ -121,7 +122,7 @@ public final class DARE {
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N) {
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
WPIMathJNI.dareDetailABQRN(
DAREJNI.dareDetailABQRN(
A.getStorage().getDDRM().getData(),
B.getStorage().getDDRM().getData(),
Q.getStorage().getDDRM().getData(),
@@ -156,7 +157,7 @@ public final class DARE {
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R) {
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
WPIMathJNI.dareABQR(
DAREJNI.dareABQR(
A.getStorage().getDDRM().getData(),
B.getStorage().getDDRM().getData(),
Q.getStorage().getDDRM().getData(),
@@ -226,7 +227,7 @@ public final class DARE {
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N) {
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
WPIMathJNI.dareABQRN(
DAREJNI.dareABQRN(
A.getStorage().getDDRM().getData(),
B.getStorage().getDDRM().getData(),
Q.getStorage().getDDRM().getData(),

9
wpimath/src/main/java/edu/wpi/first/math/Matrix.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.math;
import edu.wpi.first.math.jni.EigenJNI;
import edu.wpi.first.math.numbers.N1;
import java.util.Objects;
import org.ejml.MatrixDimensionException;
@@ -360,7 +361,7 @@ public class Matrix<R extends Num, C extends Num> {
public final <R2 extends Num, C2 extends Num> Matrix<C, C2> solveFullPivHouseholderQr(
Matrix<R2, C2> other) {
Matrix<C, C2> solution = new Matrix<>(new SimpleMatrix(this.getNumCols(), other.getNumCols()));
WPIMathJNI.solveFullPivHouseholderQr(
EigenJNI.solveFullPivHouseholderQr(
this.getData(),
this.getNumRows(),
this.getNumCols(),
@@ -387,7 +388,7 @@ public class Matrix<R extends Num, C extends Num> {
+ this.getNumCols());
}
Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
WPIMathJNI.exp(
EigenJNI.exp(
this.m_storage.getDDRM().getData(),
this.getNumRows(),
toReturn.m_storage.getDDRM().getData());
@@ -411,7 +412,7 @@ public class Matrix<R extends Num, C extends Num> {
+ this.getNumCols());
}
Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
WPIMathJNI.pow(
EigenJNI.pow(
this.m_storage.getDDRM().getData(),
this.getNumRows(),
exponent,
@@ -709,7 +710,7 @@ public class Matrix<R extends Num, C extends Num> {
* @param lowerTriangular Whether this matrix is lower triangular.
*/
public void rankUpdate(Matrix<R, N1> v, double sigma, boolean lowerTriangular) {
WPIMathJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
EigenJNI.rankUpdate(this.getData(), this.getNumRows(), v.getData(), sigma, lowerTriangular);
}
@Override

6
wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View File

@@ -5,6 +5,7 @@
package edu.wpi.first.math;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.jni.StateSpaceUtilJNI;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N4;
@@ -102,7 +103,8 @@ public final class StateSpaceUtil {
*/
public static <States extends Num, Inputs extends Num> boolean isStabilizable(
Matrix<States, States> A, Matrix<States, Inputs> B) {
return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
return StateSpaceUtilJNI.isStabilizable(
A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
}
/**
@@ -120,7 +122,7 @@ public final class StateSpaceUtil {
*/
public static <States extends Num, Outputs extends Num> boolean isDetectable(
Matrix<States, States> A, Matrix<Outputs, States> C) {
return WPIMathJNI.isStabilizable(
return StateSpaceUtilJNI.isStabilizable(
A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData());
}

463
wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
View File

@@ -1,463 +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.
package edu.wpi.first.math;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
/** WPIMath JNI. */
public final class WPIMathJNI {
static boolean libraryLoaded = false;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
RuntimeLoader.loadLibrary("wpimathjni");
} catch (Exception 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;
}
RuntimeLoader.loadLibrary("wpimathjni");
libraryLoaded = true;
}
// ArmFeedforward wrappers
/**
* Obtain a feedforward voltage from a single jointed arm feedforward object.
*
* <p>Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload
*
* @param ks The ArmFeedforward's static gain in volts.
* @param kv The ArmFeedforward's velocity gain in volt seconds per radian.
* @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian.
* @param kg The ArmFeedforward's gravity gain in volts.
* @param currentAngle The current angle in the calculation in radians.
* @param currentVelocity The current velocity in the calculation in radians per second.
* @param nextVelocity The next velocity in the calculation in radians per second.
* @param dt The time between velocity setpoints in seconds.
* @return The calculated feedforward in volts.
*/
public static native double calculate(
double ks,
double kv,
double ka,
double kg,
double currentAngle,
double currentVelocity,
double nextVelocity,
double dt);
// DARE wrappers
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
*
* <p>This internal function skips expensive precondition checks for increased performance. The
* solver may hang if any of the following occur:
*
* <ul>
* <li>Q isn't symmetric positive semidefinite
* <li>R isn't symmetric positive definite
* <li>The (A, B) pair isn't stabilizable
* <li>The (A, C) pair where Q = CᵀC isn't detectable
* </ul>
*
* <p>Only use this function if you're sure the preconditions are met. 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 dareDetailABQR(
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
*
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
*
* <pre>
* ∞ [xₖ]ᵀ[Q N][xₖ]
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
* k=0
* </pre>
*
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
*
* <pre>
* ∞
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
* k=0
* </pre>
*
* <p>This can be refactored as:
*
* <pre>
* ∞ [xₖ]ᵀ[Q 0][xₖ]
* J = Σ [uₖ] [0 R][uₖ] ΔT
* k=0
* </pre>
*
* <p>This internal function skips expensive precondition checks for increased performance. The
* solver may hang if any of the following occur:
*
* <ul>
* <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
* <li>R isn't symmetric positive definite
* <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
* <li>The (A, C) pair where Q = CᵀC isn't detectable
* </ul>
*
* <p>Only use this function if you're sure the preconditions are met.
*
* @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 N Array containing elements of N 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 dareDetailABQRN(
double[] A,
double[] B,
double[] Q,
double[] R,
double[] N,
int states,
int inputs,
double[] S);
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
*
* @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.
* @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static native void dareABQR(
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
*
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
*
* <pre>
* ∞ [xₖ]ᵀ[Q N][xₖ]
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
* k=0
* </pre>
*
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
*
* <pre>
* ∞
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
* k=0
* </pre>
*
* <p>This can be refactored as:
*
* <pre>
* ∞ [xₖ]ᵀ[Q 0][xₖ]
* J = Σ [uₖ] [0 R][uₖ] ΔT
* k=0
* </pre>
*
* @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 N Array containing elements of N 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.
* @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static native void dareABQRN(
double[] A,
double[] B,
double[] Q,
double[] R,
double[] N,
int states,
int inputs,
double[] S);
// Eigen wrappers
/**
* Computes the matrix exp.
*
* @param src Array of elements of the matrix to be exponentiated.
* @param rows How many rows there are.
* @param dst Array where the result will be stored.
*/
public static native void exp(double[] src, int rows, double[] dst);
/**
* Computes the matrix pow.
*
* @param src Array of elements of the matrix to be raised to a power.
* @param rows How many rows there are.
* @param exponent The exponent.
* @param dst Array where the result will be stored.
*/
public static native void pow(double[] src, int rows, double exponent, double[] dst);
/**
* Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
* matrix.
*
* @param mat Array of elements of the matrix to be updated.
* @param lowerTriangular Whether mat is lower triangular.
* @param rows How many rows there are.
* @param vec Vector to use for the rank update.
* @param sigma Sigma value to use for the rank update.
*/
public static native void rankUpdate(
double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
/**
* Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
*
* @param A Array of elements of the A matrix.
* @param Arows Number of rows of the A matrix.
* @param Acols Number of rows of the A matrix.
* @param B Array of elements of the B matrix.
* @param Brows Number of rows of the B matrix.
* @param Bcols Number of rows of the B matrix.
* @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
*/
public static native void solveFullPivHouseholderQr(
double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
// Ellipse2d wrappers
/**
* Returns the nearest point that is contained within the ellipse.
*
* <p>Constructs an Ellipse2d object and runs its FindNearestPoint() method.
*
* @param centerX The x coordinate of the center of the ellipse in meters.
* @param centerY The y coordinate of the center of the ellipse in meters.
* @param centerHeading The ellipse's rotation in radians.
* @param xSemiAxis The x semi-axis in meters.
* @param ySemiAxis The y semi-axis in meters.
* @param pointX The x coordinate of the point that this will find the nearest point to.
* @param pointY The y coordinate of the point that this will find the nearest point to.
* @param nearestPoint Array to store nearest point into.
*/
public static native void ellipse2dFindNearestPoint(
double centerX,
double centerY,
double centerHeading,
double xSemiAxis,
double ySemiAxis,
double pointX,
double pointY,
double[] nearestPoint);
// Pose3d wrappers
/**
* Obtain a Pose3d from a (constant curvature) velocity.
*
* <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz].
*
* @param poseX The pose's translational X component.
* @param poseY The pose's translational Y component.
* @param poseZ The pose's translational Z component.
* @param poseQw The pose quaternion's W component.
* @param poseQx The pose quaternion's X component.
* @param poseQy The pose quaternion's Y component.
* @param poseQz The pose quaternion's Z component.
* @param twistDx The twist's dx value.
* @param twistDy The twist's dy value.
* @param twistDz The twist's dz value.
* @param twistRx The twist's rx value.
* @param twistRy The twist's ry value.
* @param twistRz The twist's rz value.
* @return The new pose as a double array.
*/
public static native double[] expPose3d(
double poseX,
double poseY,
double poseZ,
double poseQw,
double poseQx,
double poseQy,
double poseQz,
double twistDx,
double twistDy,
double twistDz,
double twistRx,
double twistRy,
double twistRz);
/**
* Returns a Twist3d that maps the starting pose to the end pose.
*
* <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
*
* @param startX The starting pose's translational X component.
* @param startY The starting pose's translational Y component.
* @param startZ The starting pose's translational Z component.
* @param startQw The starting pose quaternion's W component.
* @param startQx The starting pose quaternion's X component.
* @param startQy The starting pose quaternion's Y component.
* @param startQz The starting pose quaternion's Z component.
* @param endX The ending pose's translational X component.
* @param endY The ending pose's translational Y component.
* @param endZ The ending pose's translational Z component.
* @param endQw The ending pose quaternion's W component.
* @param endQx The ending pose quaternion's X component.
* @param endQy The ending pose quaternion's Y component.
* @param endQz The ending pose quaternion's Z component.
* @return The twist that maps start to end as a double array.
*/
public static native double[] logPose3d(
double startX,
double startY,
double startZ,
double startQw,
double startQx,
double startQy,
double startQz,
double endX,
double endY,
double endZ,
double endQw,
double endQx,
double endQy,
double endQz);
// StateSpaceUtil wrappers
/**
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
* absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
* &lt; n where n is the number of states.
*
* @param states the number of states of the system.
* @param inputs the number of inputs to the system.
* @param A System matrix.
* @param B Input matrix.
* @return If the system is stabilizable.
*/
public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
// Trajectory wrappers
/**
* Loads a Pathweaver JSON.
*
* @param path The path to the JSON.
* @return A double array with the trajectory states from the JSON.
* @throws IOException if the JSON could not be read.
*/
public static native double[] fromPathweaverJson(String path) throws IOException;
/**
* Converts a trajectory into a Pathweaver JSON and saves it.
*
* @param elements The elements of the trajectory.
* @param path The location to save the JSON to.
* @throws IOException if the JSON could not be written.
*/
public static native void toPathweaverJson(double[] elements, String path) throws IOException;
/**
* Deserializes a trajectory JSON into a double[] of trajectory elements.
*
* @param json The JSON containing the serialized trajectory.
* @return A double array with the trajectory states.
*/
public static native double[] deserializeTrajectory(String json);
/**
* Serializes the trajectory into a JSON string.
*
* @param elements The elements of the trajectory.
* @return A JSON containing the serialized trajectory.
*/
public static native String serializeTrajectory(double[] elements);
/** Sets whether JNI should be loaded in the static block. */
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
/**
* Returns true if the JNI should be loaded in the static block.
*
* @return True if the JNI should be loaded in the static block.
*/
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
/**
* Sets whether the JNI should be loaded in the static block.
*
* @param load Whether the JNI should be loaded in the static block.
*/
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
/** Utility class. */
private Helper() {}
}
/** Utility class. */
private WPIMathJNI() {}
}

5
wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
View File

@@ -4,9 +4,9 @@
package edu.wpi.first.math.controller;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.math.controller.proto.ArmFeedforwardProto;
import edu.wpi.first.math.controller.struct.ArmFeedforwardStruct;
import edu.wpi.first.math.jni.ArmFeedforwardJNI;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
@@ -114,7 +114,8 @@ public class ArmFeedforward implements ProtobufSerializable, StructSerializable
*/
public double calculate(
double currentAngle, double currentVelocity, double nextVelocity, double dt) {
return WPIMathJNI.calculate(ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
return ArmFeedforwardJNI.calculate(
ks, kv, ka, kg, currentAngle, currentVelocity, nextVelocity, dt);
}
// Rearranging the main equation from the calculate() method yields the

4
wpimath/src/main/java/edu/wpi/first/math/geometry/Ellipse2d.java
View File

@@ -5,9 +5,9 @@
package edu.wpi.first.math.geometry;
import edu.wpi.first.math.Pair;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.math.geometry.proto.Ellipse2dProto;
import edu.wpi.first.math.geometry.struct.Ellipse2dStruct;
import edu.wpi.first.math.jni.Ellipse2dJNI;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
@@ -172,7 +172,7 @@ public class Ellipse2d implements ProtobufSerializable, StructSerializable {
// Find nearest point
var nearestPoint = new double[2];
WPIMathJNI.ellipse2dFindNearestPoint(
Ellipse2dJNI.findNearestPoint(
m_center.getX(),
m_center.getY(),
m_center.getRotation().getRadians(),

6
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -8,10 +8,10 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
import com.fasterxml.jackson.annotation.JsonProperty;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.math.geometry.proto.Pose3dProto;
import edu.wpi.first.math.geometry.struct.Pose3dStruct;
import edu.wpi.first.math.interpolation.Interpolatable;
import edu.wpi.first.math.jni.Pose3dJNI;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
@@ -224,7 +224,7 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
public Pose3d exp(Twist3d twist) {
var quaternion = this.getRotation().getQuaternion();
double[] resultArray =
WPIMathJNI.expPose3d(
Pose3dJNI.exp(
this.getX(),
this.getY(),
this.getZ(),
@@ -257,7 +257,7 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
var thisQuaternion = this.getRotation().getQuaternion();
var endQuaternion = end.getRotation().getQuaternion();
double[] resultArray =
WPIMathJNI.logPose3d(
Pose3dJNI.log(
this.getX(),
this.getY(),
this.getZ(),

93
wpimath/src/main/java/edu/wpi/first/math/jni/ArmFeedforwardJNI.java Normal file
View File

@@ -0,0 +1,93 @@
// 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.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
/** ArmFeedforward JNI. */
public final class ArmFeedforwardJNI {
static boolean libraryLoaded = false;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
RuntimeLoader.loadLibrary("wpimathjni");
} catch (Exception 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;
}
RuntimeLoader.loadLibrary("wpimathjni");
libraryLoaded = true;
}
/**
* Obtain a feedforward voltage from a single jointed arm feedforward object.
*
* <p>Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload
*
* @param ks The ArmFeedforward's static gain in volts.
* @param kv The ArmFeedforward's velocity gain in volt seconds per radian.
* @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian.
* @param kg The ArmFeedforward's gravity gain in volts.
* @param currentAngle The current angle in the calculation in radians.
* @param currentVelocity The current velocity in the calculation in radians per second.
* @param nextVelocity The next velocity in the calculation in radians per second.
* @param dt The time between velocity setpoints in seconds.
* @return The calculated feedforward in volts.
*/
public static native double calculate(
double ks,
double kv,
double ka,
double kg,
double currentAngle,
double currentVelocity,
double nextVelocity,
double dt);
/** Sets whether JNI should be loaded in the static block. */
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
/**
* Returns true if the JNI should be loaded in the static block.
*
* @return True if the JNI should be loaded in the static block.
*/
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
/**
* Sets whether the JNI should be loaded in the static block.
*
* @param load Whether the JNI should be loaded in the static block.
*/
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
/** Utility class. */
private Helper() {}
}
/** Utility class. */
private ArmFeedforwardJNI() {}
}

233
wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java Normal file
View File

@@ -0,0 +1,233 @@
// 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.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
/** DARE JNI. */
public final class DAREJNI {
static boolean libraryLoaded = false;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
RuntimeLoader.loadLibrary("wpimathjni");
} catch (Exception 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;
}
RuntimeLoader.loadLibrary("wpimathjni");
libraryLoaded = true;
}
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
*
* <p>This internal function skips expensive precondition checks for increased performance. The
* solver may hang if any of the following occur:
*
* <ul>
* <li>Q isn't symmetric positive semidefinite
* <li>R isn't symmetric positive definite
* <li>The (A, B) pair isn't stabilizable
* <li>The (A, C) pair where Q = CᵀC isn't detectable
* </ul>
*
* <p>Only use this function if you're sure the preconditions are met. 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 dareDetailABQR(
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
*
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
*
* <pre>
* ∞ [xₖ]ᵀ[Q N][xₖ]
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
* k=0
* </pre>
*
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
*
* <pre>
* ∞
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
* k=0
* </pre>
*
* <p>This can be refactored as:
*
* <pre>
* ∞ [xₖ]ᵀ[Q 0][xₖ]
* J = Σ [uₖ] [0 R][uₖ] ΔT
* k=0
* </pre>
*
* <p>This internal function skips expensive precondition checks for increased performance. The
* solver may hang if any of the following occur:
*
* <ul>
* <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
* <li>R isn't symmetric positive definite
* <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
* <li>The (A, C) pair where Q = CᵀC isn't detectable
* </ul>
*
* <p>Only use this function if you're sure the preconditions are met.
*
* @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 N Array containing elements of N 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 dareDetailABQRN(
double[] A,
double[] B,
double[] Q,
double[] R,
double[] N,
int states,
int inputs,
double[] S);
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
* <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
*
* @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.
* @throws IllegalArgumentException if Q isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static native void dareABQR(
double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
/**
* Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation.
*
* <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
*
* <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the
* following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
*
* <pre>
* ∞ [xₖ]ᵀ[Q N][xₖ]
* J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
* k=0
* </pre>
*
* <p>This is a more general form of the following. The linear-quadratic regulator is the feedback
* control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ:
*
* <pre>
* ∞
* J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
* k=0
* </pre>
*
* <p>This can be refactored as:
*
* <pre>
* ∞ [xₖ]ᵀ[Q 0][xₖ]
* J = Σ [uₖ] [0 R][uₖ] ΔT
* k=0
* </pre>
*
* @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 N Array containing elements of N 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.
* @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
* @throws IllegalArgumentException if R isn't symmetric positive definite.
* @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
* @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable.
*/
public static native void dareABQRN(
double[] A,
double[] B,
double[] Q,
double[] R,
double[] N,
int states,
int inputs,
double[] S);
/** Sets whether JNI should be loaded in the static block. */
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
/**
* Returns true if the JNI should be loaded in the static block.
*
* @return True if the JNI should be loaded in the static block.
*/
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
/**
* Sets whether the JNI should be loaded in the static block.
*
* @param load Whether the JNI should be loaded in the static block.
*/
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
/** Utility class. */
private Helper() {}
}
/** Utility class. */
private DAREJNI() {}
}

114
wpimath/src/main/java/edu/wpi/first/math/jni/EigenJNI.java Normal file
View File

@@ -0,0 +1,114 @@
// 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.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
/** Eigen JNI. */
public final class EigenJNI {
static boolean libraryLoaded = false;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
RuntimeLoader.loadLibrary("wpimathjni");
} catch (Exception 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;
}
RuntimeLoader.loadLibrary("wpimathjni");
libraryLoaded = true;
}
/**
* Computes the matrix exp.
*
* @param src Array of elements of the matrix to be exponentiated.
* @param rows How many rows there are.
* @param dst Array where the result will be stored.
*/
public static native void exp(double[] src, int rows, double[] dst);
/**
* Computes the matrix pow.
*
* @param src Array of elements of the matrix to be raised to a power.
* @param rows How many rows there are.
* @param exponent The exponent.
* @param dst Array where the result will be stored.
*/
public static native void pow(double[] src, int rows, double exponent, double[] dst);
/**
* Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition
* matrix.
*
* @param mat Array of elements of the matrix to be updated.
* @param lowerTriangular Whether mat is lower triangular.
* @param rows How many rows there are.
* @param vec Vector to use for the rank update.
* @param sigma Sigma value to use for the rank update.
*/
public static native void rankUpdate(
double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
/**
* Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
*
* @param A Array of elements of the A matrix.
* @param Arows Number of rows of the A matrix.
* @param Acols Number of rows of the A matrix.
* @param B Array of elements of the B matrix.
* @param Brows Number of rows of the B matrix.
* @param Bcols Number of rows of the B matrix.
* @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p.
*/
public static native void solveFullPivHouseholderQr(
double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst);
/** Sets whether JNI should be loaded in the static block. */
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
/**
* Returns true if the JNI should be loaded in the static block.
*
* @return True if the JNI should be loaded in the static block.
*/
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
/**
* Sets whether the JNI should be loaded in the static block.
*
* @param load Whether the JNI should be loaded in the static block.
*/
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
/** Utility class. */
private Helper() {}
}
/** Utility class. */
private EigenJNI() {}
}

92
wpimath/src/main/java/edu/wpi/first/math/jni/Ellipse2dJNI.java Normal file
View File

@@ -0,0 +1,92 @@
// 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.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
/** Ellipse2d JNI. */
public final class Ellipse2dJNI {
static boolean libraryLoaded = false;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
RuntimeLoader.loadLibrary("wpimathjni");
} catch (Exception 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;
}
RuntimeLoader.loadLibrary("wpimathjni");
libraryLoaded = true;
}
/**
* Returns the nearest point that is contained within the ellipse.
*
* <p>Constructs an Ellipse2d object and runs its FindNearestPoint() method.
*
* @param centerX The x coordinate of the center of the ellipse in meters.
* @param centerY The y coordinate of the center of the ellipse in meters.
* @param centerHeading The ellipse's rotation in radians.
* @param xSemiAxis The x semi-axis in meters.
* @param ySemiAxis The y semi-axis in meters.
* @param pointX The x coordinate of the point that this will find the nearest point to.
* @param pointY The y coordinate of the point that this will find the nearest point to.
* @param nearestPoint Array to store nearest point into.
*/
public static native void findNearestPoint(
double centerX,
double centerY,
double centerHeading,
double xSemiAxis,
double ySemiAxis,
double pointX,
double pointY,
double[] nearestPoint);
/** Sets whether JNI should be loaded in the static block. */
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
/**
* Returns true if the JNI should be loaded in the static block.
*
* @return True if the JNI should be loaded in the static block.
*/
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
/**
* Sets whether the JNI should be loaded in the static block.
*
* @param load Whether the JNI should be loaded in the static block.
*/
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
/** Utility class. */
private Helper() {}
}
/** Utility class. */
private Ellipse2dJNI() {}
}

140
wpimath/src/main/java/edu/wpi/first/math/jni/Pose3dJNI.java Normal file
View File

@@ -0,0 +1,140 @@
// 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.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
/** Pose3d JNI. */
public final class Pose3dJNI {
static boolean libraryLoaded = false;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
RuntimeLoader.loadLibrary("wpimathjni");
} catch (Exception 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;
}
RuntimeLoader.loadLibrary("wpimathjni");
libraryLoaded = true;
}
/**
* Obtain a Pose3d from a (constant curvature) velocity.
*
* <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz].
*
* @param poseX The pose's translational X component.
* @param poseY The pose's translational Y component.
* @param poseZ The pose's translational Z component.
* @param poseQw The pose quaternion's W component.
* @param poseQx The pose quaternion's X component.
* @param poseQy The pose quaternion's Y component.
* @param poseQz The pose quaternion's Z component.
* @param twistDx The twist's dx value.
* @param twistDy The twist's dy value.
* @param twistDz The twist's dz value.
* @param twistRx The twist's rx value.
* @param twistRy The twist's ry value.
* @param twistRz The twist's rz value.
* @return The new pose as a double array.
*/
public static native double[] exp(
double poseX,
double poseY,
double poseZ,
double poseQw,
double poseQx,
double poseQy,
double poseQz,
double twistDx,
double twistDy,
double twistDz,
double twistRx,
double twistRy,
double twistRz);
/**
* Returns a Twist3d that maps the starting pose to the end pose.
*
* <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
*
* @param startX The starting pose's translational X component.
* @param startY The starting pose's translational Y component.
* @param startZ The starting pose's translational Z component.
* @param startQw The starting pose quaternion's W component.
* @param startQx The starting pose quaternion's X component.
* @param startQy The starting pose quaternion's Y component.
* @param startQz The starting pose quaternion's Z component.
* @param endX The ending pose's translational X component.
* @param endY The ending pose's translational Y component.
* @param endZ The ending pose's translational Z component.
* @param endQw The ending pose quaternion's W component.
* @param endQx The ending pose quaternion's X component.
* @param endQy The ending pose quaternion's Y component.
* @param endQz The ending pose quaternion's Z component.
* @return The twist that maps start to end as a double array.
*/
public static native double[] log(
double startX,
double startY,
double startZ,
double startQw,
double startQx,
double startQy,
double startQz,
double endX,
double endY,
double endZ,
double endQw,
double endQx,
double endQy,
double endQz);
/** Sets whether JNI should be loaded in the static block. */
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
/**
* Returns true if the JNI should be loaded in the static block.
*
* @return True if the JNI should be loaded in the static block.
*/
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
/**
* Sets whether the JNI should be loaded in the static block.
*
* @param load Whether the JNI should be loaded in the static block.
*/
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
/** Utility class. */
private Helper() {}
}
/** Utility class. */
private Pose3dJNI() {}
}

83
wpimath/src/main/java/edu/wpi/first/math/jni/StateSpaceUtilJNI.java Normal file
View File

@@ -0,0 +1,83 @@
// 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.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
/** StateSpaceUtil JNI. */
public final class StateSpaceUtilJNI {
static boolean libraryLoaded = false;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
RuntimeLoader.loadLibrary("wpimathjni");
} catch (Exception 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;
}
RuntimeLoader.loadLibrary("wpimathjni");
libraryLoaded = true;
}
/**
* Returns true if (A, B) is a stabilizable pair.
*
* <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
* absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
* &lt; n where n is the number of states.
*
* @param states the number of states of the system.
* @param inputs the number of inputs to the system.
* @param A System matrix.
* @param B Input matrix.
* @return If the system is stabilizable.
*/
public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
/** Sets whether JNI should be loaded in the static block. */
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
/**
* Returns true if the JNI should be loaded in the static block.
*
* @return True if the JNI should be loaded in the static block.
*/
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
/**
* Sets whether the JNI should be loaded in the static block.
*
* @param load Whether the JNI should be loaded in the static block.
*/
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
/** Utility class. */
private Helper() {}
}
/** Utility class. */
private StateSpaceUtilJNI() {}
}

102
wpimath/src/main/java/edu/wpi/first/math/jni/TrajectoryUtilJNI.java Normal file
View File

@@ -0,0 +1,102 @@
// 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.jni;
import edu.wpi.first.util.RuntimeLoader;
import java.io.IOException;
import java.util.concurrent.atomic.AtomicBoolean;
/** TrajectoryUtil JNI. */
public final class TrajectoryUtilJNI {
static boolean libraryLoaded = false;
static {
if (Helper.getExtractOnStaticLoad()) {
try {
RuntimeLoader.loadLibrary("wpimathjni");
} catch (Exception 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;
}
RuntimeLoader.loadLibrary("wpimathjni");
libraryLoaded = true;
}
/**
* Loads a Pathweaver JSON.
*
* @param path The path to the JSON.
* @return A double array with the trajectory states from the JSON.
* @throws IOException if the JSON could not be read.
*/
public static native double[] fromPathweaverJson(String path) throws IOException;
/**
* Converts a trajectory into a Pathweaver JSON and saves it.
*
* @param elements The elements of the trajectory.
* @param path The location to save the JSON to.
* @throws IOException if the JSON could not be written.
*/
public static native void toPathweaverJson(double[] elements, String path) throws IOException;
/**
* Deserializes a trajectory JSON into a double[] of trajectory elements.
*
* @param json The JSON containing the serialized trajectory.
* @return A double array with the trajectory states.
*/
public static native double[] deserializeTrajectory(String json);
/**
* Serializes the trajectory into a JSON string.
*
* @param elements The elements of the trajectory.
* @return A JSON containing the serialized trajectory.
*/
public static native String serializeTrajectory(double[] elements);
/** Sets whether JNI should be loaded in the static block. */
public static class Helper {
private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
/**
* Returns true if the JNI should be loaded in the static block.
*
* @return True if the JNI should be loaded in the static block.
*/
public static boolean getExtractOnStaticLoad() {
return extractOnStaticLoad.get();
}
/**
* Sets whether the JNI should be loaded in the static block.
*
* @param load Whether the JNI should be loaded in the static block.
*/
public static void setExtractOnStaticLoad(boolean load) {
extractOnStaticLoad.set(load);
}
/** Utility class. */
private Helper() {}
}
/** Utility class. */
private TrajectoryUtilJNI() {}
}

10
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
View File

@@ -4,9 +4,9 @@
package edu.wpi.first.math.trajectory;
import edu.wpi.first.math.WPIMathJNI;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.jni.TrajectoryUtilJNI;
import java.io.IOException;
import java.nio.file.Path;
import java.util.ArrayList;
@@ -76,7 +76,7 @@ public final class TrajectoryUtil {
* @throws IOException if reading from the file fails.
*/
public static Trajectory fromPathweaverJson(Path path) throws IOException {
return createTrajectoryFromElements(WPIMathJNI.fromPathweaverJson(path.toString()));
return createTrajectoryFromElements(TrajectoryUtilJNI.fromPathweaverJson(path.toString()));
}
/**
@@ -87,7 +87,7 @@ public final class TrajectoryUtil {
* @throws IOException if writing to the file fails.
*/
public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
WPIMathJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
TrajectoryUtilJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
}
/**
@@ -98,7 +98,7 @@ public final class TrajectoryUtil {
* @throws TrajectorySerializationException if deserialization of the string fails.
*/
public static Trajectory deserializeTrajectory(String json) {
return createTrajectoryFromElements(WPIMathJNI.deserializeTrajectory(json));
return createTrajectoryFromElements(TrajectoryUtilJNI.deserializeTrajectory(json));
}
/**
@@ -109,7 +109,7 @@ public final class TrajectoryUtil {
* @throws TrajectorySerializationException if serialization of the trajectory fails.
*/
public static String serializeTrajectory(Trajectory trajectory) {
return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
return TrajectoryUtilJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
}
/** Exception for trajectory serialization failure. */

6
wpimath/src/main/native/cpp/jni/WPIMathJNI_ArmFeedforward.cpp → wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
View File

@@ -6,7 +6,7 @@
#include <wpi/jni_util.h>
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "edu_wpi_first_math_jni_ArmFeedforwardJNI.h"
#include "frc/controller/ArmFeedforward.h"
using namespace wpi::java;
@@ -14,12 +14,12 @@ using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_ArmFeedforwardJNI
* Method: calculate
* Signature: (DDDDDDDD)D
*/
JNIEXPORT jdouble JNICALL
Java_edu_wpi_first_math_WPIMathJNI_calculate
Java_edu_wpi_first_math_jni_ArmFeedforwardJNI_calculate
(JNIEnv* env, jclass, jdouble ks, jdouble kv, jdouble ka, jdouble kg,
jdouble currentAngle, jdouble currentVelocity, jdouble nextVelocity,
jdouble dt)

20
wpimath/src/main/native/cpp/jni/WPIMathJNI_DARE.cpp → wpimath/src/main/native/cpp/jni/DAREJNI.cpp
View File

@@ -8,8 +8,8 @@
#include <wpi/jni_util.h>
#include "WPIMathJNI_Exceptions.h"
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "Exceptions.h"
#include "edu_wpi_first_math_jni_DAREJNI.h"
#include "frc/DARE.h"
using namespace wpi::java;
@@ -17,12 +17,12 @@ using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_DAREJNI
* Method: dareDetailABQR
* Signature: ([D[D[D[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQR
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
@@ -54,12 +54,12 @@ Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_DAREJNI
* Method: dareDetailABQRN
* Signature: ([D[D[D[D[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQRN
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
{
@@ -95,12 +95,12 @@ Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_DAREJNI
* Method: dareABQR
* Signature: ([D[D[D[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_dareABQR
Java_edu_wpi_first_math_jni_DAREJNI_dareABQR
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jint states, jint inputs, jdoubleArray S)
{
@@ -133,12 +133,12 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQR
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_DAREJNI
* Method: dareABQRN
* Signature: ([D[D[D[D[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
Java_edu_wpi_first_math_jni_DAREJNI_dareABQRN
(JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
{

18
wpimath/src/main/native/cpp/jni/WPIMathJNI_Eigen.cpp → wpimath/src/main/native/cpp/jni/EigenJNI.cpp
View File

@@ -10,19 +10,19 @@
#include <unsupported/Eigen/MatrixFunctions>
#include <wpi/jni_util.h>
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "edu_wpi_first_math_jni_EigenJNI.h"
using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_EigenJNI
* Method: exp
* Signature: ([DI[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_exp
Java_edu_wpi_first_math_jni_EigenJNI_exp
(JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst)
{
JSpan<const jdouble> arrayBody{env, src};
@@ -37,12 +37,12 @@ Java_edu_wpi_first_math_WPIMathJNI_exp
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_EigenJNI
* Method: pow
* Signature: ([DID[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_pow
Java_edu_wpi_first_math_jni_EigenJNI_pow
(JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
jdoubleArray dst)
{
@@ -58,12 +58,12 @@ Java_edu_wpi_first_math_WPIMathJNI_pow
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_EigenJNI
* Method: rankUpdate
* Signature: ([DI[DDZ)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
Java_edu_wpi_first_math_jni_EigenJNI_rankUpdate
(JNIEnv* env, jclass, jdoubleArray mat, jint rows, jdoubleArray vec,
jdouble sigma, jboolean lowerTriangular)
{
@@ -84,12 +84,12 @@ Java_edu_wpi_first_math_WPIMathJNI_rankUpdate
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_EigenJNI
* Method: solveFullPivHouseholderQr
* Signature: ([DII[DII[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_solveFullPivHouseholderQr
Java_edu_wpi_first_math_jni_EigenJNI_solveFullPivHouseholderQr
(JNIEnv* env, jclass, jdoubleArray A, jint Arows, jint Acols, jdoubleArray B,
jint Brows, jint Bcols, jdoubleArray dst)
{

8
wpimath/src/main/native/cpp/jni/WPIMathJNI_Ellipse2d.cpp → wpimath/src/main/native/cpp/jni/Ellipse2dJNI.cpp
View File

@@ -7,7 +7,7 @@
#include <wpi/array.h>
#include <wpi/jni_util.h>
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "edu_wpi_first_math_jni_Ellipse2dJNI.h"
#include "frc/geometry/Ellipse2d.h"
using namespace wpi::java;
@@ -15,12 +15,12 @@ using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: ellipse2dFindNearestPoint
* Class: edu_wpi_first_math_jni_Ellipse2dJNI
* Method: findNearestPoint
* Signature: (DDDDDDD[D)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_ellipse2dFindNearestPoint
Java_edu_wpi_first_math_jni_Ellipse2dJNI_findNearestPoint
(JNIEnv* env, jclass, jdouble centerX, jdouble centerY, jdouble centerHeading,
jdouble xSemiAxis, jdouble ySemiAxis, jdouble pointX, jdouble pointY,
jdoubleArray nearestPoint)

2
wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.cpp → wpimath/src/main/native/cpp/jni/Exceptions.cpp
View File

@@ -2,7 +2,7 @@
// 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 "WPIMathJNI_Exceptions.h"
#include "Exceptions.h"
#include <jni.h>

0
wpimath/src/main/native/cpp/jni/WPIMathJNI_Exceptions.h → wpimath/src/main/native/cpp/jni/Exceptions.h
View File

14
wpimath/src/main/native/cpp/jni/WPIMathJNI_Pose3d.cpp → wpimath/src/main/native/cpp/jni/Pose3dJNI.cpp
View File

@@ -6,7 +6,7 @@
#include <wpi/jni_util.h>
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "edu_wpi_first_math_jni_Pose3dJNI.h"
#include "frc/geometry/Pose3d.h"
using namespace wpi::java;
@@ -14,12 +14,12 @@ using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: expPose3d
* Class: edu_wpi_first_math_jni_Pose3dJNI
* Method: exp
* Signature: (DDDDDDDDDDDDD)[D
*/
JNIEXPORT jdoubleArray JNICALL
Java_edu_wpi_first_math_WPIMathJNI_expPose3d
Java_edu_wpi_first_math_jni_Pose3dJNI_exp
(JNIEnv* env, jclass, jdouble poseX, jdouble poseY, jdouble poseZ,
jdouble poseQw, jdouble poseQx, jdouble poseQy, jdouble poseQz,
jdouble twistDx, jdouble twistDy, jdouble twistDz, jdouble twistRx,
@@ -42,12 +42,12 @@ Java_edu_wpi_first_math_WPIMathJNI_expPose3d
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Method: logPose3d
* Class: edu_wpi_first_math_jni_Pose3dJNI
* Method: log
* Signature: (DDDDDDDDDDDDDD)[D
*/
JNIEXPORT jdoubleArray JNICALL
Java_edu_wpi_first_math_WPIMathJNI_logPose3d
Java_edu_wpi_first_math_jni_Pose3dJNI_log
(JNIEnv* env, jclass, jdouble startX, jdouble startY, jdouble startZ,
jdouble startQw, jdouble startQx, jdouble startQy, jdouble startQz,
jdouble endX, jdouble endY, jdouble endZ, jdouble endQw, jdouble endQx,

6
wpimath/src/main/native/cpp/jni/WPIMathJNI_StateSpaceUtil.cpp → wpimath/src/main/native/cpp/jni/StateSpaceUtilJNI.cpp
View File

@@ -7,7 +7,7 @@
#include <Eigen/Core>
#include <wpi/jni_util.h>
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "edu_wpi_first_math_jni_StateSpaceUtilJNI.h"
#include "frc/StateSpaceUtil.h"
using namespace wpi::java;
@@ -15,12 +15,12 @@ using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_StateSpaceUtilJNI
* Method: isStabilizable
* Signature: (II[D[D)Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_math_WPIMathJNI_isStabilizable
Java_edu_wpi_first_math_jni_StateSpaceUtilJNI_isStabilizable
(JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc,
jdoubleArray bSrc)
{

20
wpimath/src/main/native/cpp/jni/WPIMathJNI_Trajectory.cpp → wpimath/src/main/native/cpp/jni/TrajectoryUtilJNI.cpp
View File

@@ -8,8 +8,8 @@
#include <wpi/jni_util.h>
#include "WPIMathJNI_Exceptions.h"
#include "edu_wpi_first_math_WPIMathJNI.h"
#include "Exceptions.h"
#include "edu_wpi_first_math_jni_TrajectoryUtilJNI.h"
#include "frc/trajectory/TrajectoryUtil.h"
using namespace wpi::java;
@@ -57,12 +57,12 @@ frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
extern "C" {
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_TrajectoryUtilJNI
* Method: fromPathweaverJson
* Signature: (Ljava/lang/String;)[D
*/
JNIEXPORT jdoubleArray JNICALL
Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
Java_edu_wpi_first_math_jni_TrajectoryUtilJNI_fromPathweaverJson
(JNIEnv* env, jclass, jstring path)
{
try {
@@ -77,12 +77,12 @@ Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_TrajectoryUtilJNI
* Method: toPathweaverJson
* Signature: ([DLjava/lang/String;)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
Java_edu_wpi_first_math_jni_TrajectoryUtilJNI_toPathweaverJson
(JNIEnv* env, jclass, jdoubleArray elements, jstring path)
{
try {
@@ -96,12 +96,12 @@ Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_TrajectoryUtilJNI
* Method: deserializeTrajectory
* Signature: (Ljava/lang/String;)[D
*/
JNIEXPORT jdoubleArray JNICALL
Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
Java_edu_wpi_first_math_jni_TrajectoryUtilJNI_deserializeTrajectory
(JNIEnv* env, jclass, jstring json)
{
try {
@@ -116,12 +116,12 @@ Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
}
/*
* Class: edu_wpi_first_math_WPIMathJNI
* Class: edu_wpi_first_math_jni_TrajectoryUtilJNI
* Method: serializeTrajectory
* Signature: ([D)Ljava/lang/String;
*/
JNIEXPORT jstring JNICALL
Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
Java_edu_wpi_first_math_jni_TrajectoryUtilJNI_serializeTrajectory
(JNIEnv* env, jclass, jdoubleArray elements)
{
try {

16
wpimath/src/test/java/edu/wpi/first/math/jni/ArmFeedforwardJNITest.java Normal file
View File

@@ -0,0 +1,16 @@
// 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.jni;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import org.junit.jupiter.api.Test;
public class ArmFeedforwardJNITest {
@Test
public void testLink() {
assertDoesNotThrow(ArmFeedforwardJNI::forceLoad);
}
}

6
wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java → wpimath/src/test/java/edu/wpi/first/math/jni/DAREJNITest.java
View File

@@ -2,15 +2,15 @@
// 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;
package edu.wpi.first.math.jni;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import org.junit.jupiter.api.Test;
public class WPIMathJNITest {
public class DAREJNITest {
@Test
public void testLink() {
assertDoesNotThrow(WPIMathJNI::forceLoad);
assertDoesNotThrow(DAREJNI::forceLoad);
}
}

16
wpimath/src/test/java/edu/wpi/first/math/jni/EigenJNITest.java Normal file
View File

@@ -0,0 +1,16 @@
// 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.jni;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import org.junit.jupiter.api.Test;
public class EigenJNITest {
@Test
public void testLink() {
assertDoesNotThrow(EigenJNI::forceLoad);
}
}

16
wpimath/src/test/java/edu/wpi/first/math/jni/Ellipse2dJNITest.java Normal file
View File

@@ -0,0 +1,16 @@
// 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.jni;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import org.junit.jupiter.api.Test;
public class Ellipse2dJNITest {
@Test
public void testLink() {
assertDoesNotThrow(Ellipse2dJNI::forceLoad);
}
}

16
wpimath/src/test/java/edu/wpi/first/math/jni/Pose3dJNITest.java Normal file
View File

@@ -0,0 +1,16 @@
// 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.jni;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import org.junit.jupiter.api.Test;
public class Pose3dJNITest {
@Test
public void testLink() {
assertDoesNotThrow(Pose3dJNI::forceLoad);
}
}

16
wpimath/src/test/java/edu/wpi/first/math/jni/StateSpaceUtilJNITest.java Normal file
View File

@@ -0,0 +1,16 @@
// 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.jni;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import org.junit.jupiter.api.Test;
public class StateSpaceUtilJNITest {
@Test
public void testLink() {
assertDoesNotThrow(StateSpaceUtilJNI::forceLoad);
}
}

16
wpimath/src/test/java/edu/wpi/first/math/jni/TrajectoryUtilJNITest.java Normal file
View File

@@ -0,0 +1,16 @@
// 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.jni;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import org.junit.jupiter.api.Test;
public class TrajectoryUtilJNITest {
@Test
public void testLink() {
assertDoesNotThrow(TrajectoryUtilJNI::forceLoad);
}
}