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

This is useful for implementing implicit model following.

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 <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()));
+  }
 }

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

@@ -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.
    *

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

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

wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h

@@ -5,8 +5,7 @@
 
 #include <Eigen/Core>
 
-namespace drake {
+namespace drake::math {
-namespace 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

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 A  Continuous system matrix of the plant being controlled.
-   * @param B      Continuous input matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
-   * @param Q      The state cost matrix.
+   * @param Q  The state cost matrix.
-   * @param R      The input cost matrix.
+   * @param R  The input cost matrix.
-   * @param dt     Discretization timestep.
+   * @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 =
+    m_K = (discB.transpose() * S * discB + R)
-        discB.transpose() * S * discB + R;
+              .llt()
-    m_K = tmp.llt().solve(discB.transpose() * S * discA);
+              .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 A  Continuous system matrix of the plant being controlled.
-   * @param B      Continuous input matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
-   * @param Q      The state cost matrix.
+   * @param Q  The state cost matrix.
-   * @param R      The input cost matrix.
+   * @param R  The input cost matrix.
-   * @param dt     Discretization timestep.
+   * @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;
 };