[wpimath] Refactor DARE tests to reduce RAM usage at compile time (#5557)

2026-06-20 00:51:42 +00:00 · 2023-08-23 10:46:50 -07:00
parent 89e738262c
commit 7c20fa1b18
7 changed files with 282 additions and 183 deletions
--- a/wpimath/src/test/native/cpp/DARETestUtil.cpp
+++ b/wpimath/src/test/native/cpp/DARETestUtil.cpp
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "DARETestUtil.h"
+
+#include <fmt/core.h>
+
+#include "Eigen/Eigenvalues"
+#include "frc/fmt/Eigen.h"
+#include "gtest/gtest.h"
+
+void ExpectMatrixEqual(const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs,
+                       double tolerance) {
+  for (int row = 0; row < lhs.rows(); ++row) {
+    for (int col = 0; col < lhs.cols(); ++col) {
+      EXPECT_NEAR(lhs(row, col), rhs(row, col), tolerance)
+          << fmt::format("row = {}, col = {}", row, col);
+    }
+  }
+
+  if (::testing::Test::HasFailure()) {
+    fmt::print("lhs =\n{}\n", lhs);
+    fmt::print("rhs =\n{}\n", rhs);
+    fmt::print("delta =\n{}\n", Eigen::MatrixXd{lhs - rhs});
+  }
+}
+
+void ExpectPositiveSemidefinite(const Eigen::Ref<const Eigen::MatrixXd>& X) {
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX{X,
+                                                      Eigen::EigenvaluesOnly};
+  for (int i = 0; i < X.rows(); ++i) {
+    EXPECT_GE(eigX.eigenvalues()[i], 0.0);
+  }
+}
+
+void ExpectDARESolution(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>& X) {
+  // Check that X is the solution to the DARE
+  // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+  // clang-format off
+  Eigen::MatrixXd Y =
+      A.transpose() * X * A
+      - X
+      - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
+        * B.transpose() * X * A)
+      + Q;
+  // clang-format on
+  ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
+}
+
+void ExpectDARESolution(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,
+                        const Eigen::Ref<const Eigen::MatrixXd>& X) {
+  // Check that X is the solution to the DARE
+  // Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+  // clang-format off
+  Eigen::MatrixXd Y =
+      A.transpose() * X * A
+      - X
+      - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
+        * (B.transpose() * X * A + N.transpose()))
+      + Q;
+  // clang-format on
+  ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
+}