Skip to content

Commit

Permalink
[wpimath] Remove redundant internal DARE function (wpilibsuite#7442)
Browse files Browse the repository at this point in the history
  • Loading branch information
@calcmogul
calcmogul authored Nov 29, 2024
1 parent f377a9c commit a0af0fd
Show file tree
Hide file tree
Showing 6 changed files with 18 additions and 105 deletions.
8 changes: 4 additions & 4 deletions wpimath/src/main/java/edu/wpi/first/math/DARE.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,13 @@ private 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(),
Expand Down Expand Up @@ -115,14 +115,14 @@ public static <States extends Num, Inputs extends Num> Matrix<States, States> da
* @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(),
Expand Down
2 changes: 1 addition & 1 deletion wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
2 changes: 1 addition & 1 deletion wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down
4 changes: 2 additions & 2 deletions wpimath/src/main/java/edu/wpi/first/math/jni/DAREJNI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

/**
Expand Down Expand Up @@ -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,
Expand Down
24 changes: 10 additions & 14 deletions wpimath/src/main/native/cpp/jni/DAREJNI.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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)
{
Expand All @@ -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());
}
Expand Down
83 changes: 0 additions & 83 deletions wpimath/src/main/native/include/frc/DARE.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

/**
Expand Down

0 comments on commit a0af0fd

Please sign in to comment.