Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove redundant internal DARE function (#7442)

Browse Source
This commit is contained in:
Tyler Veness
2024-11-28 21:24:13 -08:00
committed by GitHub
parent f377a9c573
commit a0af0fd572
6 changed files with 18 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
wpimath/src/main/native/cpp/jni/DAREJNI.cpp
View File

@@ -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<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat,
Qmat, R_llt);
auto result =
frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(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<Eigen::Dynamic, Eigen::Dynamic>(
Amat, Bmat, Qmat, R_llt, Nmat);
auto result = frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat,
Rmat, Nmat, false)
.value();
env->SetDoubleArrayRegion(S, 0, states * states, result.data());
}

83
wpimath/src/main/native/include/frc/DARE.h
View File

@@ -143,89 +143,6 @@ Eigen::Matrix<double, States, States> 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:
<ul>
<li>Q₂ isn't symmetric positive semidefinite</li>
<li>R isn't symmetric positive definite</li>
<li>The (A₂, B) pair isn't stabilizable</li>
<li>The (A₂, C) pair where Q₂ = CᵀC isn't detectable</li>
</ul>
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 <int States, int Inputs>
Eigen::Matrix<double, States, States> DARE(
const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::LLT<Eigen::Matrix<double, Inputs, Inputs>>& R_llt,
const Eigen::Matrix<double, Inputs, Inputs>& 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<States, Inputs>(A - B * R_llt.solve(N.transpose()), B,
Q - N * R_llt.solve(N.transpose()),
R_llt);
}
} // namespace detail
/**