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

[wpimath] Add DARE solver for Q, R, and N with LQR ctor overloads

Browse Source 
This is useful for implementing implicit model following.
This commit is contained in:
Tyler Veness
2021-02-15 18:17:55 -08:00
committed by Peter Johnson
parent b2c3b2dd8e
commit edd2f0232c
6 changed files with 216 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
wpimath/src/main/java/edu/wpi/first/math/Drake.java
View File

@@ -53,4 +53,64 @@ public final class Drake {
discreteAlgebraicRiccatiEquation(
A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
*/
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public static SimpleMatrix discreteAlgebraicRiccatiEquation(
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
// See
// https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
// for the change of variables used here.
var scrA = A.minus(B.mult(R.solve(N.transpose())));
var scrQ = Q.minus(N.mult(R.solve(N.transpose())));
var S = new SimpleMatrix(A.numRows(), A.numCols());
WPIMathJNI.discreteAlgebraicRiccatiEquation(
scrA.getDDRM().getData(),
B.getDDRM().getData(),
scrQ.getDDRM().getData(),
R.getDDRM().getData(),
A.numCols(),
B.numCols(),
S.getDDRM().getData());
return S;
}
/**
* Solves the discrete alegebraic Riccati equation.
*
* @param A System matrix.
* @param B Input matrix.
* @param Q State cost matrix.
* @param R Input cost matrix.
* @param N State-input cross-term cost matrix.
* @return Solution of DARE.
*/
@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
public static <States extends Num, Inputs extends Num>
Matrix<States, States> discreteAlgebraicRiccatiEquation(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N) {
// See
// https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
// for the change of variables used here.
var scrA = A.minus(B.times(R.solve(N.transpose())));
var scrQ = Q.minus(N.times(R.solve(N.transpose())));
return new Matrix<>(
discreteAlgebraicRiccatiEquation(
scrA.getStorage(), B.getStorage(), scrQ.getStorage(), R.getStorage()));
}
}

34
wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
View File

@@ -113,6 +113,40 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
reset();
}
/**
* Constructs a controller with the given coefficients and plant.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param N The state-input cross-term cost matrix.
* @param dtSeconds Discretization timestep.
*/
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public LinearQuadraticRegulator(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N,
double dtSeconds) {
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
var temp = discB.transpose().times(S).times(discB).plus(R);
m_K = temp.solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
reset();
}
/**
* Constructs a controller with the given coefficients and plant.
*

12
wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
View File

@@ -19,6 +19,12 @@ LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 1, 1>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
@@ -32,4 +38,10 @@ LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N, units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
} // namespace frc

13
wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
View File

@@ -456,5 +456,18 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
return X;
}
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N) {
DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
Eigen::MatrixXd scrA = A - B * R.llt().solve(N.transpose());
Eigen::MatrixXd scrQ = Q - N * R.llt().solve(N.transpose());
return DiscreteAlgebraicRiccatiEquation(scrA, B, scrQ, R);
}
} // namespace math
} // namespace drake

26
wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
View File

@@ -5,8 +5,7 @@
#include <Eigen/Core>
namespace drake {
namespace math {
namespace drake::math {
/// Computes the unique stabilizing solution X to the discrete-time algebraic
/// Riccati equation:
@@ -27,6 +26,25 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R);
} // namespace math
} // namespace drake
/// DiscreteAlgebraicRiccatiEquation function
/// computes the unique stabilizing solution X to the discrete-time algebraic
/// Riccati equation:
/// \f[
/// A'XA - X - (A'XB + N)(B'XB + R)^{-1}(B'XA + N') + Q = 0
/// \f]
///
/// See
/// https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
/// for the change of variables used here.
///
/// @throws std::runtime_error if Q is not positive semi-definite.
/// @throws std::runtime_error if R is not positive definite.
///
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N);
} // namespace drake::math

88
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
View File

@@ -69,11 +69,11 @@ class LinearQuadraticRegulatorImpl {
/**
* Constructs a controller with the given coefficients and plant.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dt Discretization timestep.
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
@@ -86,9 +86,38 @@ class LinearQuadraticRegulatorImpl {
Eigen::Matrix<double, States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
Eigen::Matrix<double, Inputs, Inputs> tmp =
discB.transpose() * S * discB + R;
m_K = tmp.llt().solve(discB.transpose() * S * discA);
m_K = (discB.transpose() * S * discB + R)
.llt()
.solve(discB.transpose() * S * discA);
Reset();
}
/**
* Constructs a controller with the given coefficients and plant.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param N The state-input cross-term cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, Inputs>& N,
units::second_t dt) {
Eigen::Matrix<double, States, States> discA;
Eigen::Matrix<double, States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Eigen::Matrix<double, States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
m_K = (B.transpose() * S * B + R)
.llt()
.solve(discB.transpose() * S * discA + N.transpose());
Reset();
}
@@ -251,11 +280,11 @@ class LinearQuadraticRegulator
/**
* Constructs a controller with the given coefficients and plant.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dt Discretization timestep.
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
@@ -264,6 +293,25 @@ class LinearQuadraticRegulator
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
/**
* Constructs a controller with the given coefficients and plant.
*
* @param A Continuous system matrix of the plant being controlled.
* @param B Continuous input matrix of the plant being controlled.
* @param Q The state cost matrix.
* @param R The input cost matrix.
* @param N The state-input cross-term cost matrix.
* @param dt Discretization timestep.
*/
LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
const Eigen::Matrix<double, States, Inputs>& B,
const Eigen::Matrix<double, States, States>& Q,
const Eigen::Matrix<double, Inputs, Inputs>& R,
const Eigen::Matrix<double, States, Inputs>& N,
units::second_t dt)
: detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q,
R, N, dt} {}
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
};
@@ -293,6 +341,13 @@ class LinearQuadraticRegulator<1, 1>
const Eigen::Matrix<double, 1, 1>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
const Eigen::Matrix<double, 1, 1>& B,
const Eigen::Matrix<double, 1, 1>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 1, 1>& N,
units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
};
@@ -322,6 +377,13 @@ class LinearQuadraticRegulator<2, 1>
const Eigen::Matrix<double, 1, 1>& R,
units::second_t dt);
LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
const Eigen::Matrix<double, 2, 1>& B,
const Eigen::Matrix<double, 2, 2>& Q,
const Eigen::Matrix<double, 1, 1>& R,
const Eigen::Matrix<double, 2, 1>& N,
units::second_t dt);
LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
};