diff --git a/wpimath/src/main/java/edu/wpi/first/math/DARE.java b/wpimath/src/main/java/edu/wpi/first/math/DARE.java index 504a863279b..00ca8a0cae9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/DARE.java +++ b/wpimath/src/main/java/edu/wpi/first/math/DARE.java @@ -38,13 +38,13 @@ private DARE() { * @param R Input cost matrix. * @return Solution of DARE. */ - public static Matrix dareDetail( + public static Matrix dareNoPrecond( Matrix A, Matrix B, Matrix Q, Matrix R) { var S = new Matrix(new SimpleMatrix(A.getNumRows(), A.getNumCols())); - DAREJNI.dareDetailABQR( + DAREJNI.dareNoPrecondABQR( A.getStorage().getDDRM().getData(), B.getStorage().getDDRM().getData(), Q.getStorage().getDDRM().getData(), @@ -115,14 +115,14 @@ public static Matrix da * @param N State-input cross-term cost matrix. * @return Solution of DARE. */ - public static Matrix dareDetail( + public static Matrix dareNoPrecond( Matrix A, Matrix B, Matrix Q, Matrix R, Matrix N) { var S = new Matrix(new SimpleMatrix(A.getNumRows(), A.getNumCols())); - DAREJNI.dareDetailABQRN( + DAREJNI.dareNoPrecondABQRN( A.getStorage().getDDRM().getData(), B.getStorage().getDDRM().getData(), Q.getStorage().getDDRM().getData(), diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index 116513eb7f1..c5b6defa0d1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -163,7 +163,7 @@ public LTVDifferentialDriveController( var discA = discABPair.getFirst(); var discB = discABPair.getSecond(); - var S = DARE.dareDetail(discA, discB, Q, R); + var S = DARE.dareNoPrecond(discA, discB, Q, R); // K = (BᵀSB + R)⁻¹BᵀSA m_table.put( diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index c3c54f32129..8437eafac8e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -166,7 +166,7 @@ public LTVUnicycleController( var discA = discABPair.getFirst(); var discB = discABPair.getSecond(); - var S = DARE.dareDetail(discA, discB, Q, R); + var S = DARE.dareNoPrecond(discA, discB, Q, R); // K = (BᵀSB + R)⁻¹BᵀSA m_table.put( diff --git a/wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java b/wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java index 72f3db7171e..7794309b86d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java @@ -32,7 +32,7 @@ public final class DAREJNI extends WPIMathJNI { * @param inputs Number of inputs in B matrix. * @param S Array storage for DARE solution. */ - public static native void dareDetailABQR( + public static native void dareNoPrecondABQR( double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S); /** @@ -87,7 +87,7 @@ public static native void dareDetailABQR( * @param inputs Number of inputs in B matrix. * @param S Array storage for DARE solution. */ - public static native void dareDetailABQRN( + public static native void dareNoPrecondABQRN( double[] A, double[] B, double[] Q, diff --git a/wpimath/src/main/native/cpp/jni/DAREJNI.cpp b/wpimath/src/main/native/cpp/jni/DAREJNI.cpp index 5220be31d6b..b5d441dee73 100644 --- a/wpimath/src/main/native/cpp/jni/DAREJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/DAREJNI.cpp @@ -20,11 +20,11 @@ extern "C" { /* * Class: edu_wpi_first_math_jni_DAREJNI - * Method: dareDetailABQR + * Method: dareNoPrecondABQR * Signature: ([D[D[D[DII[D)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQR +Java_edu_wpi_first_math_jni_DAREJNI_dareNoPrecondABQR (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q, jdoubleArray R, jint states, jint inputs, jdoubleArray S) { @@ -46,22 +46,20 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQR Eigen::RowMajor>> Rmat{nativeR.data(), inputs, inputs}; - Eigen::MatrixXd RmatCopy{Rmat}; - auto R_llt = RmatCopy.llt(); - - auto result = frc::detail::DARE(Amat, Bmat, - Qmat, R_llt); + auto result = + frc::DARE(Amat, Bmat, Qmat, Rmat, false) + .value(); env->SetDoubleArrayRegion(S, 0, states * states, result.data()); } /* * Class: edu_wpi_first_math_jni_DAREJNI - * Method: dareDetailABQRN + * Method: dareNoPrecondABQRN * Signature: ([D[D[D[D[DII[D)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQRN +Java_edu_wpi_first_math_jni_DAREJNI_dareNoPrecondABQRN (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q, jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S) { @@ -87,11 +85,9 @@ Java_edu_wpi_first_math_jni_DAREJNI_dareDetailABQRN Eigen::RowMajor>> Nmat{nativeN.data(), states, inputs}; - Eigen::MatrixXd Rcopy{Rmat}; - auto R_llt = Rcopy.llt(); - - auto result = frc::detail::DARE( - Amat, Bmat, Qmat, R_llt, Nmat); + auto result = frc::DARE(Amat, Bmat, Qmat, + Rmat, Nmat, false) + .value(); env->SetDoubleArrayRegion(S, 0, states * states, result.data()); } diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h index 0c5ba7f1043..fba06ccb0ab 100644 --- a/wpimath/src/main/native/include/frc/DARE.h +++ b/wpimath/src/main/native/include/frc/DARE.h @@ -143,89 +143,6 @@ Eigen::Matrix DARE( return H_k1; } -/** -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 is equivalent to solving the original DARE: - - A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 - -where A₂ and Q₂ are a change of variables: - - A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ - -This overload of the DARE is useful for finding the control law uₖ that -minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ. - -@verbatim - ∞ [xₖ]ᵀ[Q N][xₖ] -J = Σ [uₖ] [Nᵀ R][uₖ] ΔT - k=0 -@endverbatim - -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ₖ: - -@verbatim - ∞ -J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT - k=0 -@endverbatim - -This can be refactored as: - -@verbatim - ∞ [xₖ]ᵀ[Q 0][xₖ] -J = Σ [uₖ] [0 R][uₖ] ΔT - k=0 -@endverbatim - -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. - -@tparam States Number of states. -@tparam Inputs Number of inputs. -@param A The system matrix. -@param B The input matrix. -@param Q The state cost matrix. -@param R_llt The LLT decomposition of the input cost matrix. -@param N The state-input cross cost matrix. -@return Solution to the DARE. -*/ -template -Eigen::Matrix DARE( - const Eigen::Matrix& A, - const Eigen::Matrix& B, - const Eigen::Matrix& Q, - const Eigen::LLT>& R_llt, - const Eigen::Matrix& N) { - // This is a change of variables to make the DARE that includes Q, R, and N - // cost matrices fit the form of the DARE that includes only Q and R cost - // matrices. - // - // This is equivalent to solving the original DARE: - // - // A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 - // - // where A₂ and Q₂ are a change of variables: - // - // A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ - return detail::DARE(A - B * R_llt.solve(N.transpose()), B, - Q - N * R_llt.solve(N.transpose()), - R_llt); -} - } // namespace detail /**