[wpimath] Make LTV controller constructors use faster DARE solver (#5543)

Made JNI modifications to expose the faster function, made the API use
the typesafe Matrix API, and synchronized the documentation with C++.

Sped up C++ LTV diff drive test from 20 ms to 15 ms.
Sped up C++ LTV unicycle test from 15 ms to 10 ms.

wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp

@@ -7,9 +7,11 @@
 #include <cmath>
 #include <stdexcept>
 
+#include "Eigen/Cholesky"
+#include "frc/DARE.h"
 #include "frc/MathUtil.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
 
 using namespace frc;
 
@@ -64,6 +66,7 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
   // Ax = -Bu
   // x = -A⁻¹Bu
   units::meters_per_second_t maxV{
+      // NOLINTNEXTLINE(clang-analyzer-unix.Malloc)
       -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
 
   if (maxV <= 0_mps) {
@@ -75,16 +78,27 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
         "Max velocity of plant with 12 V input must be less than 15 m/s.");
   }
 
+  auto R_llt = R.llt();
+
   for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.
     if (units::math::abs(velocity) < 1e-4_mps) {
-      m_table.insert(velocity, Matrixd<2, 5>::Zero());
+      A(State::kY, State::kHeading) = 1e-4;
     } else {
       A(State::kY, State::kHeading) = velocity.value();
-      m_table.insert(velocity,
-                     frc::LinearQuadraticRegulator<5, 2>{A, B, Q, R, dt}.K());
     }
+
+    Matrixd<5, 5> discA;
+    Matrixd<5, 2> discB;
+    DiscretizeAB(A, B, dt, &discA, &discB);
+
+    Matrixd<5, 5> S = detail::DARE<5, 2>(discA, discB, Q, R_llt);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_table.insert(velocity, (discB.transpose() * S * discB + R)
+                                 .llt()
+                                 .solve(discB.transpose() * S * discA));
   }
 }
 

wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp

@@ -6,8 +6,10 @@
 
 #include <stdexcept>
 
+#include "Eigen/Cholesky"
+#include "frc/DARE.h"
 #include "frc/StateSpaceUtil.h"
-#include "frc/controller/LinearQuadraticRegulator.h"
+#include "frc/system/Discretization.h"
 #include "units/math.h"
 
 using namespace frc;
@@ -83,17 +85,28 @@ LTVUnicycleController::LTVUnicycleController(
   Matrixd<3, 3> Q = frc::MakeCostMatrix(Qelems);
   Matrixd<2, 2> R = frc::MakeCostMatrix(Relems);
 
+  auto R_llt = R.llt();
+
   for (auto velocity = -maxVelocity; velocity < maxVelocity;
        velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.
     if (units::math::abs(velocity) < 1e-4_mps) {
-      m_table.insert(velocity, Matrixd<2, 3>::Zero());
+      A(State::kY, State::kHeading) = 1e-4;
     } else {
       A(State::kY, State::kHeading) = velocity.value();
-      m_table.insert(velocity,
-                     frc::LinearQuadraticRegulator<3, 2>{A, B, Q, R, dt}.K());
     }
+
+    Matrixd<3, 3> discA;
+    Matrixd<3, 2> discB;
+    DiscretizeAB(A, B, dt, &discA, &discB);
+
+    Matrixd<3, 3> S = detail::DARE<3, 2>(discA, discB, Q, R_llt);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_table.insert(velocity, (discB.transpose() * S * discB + R)
+                                 .llt()
+                                 .solve(discB.transpose() * S * discA));
   }
 }
 

wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp

