Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11: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

8
wpimath/src/main/java/edu/wpi/first/math/DARE.java
View File

@@ -38,13 +38,13 @@ public final class DARE {
* @param R Input cost matrix.
* @return Solution of DARE.
*/
public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
public static <States extends Num, Inputs extends Num> Matrix<States, States> dareNoPrecond(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R) {
var S = new Matrix<States, States>(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 final class DARE {
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
*/
public static <States extends Num, Inputs extends Num> Matrix<States, States> dareDetail(
public static <States extends Num, Inputs extends Num> Matrix<States, States> dareNoPrecond(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N) {
var S = new Matrix<States, States>(new SimpleMatrix(A.getNumRows(), A.getNumCols()));
DAREJNI.dareDetailABQRN(
DAREJNI.dareNoPrecondABQRN(
A.getStorage().getDDRM().getData(),
B.getStorage().getDDRM().getData(),
Q.getStorage().getDDRM().getData(),

2
wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
View File

@@ -163,7 +163,7 @@ public class 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(

2
wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
View File

@@ -166,7 +166,7 @@ public class 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(

4
wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java
View File

@@ -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 final class DAREJNI extends WPIMathJNI {
* @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,

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
/**