001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math; 006 007import edu.wpi.first.util.RuntimeLoader; 008import java.io.IOException; 009import java.util.concurrent.atomic.AtomicBoolean; 010 011/** WPIMath JNI. */ 012public final class WPIMathJNI { 013 static boolean libraryLoaded = false; 014 static RuntimeLoader<WPIMathJNI> loader = null; 015 016 static { 017 if (Helper.getExtractOnStaticLoad()) { 018 try { 019 loader = 020 new RuntimeLoader<>( 021 "wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class); 022 loader.loadLibrary(); 023 } catch (IOException ex) { 024 ex.printStackTrace(); 025 System.exit(1); 026 } 027 libraryLoaded = true; 028 } 029 } 030 031 /** 032 * Force load the library. 033 * 034 * @throws IOException If the library could not be loaded or found. 035 */ 036 public static synchronized void forceLoad() throws IOException { 037 if (libraryLoaded) { 038 return; 039 } 040 loader = 041 new RuntimeLoader<>( 042 "wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class); 043 loader.loadLibrary(); 044 libraryLoaded = true; 045 } 046 047 // ArmFeedforward wrappers 048 049 /** 050 * Obtain a feedforward voltage from a single jointed arm feedforward object. 051 * 052 * <p>Constructs an ArmFeedforward object and runs its currentVelocity and nextVelocity overload 053 * 054 * @param ks The ArmFeedforward's static gain in volts. 055 * @param kv The ArmFeedforward's velocity gain in volt seconds per radian. 056 * @param ka The ArmFeedforward's acceleration gain in volt seconds² per radian. 057 * @param kg The ArmFeedforward's gravity gain in volts. 058 * @param currentAngle The current angle in the calculation in radians. 059 * @param currentVelocity The current velocity in the calculation in radians per second. 060 * @param nextVelocity The next velocity in the calculation in radians per second. 061 * @param dt The time between velocity setpoints in seconds. 062 * @return The calculated feedforward in volts. 063 */ 064 public static native double calculate( 065 double ks, 066 double kv, 067 double ka, 068 double kg, 069 double currentAngle, 070 double currentVelocity, 071 double nextVelocity, 072 double dt); 073 074 // DARE wrappers 075 076 /** 077 * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation. 078 * 079 * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 080 * 081 * <p>This internal function skips expensive precondition checks for increased performance. The 082 * solver may hang if any of the following occur: 083 * 084 * <ul> 085 * <li>Q isn't symmetric positive semidefinite 086 * <li>R isn't symmetric positive definite 087 * <li>The (A, B) pair isn't stabilizable 088 * <li>The (A, C) pair where Q = CᵀC isn't detectable 089 * </ul> 090 * 091 * <p>Only use this function if you're sure the preconditions are met. Solves the discrete 092 * alegebraic Riccati equation. 093 * 094 * @param A Array containing elements of A in row-major order. 095 * @param B Array containing elements of B in row-major order. 096 * @param Q Array containing elements of Q in row-major order. 097 * @param R Array containing elements of R in row-major order. 098 * @param states Number of states in A matrix. 099 * @param inputs Number of inputs in B matrix. 100 * @param S Array storage for DARE solution. 101 */ 102 public static native void dareDetailABQR( 103 double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S); 104 105 /** 106 * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation. 107 * 108 * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 109 * 110 * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the 111 * following cost function subject to xₖ₊₁ = Axₖ + Buₖ. 112 * 113 * <pre> 114 * ∞ [xₖ]ᵀ[Q N][xₖ] 115 * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT 116 * k=0 117 * </pre> 118 * 119 * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback 120 * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ: 121 * 122 * <pre> 123 * ∞ 124 * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT 125 * k=0 126 * </pre> 127 * 128 * <p>This can be refactored as: 129 * 130 * <pre> 131 * ∞ [xₖ]ᵀ[Q 0][xₖ] 132 * J = Σ [uₖ] [0 R][uₖ] ΔT 133 * k=0 134 * </pre> 135 * 136 * <p>This internal function skips expensive precondition checks for increased performance. The 137 * solver may hang if any of the following occur: 138 * 139 * <ul> 140 * <li>Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite 141 * <li>R isn't symmetric positive definite 142 * <li>The (A − BR⁻¹Nᵀ, B) pair isn't stabilizable 143 * <li>The (A, C) pair where Q = CᵀC isn't detectable 144 * </ul> 145 * 146 * <p>Only use this function if you're sure the preconditions are met. 147 * 148 * @param A Array containing elements of A in row-major order. 149 * @param B Array containing elements of B in row-major order. 150 * @param Q Array containing elements of Q in row-major order. 151 * @param R Array containing elements of R in row-major order. 152 * @param N Array containing elements of N in row-major order. 153 * @param states Number of states in A matrix. 154 * @param inputs Number of inputs in B matrix. 155 * @param S Array storage for DARE solution. 156 */ 157 public static native void dareDetailABQRN( 158 double[] A, 159 double[] B, 160 double[] Q, 161 double[] R, 162 double[] N, 163 int states, 164 int inputs, 165 double[] S); 166 167 /** 168 * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation. 169 * 170 * <p>AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 171 * 172 * @param A Array containing elements of A in row-major order. 173 * @param B Array containing elements of B in row-major order. 174 * @param Q Array containing elements of Q in row-major order. 175 * @param R Array containing elements of R in row-major order. 176 * @param states Number of states in A matrix. 177 * @param inputs Number of inputs in B matrix. 178 * @param S Array storage for DARE solution. 179 * @throws IllegalArgumentException if Q isn't symmetric positive semidefinite. 180 * @throws IllegalArgumentException if R isn't symmetric positive definite. 181 * @throws IllegalArgumentException if the (A, B) pair isn't stabilizable. 182 * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable. 183 */ 184 public static native void dareABQR( 185 double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S); 186 187 /** 188 * Computes the unique stabilizing solution X to the discrete-time algebraic Riccati equation. 189 * 190 * <p>AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 191 * 192 * <p>This overload of the DARE is useful for finding the control law uₖ that minimizes the 193 * following cost function subject to xₖ₊₁ = Axₖ + Buₖ. 194 * 195 * <pre> 196 * ∞ [xₖ]ᵀ[Q N][xₖ] 197 * J = Σ [uₖ] [Nᵀ R][uₖ] ΔT 198 * k=0 199 * </pre> 200 * 201 * <p>This is a more general form of the following. The linear-quadratic regulator is the feedback 202 * control law uₖ that minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ: 203 * 204 * <pre> 205 * ∞ 206 * J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT 207 * k=0 208 * </pre> 209 * 210 * <p>This can be refactored as: 211 * 212 * <pre> 213 * ∞ [xₖ]ᵀ[Q 0][xₖ] 214 * J = Σ [uₖ] [0 R][uₖ] ΔT 215 * k=0 216 * </pre> 217 * 218 * @param A Array containing elements of A in row-major order. 219 * @param B Array containing elements of B in row-major order. 220 * @param Q Array containing elements of Q in row-major order. 221 * @param R Array containing elements of R in row-major order. 222 * @param N Array containing elements of N in row-major order. 223 * @param states Number of states in A matrix. 224 * @param inputs Number of inputs in B matrix. 225 * @param S Array storage for DARE solution. 226 * @throws IllegalArgumentException if Q − NR⁻¹Nᵀ isn't symmetric positive semidefinite. 227 * @throws IllegalArgumentException if R isn't symmetric positive definite. 228 * @throws IllegalArgumentException if the (A − BR⁻¹Nᵀ, B) pair isn't stabilizable. 229 * @throws IllegalArgumentException if the (A, C) pair where Q = CᵀC isn't detectable. 230 */ 231 public static native void dareABQRN( 232 double[] A, 233 double[] B, 234 double[] Q, 235 double[] R, 236 double[] N, 237 int states, 238 int inputs, 239 double[] S); 240 241 // Eigen wrappers 242 243 /** 244 * Computes the matrix exp. 245 * 246 * @param src Array of elements of the matrix to be exponentiated. 247 * @param rows How many rows there are. 248 * @param dst Array where the result will be stored. 249 */ 250 public static native void exp(double[] src, int rows, double[] dst); 251 252 /** 253 * Computes the matrix pow. 254 * 255 * @param src Array of elements of the matrix to be raised to a power. 256 * @param rows How many rows there are. 257 * @param exponent The exponent. 258 * @param dst Array where the result will be stored. 259 */ 260 public static native void pow(double[] src, int rows, double exponent, double[] dst); 261 262 /** 263 * Performs an inplace rank one update (or downdate) of an upper triangular Cholesky decomposition 264 * matrix. 265 * 266 * @param mat Array of elements of the matrix to be updated. 267 * @param lowerTriangular Whether mat is lower triangular. 268 * @param rows How many rows there are. 269 * @param vec Vector to use for the rank update. 270 * @param sigma Sigma value to use for the rank update. 271 */ 272 public static native void rankUpdate( 273 double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular); 274 275 /** 276 * Solves the least-squares problem Ax=B using a QR decomposition with full pivoting. 277 * 278 * @param A Array of elements of the A matrix. 279 * @param Arows Number of rows of the A matrix. 280 * @param Acols Number of rows of the A matrix. 281 * @param B Array of elements of the B matrix. 282 * @param Brows Number of rows of the B matrix. 283 * @param Bcols Number of rows of the B matrix. 284 * @param dst Array to store solution in. If A is m-n and B is m-p, dst is n-p. 285 */ 286 public static native void solveFullPivHouseholderQr( 287 double[] A, int Arows, int Acols, double[] B, int Brows, int Bcols, double[] dst); 288 289 // Pose3d wrappers 290 291 /** 292 * Obtain a Pose3d from a (constant curvature) velocity. 293 * 294 * <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz]. 295 * 296 * @param poseX The pose's translational X component. 297 * @param poseY The pose's translational Y component. 298 * @param poseZ The pose's translational Z component. 299 * @param poseQw The pose quaternion's W component. 300 * @param poseQx The pose quaternion's X component. 301 * @param poseQy The pose quaternion's Y component. 302 * @param poseQz The pose quaternion's Z component. 303 * @param twistDx The twist's dx value. 304 * @param twistDy The twist's dy value. 305 * @param twistDz The twist's dz value. 306 * @param twistRx The twist's rx value. 307 * @param twistRy The twist's ry value. 308 * @param twistRz The twist's rz value. 309 * @return The new pose as a double array. 310 */ 311 public static native double[] expPose3d( 312 double poseX, 313 double poseY, 314 double poseZ, 315 double poseQw, 316 double poseQx, 317 double poseQy, 318 double poseQz, 319 double twistDx, 320 double twistDy, 321 double twistDz, 322 double twistRx, 323 double twistRy, 324 double twistRz); 325 326 /** 327 * Returns a Twist3d that maps the starting pose to the end pose. 328 * 329 * <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz]. 330 * 331 * @param startX The starting pose's translational X component. 332 * @param startY The starting pose's translational Y component. 333 * @param startZ The starting pose's translational Z component. 334 * @param startQw The starting pose quaternion's W component. 335 * @param startQx The starting pose quaternion's X component. 336 * @param startQy The starting pose quaternion's Y component. 337 * @param startQz The starting pose quaternion's Z component. 338 * @param endX The ending pose's translational X component. 339 * @param endY The ending pose's translational Y component. 340 * @param endZ The ending pose's translational Z component. 341 * @param endQw The ending pose quaternion's W component. 342 * @param endQx The ending pose quaternion's X component. 343 * @param endQy The ending pose quaternion's Y component. 344 * @param endQz The ending pose quaternion's Z component. 345 * @return The twist that maps start to end as a double array. 346 */ 347 public static native double[] logPose3d( 348 double startX, 349 double startY, 350 double startZ, 351 double startQw, 352 double startQx, 353 double startQy, 354 double startQz, 355 double endX, 356 double endY, 357 double endZ, 358 double endQw, 359 double endQx, 360 double endQy, 361 double endQz); 362 363 // StateSpaceUtil wrappers 364 365 /** 366 * Returns true if (A, B) is a stabilizable pair. 367 * 368 * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have 369 * absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B) 370 * < n where n is the number of states. 371 * 372 * @param states the number of states of the system. 373 * @param inputs the number of inputs to the system. 374 * @param A System matrix. 375 * @param B Input matrix. 376 * @return If the system is stabilizable. 377 */ 378 public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B); 379 380 // Trajectory wrappers 381 382 /** 383 * Loads a Pathweaver JSON. 384 * 385 * @param path The path to the JSON. 386 * @return A double array with the trajectory states from the JSON. 387 * @throws IOException if the JSON could not be read. 388 */ 389 public static native double[] fromPathweaverJson(String path) throws IOException; 390 391 /** 392 * Converts a trajectory into a Pathweaver JSON and saves it. 393 * 394 * @param elements The elements of the trajectory. 395 * @param path The location to save the JSON to. 396 * @throws IOException if the JSON could not be written. 397 */ 398 public static native void toPathweaverJson(double[] elements, String path) throws IOException; 399 400 /** 401 * Deserializes a trajectory JSON into a double[] of trajectory elements. 402 * 403 * @param json The JSON containing the serialized trajectory. 404 * @return A double array with the trajectory states. 405 */ 406 public static native double[] deserializeTrajectory(String json); 407 408 /** 409 * Serializes the trajectory into a JSON string. 410 * 411 * @param elements The elements of the trajectory. 412 * @return A JSON containing the serialized trajectory. 413 */ 414 public static native String serializeTrajectory(double[] elements); 415 416 /** Sets whether JNI should be loaded in the static block. */ 417 public static class Helper { 418 private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); 419 420 /** 421 * Returns true if the JNI should be loaded in the static block. 422 * 423 * @return True if the JNI should be loaded in the static block. 424 */ 425 public static boolean getExtractOnStaticLoad() { 426 return extractOnStaticLoad.get(); 427 } 428 429 /** 430 * Sets whether the JNI should be loaded in the static block. 431 * 432 * @param load Whether the JNI should be loaded in the static block. 433 */ 434 public static void setExtractOnStaticLoad(boolean load) { 435 extractOnStaticLoad.set(load); 436 } 437 438 /** Utility class. */ 439 private Helper() {} 440 } 441 442 /** Utility class. */ 443 private WPIMathJNI() {} 444}