mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpimath] Disambiguate wpimath JNI functions (#6695)
Each collection of JNI functions now has its own class.
This commit is contained in:
Tyler Veness
@@ -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(),
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
|
||||
|
||||
@@ -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)
|
||||
* < 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() {}
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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(),
|
||||
|
||||
@@ -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(),
|
||||
|
||||
@@ -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
233
wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java
Normal 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
114
wpimath/src/main/java/edu/wpi/first/math/jni/EigenJNI.java
Normal 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() {}
|
||||
}
|
||||
@@ -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
140
wpimath/src/main/java/edu/wpi/first/math/jni/Pose3dJNI.java
Normal 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() {}
|
||||
}
|
||||
@@ -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)
|
||||
* < 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() {}
|
||||
}
|
||||
@@ -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() {}
|
||||
}
|
||||
@@ -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. */
|
||||
|
||||
Reference in New Issue
Block a user