Package edu.wpi.first.math

Class WPIMathJNI

java.lang.Object
edu.wpi.first.math.WPIMathJNI
public final class WPIMathJNI
extends Object
WPIMath JNI.

  • Nested Class Summary

    Nested Classes 
    Modifier and Type Class Description
    static class  WPIMathJNI.Helper
    Sets whether JNI should be loaded in the static block.

  • Method Summary

    Modifier and Type Method Description
    static 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.
    static void dareABQRN​(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.
    static 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.
    static 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.
    static double[] deserializeTrajectory​(String json)
    Deserializes a trajectory JSON into a double[] of trajectory elements.
    static void exp​(double[] src, int rows, double[] dst)
    Computes the matrix exp.
    static 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)
    Obtain a Pose3d from a (constant curvature) velocity.
    static void forceLoad()
    Force load the library.
    static double[] fromPathweaverJson​(String path)
    Loads a Pathweaver JSON.
    static boolean isStabilizable​(int states, int inputs, double[] A, double[] B)
    Returns true if (A, B) is a stabilizable pair.
    static 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)
    Returns a Twist3d that maps the starting pose to the end pose.
    static void pow​(double[] src, int rows, double exponent, double[] dst)
    Computes the matrix pow.
    static void rankUpdate​(double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular)
    Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition matrix.
    static String serializeTrajectory​(double[] elements)
    Serializes the trajectory into a JSON string.
    static void solveFullPivHouseholderQr​(double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst)
    Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
    static void toPathweaverJson​(double[] elements, String path)
    Converts a trajectory into a Pathweaver JSON and saves it.

    Methods inherited from class java.lang.Object

    clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

  • Method Details

    • forceLoad

      public static void forceLoad() throws IOException
      Force load the library.
      Throws:
      IOException - If the library could not be loaded or found.

    • dareDetailABQR

      public static 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.

      AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0

      This internal function skips expensive precondition checks for increased performance. The solver may hang if any of the following occur:

      • Q isn't symmetric positive semidefinite
      • R isn't symmetric positive definite
      • The (A, B) pair isn't stabilizable
      • The (A, C) pair where Q = CᵀC isn't detectable

      Only use this function if you're sure the preconditions are met. Solves the discrete alegebraic Riccati equation.

      Parameters:
      A - Array containing elements of A in row-major order.
      B - Array containing elements of B in row-major order.
      Q - Array containing elements of Q in row-major order.
      R - Array containing elements of R in row-major order.
      states - Number of states in A matrix.
      inputs - Number of inputs in B matrix.
      S - Array storage for DARE solution.

    • dareDetailABQRN

      public static 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.

      AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0

      This overload of the DARE is useful for finding the control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ. 

           ∞ [xₖ]ᵀ[Q  N][xₖ]
 J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
    k=0

      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ₖ: 

           ∞
 J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
    k=0

      This can be refactored as: 

           ∞ [xₖ]ᵀ[Q 0][xₖ]
 J = Σ [uₖ] [0 R][uₖ] ΔT
    k=0

      This internal function skips expensive precondition checks for increased performance. The solver may hang if any of the following occur:

      • Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite
      • R isn't symmetric positive definite
      • The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable
      • The (A, C) pair where Q = CᵀC isn't detectable

      Only use this function if you're sure the preconditions are met.

      Parameters:
      A - Array containing elements of A in row-major order.
      B - Array containing elements of B in row-major order.
      Q - Array containing elements of Q in row-major order.
      R - Array containing elements of R in row-major order.
      N - Array containing elements of N in row-major order.
      states - Number of states in A matrix.
      inputs - Number of inputs in B matrix.
      S - Array storage for DARE solution.

    • dareABQR

      public static 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.

      AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0

      Parameters:
      A - Array containing elements of A in row-major order.
      B - Array containing elements of B in row-major order.
      Q - Array containing elements of Q in row-major order.
      R - Array containing elements of R in row-major order.
      states - Number of states in A matrix.
      inputs - Number of inputs in B matrix.
      S - Array storage for DARE solution.
      Throws:
      IllegalArgumentException - if Q isn't symmetric positive semidefinite.
      IllegalArgumentException - if R isn't symmetric positive definite.
      IllegalArgumentException - if the (A, B) pair isn't stabilizable.
      IllegalArgumentException - if the (A, C) pair where Q = CᵀC isn't detectable.

    • dareABQRN

      public static void dareABQRN​(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.

      AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0

      This overload of the DARE is useful for finding the control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ. 

           ∞ [xₖ]ᵀ[Q  N][xₖ]
 J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
    k=0

      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ₖ: 

           ∞
 J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
    k=0

      This can be refactored as: 

           ∞ [xₖ]ᵀ[Q 0][xₖ]
 J = Σ [uₖ] [0 R][uₖ] ΔT
    k=0
      Parameters:
      A - Array containing elements of A in row-major order.
      B - Array containing elements of B in row-major order.
      Q - Array containing elements of Q in row-major order.
      R - Array containing elements of R in row-major order.
      N - Array containing elements of N in row-major order.
      states - Number of states in A matrix.
      inputs - Number of inputs in B matrix.
      S - Array storage for DARE solution.
      Throws:
      IllegalArgumentException - if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite.
      IllegalArgumentException - if R isn't symmetric positive definite.
      IllegalArgumentException - if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable.
      IllegalArgumentException - if the (A, C) pair where Q = CᵀC isn't detectable.

    • exp

      public static void exp​(double[] src, int rows, double[] dst)
      Computes the matrix exp.
      Parameters:
      src - Array of elements of the matrix to be exponentiated.
      rows - How many rows there are.
      dst - Array where the result will be stored.

    • pow

      public static void pow​(double[] src, int rows, double exponent, double[] dst)
      Computes the matrix pow.
      Parameters:
      src - Array of elements of the matrix to be raised to a power.
      rows - How many rows there are.
      exponent - The exponent.
      dst - Array where the result will be stored.

    • rankUpdate

      public static void rankUpdate​(double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular)
      Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition matrix.
      Parameters:
      mat - Array of elements of the matrix to be updated.
      lowerTriangular - Whether mat is lower triangular.
      rows - How many rows there are.
      vec - Vector to use for the rank update.
      sigma - Sigma value to use for the rank update.

    • solveFullPivHouseholderQr

      public static void solveFullPivHouseholderQr​(double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst)
      Solves the least-squares problem Ax=B using a QR decomposition with full pivoting.
      Parameters:
      A - Array of elements of the A matrix.
      Arows - Number of rows of the A matrix.
      Acols - Number of rows of the A matrix.
      B - Array of elements of the B matrix.
      Brows - Number of rows of the B matrix.
      Bcols - Number of rows of the B matrix.
      dst - Array to store solution in. If A is m-n and B is m-p, dst is n-p.

    • expPose3d

      public static 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)
      Obtain a Pose3d from a (constant curvature) velocity.

      The double array returned is of the form [dx, dy, dz, qx, qy, qz].

      Parameters:
      poseX - The pose's translational X component.
      poseY - The pose's translational Y component.
      poseZ - The pose's translational Z component.
      poseQw - The pose quaternion's W component.
      poseQx - The pose quaternion's X component.
      poseQy - The pose quaternion's Y component.
      poseQz - The pose quaternion's Z component.
      twistDx - The twist's dx value.
      twistDy - The twist's dy value.
      twistDz - The twist's dz value.
      twistRx - The twist's rx value.
      twistRy - The twist's ry value.
      twistRz - The twist's rz value.
      Returns:
      The new pose as a double array.

    • logPose3d

      public static 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)
      Returns a Twist3d that maps the starting pose to the end pose.

      The double array returned is of the form [dx, dy, dz, rx, ry, rz].

      Parameters:
      startX - The starting pose's translational X component.
      startY - The starting pose's translational Y component.
      startZ - The starting pose's translational Z component.
      startQw - The starting pose quaternion's W component.
      startQx - The starting pose quaternion's X component.
      startQy - The starting pose quaternion's Y component.
      startQz - The starting pose quaternion's Z component.
      endX - The ending pose's translational X component.
      endY - The ending pose's translational Y component.
      endZ - The ending pose's translational Z component.
      endQw - The ending pose quaternion's W component.
      endQx - The ending pose quaternion's X component.
      endQy - The ending pose quaternion's Y component.
      endQz - The ending pose quaternion's Z component.
      Returns:
      The twist that maps start to end as a double array.

    • isStabilizable

      public static boolean isStabilizable​(int states, int inputs, double[] A, double[] B)
      Returns true if (A, B) is a stabilizable pair.

      (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.

      Parameters:
      states - the number of states of the system.
      inputs - the number of inputs to the system.
      A - System matrix.
      B - Input matrix.
      Returns:
      If the system is stabilizable.

    • fromPathweaverJson

      public static double[] fromPathweaverJson​(String path) throws IOException
      Loads a Pathweaver JSON.
      Parameters:
      path - The path to the JSON.
      Returns:
      A double array with the trajectory states from the JSON.
      Throws:
      IOException - if the JSON could not be read.

    • toPathweaverJson

      public static void toPathweaverJson​(double[] elements, String path) throws IOException
      Converts a trajectory into a Pathweaver JSON and saves it.
      Parameters:
      elements - The elements of the trajectory.
      path - The location to save the JSON to.
      Throws:
      IOException - if the JSON could not be written.

    • deserializeTrajectory

      public static double[] deserializeTrajectory​(String json)
      Deserializes a trajectory JSON into a double[] of trajectory elements.
      Parameters:
      json - The JSON containing the serialized trajectory.
      Returns:
      A double array with the trajectory states.

    • serializeTrajectory

      public static String serializeTrajectory​(double[] elements)
      Serializes the trajectory into a JSON string.
      Parameters:
      elements - The elements of the trajectory.
      Returns:
      A JSON containing the serialized trajectory.