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