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