@@ -62,6 +62,84 @@ frc::Trajectory CreateTrajectoryFromElements(std::span<const double> elements) {
 
 extern "C" {
 
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareDetailABQR
+ * Signature: ([D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQR
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jint states, jint inputs, jdoubleArray S)
+{
+  JDoubleArrayRef nativeA{env, A};
+  JDoubleArrayRef nativeB{env, B};
+  JDoubleArrayRef nativeQ{env, Q};
+  JDoubleArrayRef nativeR{env, R};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.array().data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.array().data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.array().data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.array().data(), inputs, inputs};
+
+  Eigen::MatrixXd RmatCopy{Rmat};
+  auto R_llt = RmatCopy.llt();
+
+  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+      Amat, Bmat, Qmat, R_llt);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    dareDetailABQRN
+ * Signature: ([D[D[D[D[DII[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_dareDetailABQRN
+  (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
+   jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
+{
+  JDoubleArrayRef nativeA{env, A};
+  JDoubleArrayRef nativeB{env, B};
+  JDoubleArrayRef nativeQ{env, Q};
+  JDoubleArrayRef nativeR{env, R};
+  JDoubleArrayRef nativeN{env, N};
+
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.array().data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.array().data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.array().data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.array().data(), inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Nmat{nativeN.array().data(), states, inputs};
+
+  Eigen::MatrixXd Rcopy{Rmat};
+  auto R_llt = Rcopy.llt();
+
+  Eigen::MatrixXd result = frc::detail::DARE<Eigen::Dynamic, Eigen::Dynamic>(
+      Amat, Bmat, Qmat, R_llt, Nmat);
+
+  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+}
+
 /*
  * Class:     edu_wpi_first_math_WPIMathJNI
  * Method:    dareABQR
@@ -72,33 +150,28 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQR
   (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
    jdoubleArray R, jint states, jint inputs, jdoubleArray S)
 {
-  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
-  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
-  jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
-  jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
+  JDoubleArrayRef nativeA{env, A};
+  JDoubleArrayRef nativeB{env, B};
+  JDoubleArrayRef nativeQ{env, Q};
+  JDoubleArrayRef nativeR{env, R};
 
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{nativeA, states, states};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Bmat{nativeB, states, inputs};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Qmat{nativeQ, states, states};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Rmat{nativeR, inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.array().data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.array().data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.array().data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.array().data(), inputs, inputs};
 
   try {
     Eigen::MatrixXd result =
         frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat);
 
-    env->ReleaseDoubleArrayElements(A, nativeA, 0);
-    env->ReleaseDoubleArrayElements(B, nativeB, 0);
-    env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
-    env->ReleaseDoubleArrayElements(R, nativeR, 0);
-
     env->SetDoubleArrayRegion(S, 0, states * states, result.data());
   } catch (const std::invalid_argument& e) {
     jclass cls = env->FindClass("java/lang/IllegalArgumentException");
@@ -118,38 +191,32 @@ Java_edu_wpi_first_math_WPIMathJNI_dareABQRN
   (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q,
    jdoubleArray R, jdoubleArray N, jint states, jint inputs, jdoubleArray S)
 {
-  jdouble* nativeA = env->GetDoubleArrayElements(A, nullptr);
-  jdouble* nativeB = env->GetDoubleArrayElements(B, nullptr);
-  jdouble* nativeQ = env->GetDoubleArrayElements(Q, nullptr);
-  jdouble* nativeR = env->GetDoubleArrayElements(R, nullptr);
-  jdouble* nativeN = env->GetDoubleArrayElements(N, nullptr);
+  JDoubleArrayRef nativeA{env, A};
+  JDoubleArrayRef nativeB{env, B};
+  JDoubleArrayRef nativeQ{env, Q};
+  JDoubleArrayRef nativeR{env, R};
+  JDoubleArrayRef nativeN{env, N};
 
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Amat{nativeA, states, states};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Bmat{nativeB, states, inputs};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Qmat{nativeQ, states, states};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Rmat{nativeR, inputs, inputs};
-  Eigen::Map<
-      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
-      Nmat{nativeN, states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Amat{nativeA.array().data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Bmat{nativeB.array().data(), states, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Qmat{nativeQ.array().data(), states, states};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Rmat{nativeR.array().data(), inputs, inputs};
+  Eigen::Map<const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
+                                 Eigen::RowMajor>>
+      Nmat{nativeN.array().data(), states, inputs};
 
   try {
     Eigen::MatrixXd result =
         frc::DARE<Eigen::Dynamic, Eigen::Dynamic>(Amat, Bmat, Qmat, Rmat, Nmat);
 
-    env->ReleaseDoubleArrayElements(A, nativeA, 0);
-    env->ReleaseDoubleArrayElements(B, nativeB, 0);
-    env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
-    env->ReleaseDoubleArrayElements(R, nativeR, 0);
-    env->ReleaseDoubleArrayElements(N, nativeN, 0);
-
     env->SetDoubleArrayRegion(S, 0, states * states, result.data());
   } catch (const std::invalid_argument& e) {
     jclass cls = env->FindClass("java/lang/IllegalArgumentException");