diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/Drake.java index 886f7da6f4..817e663134 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java +++ b/wpimath/src/main/java/edu/wpi/first/math/Drake.java @@ -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 + Matrix discreteAlgebraicRiccatiEquation( + Matrix A, + Matrix B, + Matrix Q, + Matrix R, + Matrix 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())); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java index 71631b67c5..4dc9577c05 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java @@ -113,6 +113,40 @@ public class LinearQuadraticRegulator A, + Matrix B, + Matrix Q, + Matrix R, + Matrix 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. * diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp index 140ffaa138..c3a00eadd8 100644 --- a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp +++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp @@ -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& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N, units::second_t dt) + : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {} + LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( const Eigen::Matrix& A, const Eigen::Matrix& B, const wpi::array& Qelems, const wpi::array& 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& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& N, units::second_t dt) + : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {} + } // namespace frc diff --git a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp index 3f78218cce..cf5267f423 100644 --- a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp +++ b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp @@ -456,5 +456,18 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( return X; } +Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( + const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& 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 diff --git a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h index 9ce76fce01..fc5efb313f 100644 --- a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h +++ b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h @@ -5,8 +5,7 @@ #include -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& B, const Eigen::Ref& Q, const Eigen::Ref& 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& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& N); +} // namespace drake::math diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index 9662710c1e..2101a3499d 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -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& A, const Eigen::Matrix& B, @@ -86,9 +86,38 @@ class LinearQuadraticRegulatorImpl { Eigen::Matrix S = drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R); - Eigen::Matrix 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& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + const Eigen::Matrix& N, + units::second_t dt) { + Eigen::Matrix discA; + Eigen::Matrix discB; + DiscretizeAB(A, B, dt, &discA, &discB); + + Eigen::Matrix 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& A, const Eigen::Matrix& B, @@ -264,6 +293,25 @@ class LinearQuadraticRegulator units::second_t dt) : detail::LinearQuadraticRegulatorImpl{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& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + const Eigen::Matrix& N, + units::second_t dt) + : detail::LinearQuadraticRegulatorImpl{A, B, Q, + R, N, dt} {} + LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; }; @@ -293,6 +341,13 @@ class LinearQuadraticRegulator<1, 1> const Eigen::Matrix& R, units::second_t dt); + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + const Eigen::Matrix& N, + units::second_t dt); + LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; }; @@ -322,6 +377,13 @@ class LinearQuadraticRegulator<2, 1> const Eigen::Matrix& R, units::second_t dt); + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + const Eigen::Matrix& N, + units::second_t dt); + LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; };