From 52bd5b972d13d40408c7b63fe2d9bb6a28b67366 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 14 May 2023 22:23:00 -0700 Subject: [PATCH] [wpimath] Rewrite DARE solver (#5328) I timed the DARE unit tests, and the new solver is 0 to 100% faster in all cases (that is, it's at least as fast as Drake's and up to 2x faster in some cases). The new solver is also much simpler, takes less time to compile, and drops the libwpimath.so size from 325 MB to 301 MB. I think most of the compilation time is coming from the eigenvalue decompositions used to enforce argument preconditions. --- .github/workflows/upstream-utils.yml | 4 - ...-Replace-Eigen-Dense-with-Eigen-Core.patch | 76 --- ...EXPORT-to-DARE-function-declarations.patch | 37 -- upstream_utils/update_drake.py | 104 ---- .../wpi/first/math/{Drake.java => DARE.java} | 65 +-- .../java/edu/wpi/first/math/WPIMathJNI.java | 2 +- .../controller/LinearQuadraticRegulator.java | 6 +- .../math/estimator/ExtendedKalmanFilter.java | 5 +- .../first/math/estimator/KalmanFilter.java | 6 +- wpimath/src/main/native/cpp/DARE.cpp | 265 ++++++++++ .../src/main/native/cpp/jni/WPIMathJNI.cpp | 35 +- wpimath/src/main/native/include/frc/DARE.h | 91 ++++ .../controller/LinearQuadraticRegulator.inc | 9 +- .../frc/estimator/ExtendedKalmanFilter.inc | 8 +- .../include/frc/estimator/KalmanFilter.inc | 6 +- .../drake/include/drake/common/drake_assert.h | 162 ------ .../drake/common/drake_assertion_error.h | 18 - .../include/drake/common/drake_copyable.h | 90 ---- .../drake/include/drake/common/drake_throw.h | 47 -- .../drake/common/is_approx_equal_abstol.h | 62 --- .../include/drake/common/never_destroyed.h | 103 ---- .../discrete_algebraic_riccati_equation.h | 85 ---- .../src/common/drake_assert_and_throw.cpp | 87 ---- .../discrete_algebraic_riccati_equation.cpp | 475 ------------------ .../java/edu/wpi/first/math/DARETest.java | 185 +++++++ .../java/edu/wpi/first/math/DrakeTest.java | 73 --- wpimath/src/test/native/cpp/DARETest.cpp | 220 ++++++++ .../native/cpp/drake/common/fmt_eigen.cpp | 41 -- ...screte_algebraic_riccati_equation_test.cpp | 124 ----- .../test/native/include/drake/common/fmt.h | 164 ------ .../native/include/drake/common/fmt_eigen.h | 87 ---- .../test_utilities/eigen_matrix_compare.h | 113 ----- 32 files changed, 831 insertions(+), 2024 deletions(-) delete mode 100644 upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch delete mode 100644 upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch delete mode 100755 upstream_utils/update_drake.py rename wpimath/src/main/java/edu/wpi/first/math/{Drake.java => DARE.java} (61%) create mode 100644 wpimath/src/main/native/cpp/DARE.cpp create mode 100644 wpimath/src/main/native/include/frc/DARE.h delete mode 100644 wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h delete mode 100644 wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h delete mode 100644 wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h delete mode 100644 wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h delete mode 100644 wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h delete mode 100644 wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h delete mode 100644 wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h delete mode 100644 wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp delete mode 100644 wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp create mode 100644 wpimath/src/test/java/edu/wpi/first/math/DARETest.java delete mode 100644 wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java create mode 100644 wpimath/src/test/native/cpp/DARETest.cpp delete mode 100644 wpimath/src/test/native/cpp/drake/common/fmt_eigen.cpp delete mode 100644 wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp delete mode 100644 wpimath/src/test/native/include/drake/common/fmt.h delete mode 100644 wpimath/src/test/native/include/drake/common/fmt_eigen.h delete mode 100644 wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h diff --git a/.github/workflows/upstream-utils.yml b/.github/workflows/upstream-utils.yml index ea6e9aa7fa..063df4e949 100644 --- a/.github/workflows/upstream-utils.yml +++ b/.github/workflows/upstream-utils.yml @@ -29,10 +29,6 @@ jobs: run: | git config --global user.email "you@example.com" git config --global user.name "Your Name" - - name: Run update_drake.py - run: | - cd upstream_utils - ./update_drake.py - name: Run update_eigen.py run: | cd upstream_utils diff --git a/upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch b/upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch deleted file mode 100644 index 2992678dd1..0000000000 --- a/upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch +++ /dev/null @@ -1,76 +0,0 @@ -From 32dd1aa796fbb45d02f8bd445cc962e6a91318e7 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Wed, 18 May 2022 11:13:21 -0700 -Subject: [PATCH 1/2] Replace with - ---- - common/is_approx_equal_abstol.h | 2 +- - common/test_utilities/eigen_matrix_compare.h | 2 +- - math/discrete_algebraic_riccati_equation.cc | 3 +++ - math/discrete_algebraic_riccati_equation.h | 2 +- - math/test/discrete_algebraic_riccati_equation_test.cc | 1 + - 5 files changed, 7 insertions(+), 3 deletions(-) - -diff --git a/common/is_approx_equal_abstol.h b/common/is_approx_equal_abstol.h -index 9af0c45252..b3f369ca01 100644 ---- a/common/is_approx_equal_abstol.h -+++ b/common/is_approx_equal_abstol.h -@@ -2,7 +2,7 @@ - - #include - --#include -+#include - - namespace drake { - -diff --git a/common/test_utilities/eigen_matrix_compare.h b/common/test_utilities/eigen_matrix_compare.h -index 01821ff847..105f6b381c 100644 ---- a/common/test_utilities/eigen_matrix_compare.h -+++ b/common/test_utilities/eigen_matrix_compare.h -@@ -5,7 +5,7 @@ - #include - #include - --#include -+#include - #include - - #include "drake/common/fmt_eigen.h" -diff --git a/math/discrete_algebraic_riccati_equation.cc b/math/discrete_algebraic_riccati_equation.cc -index 901f2ef240..20ea2b7bbe 100644 ---- a/math/discrete_algebraic_riccati_equation.cc -+++ b/math/discrete_algebraic_riccati_equation.cc -@@ -1,5 +1,8 @@ - #include "drake/math/discrete_algebraic_riccati_equation.h" - -+#include -+#include -+ - #include "drake/common/drake_assert.h" - #include "drake/common/drake_throw.h" - #include "drake/common/is_approx_equal_abstol.h" -diff --git a/math/discrete_algebraic_riccati_equation.h b/math/discrete_algebraic_riccati_equation.h -index 891373ff9d..df7a58b2b8 100644 ---- a/math/discrete_algebraic_riccati_equation.h -+++ b/math/discrete_algebraic_riccati_equation.h -@@ -3,7 +3,7 @@ - #include - #include - --#include -+#include - - namespace drake { - namespace math { -diff --git a/math/test/discrete_algebraic_riccati_equation_test.cc b/math/test/discrete_algebraic_riccati_equation_test.cc -index 533ced151d..e4ecfd2eb5 100644 ---- a/math/test/discrete_algebraic_riccati_equation_test.cc -+++ b/math/test/discrete_algebraic_riccati_equation_test.cc -@@ -1,5 +1,6 @@ - #include "drake/math/discrete_algebraic_riccati_equation.h" - -+#include - #include - - #include "drake/common/test_utilities/eigen_matrix_compare.h" diff --git a/upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch b/upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch deleted file mode 100644 index 0690caa865..0000000000 --- a/upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch +++ /dev/null @@ -1,37 +0,0 @@ -From 48ed223a299304724a38ad9911f7a732bf574b71 Mon Sep 17 00:00:00 2001 -From: Tyler Veness -Date: Wed, 18 May 2022 11:15:27 -0700 -Subject: [PATCH 2/2] Add WPILIB_DLLEXPORT to DARE function declarations - ---- - math/discrete_algebraic_riccati_equation.h | 3 +++ - 1 file changed, 3 insertions(+) - -diff --git a/math/discrete_algebraic_riccati_equation.h b/math/discrete_algebraic_riccati_equation.h -index df7a58b2b8..55b8442bf4 100644 ---- a/math/discrete_algebraic_riccati_equation.h -+++ b/math/discrete_algebraic_riccati_equation.h -@@ -4,6 +4,7 @@ - #include - - #include -+#include - - namespace drake { - namespace math { -@@ -21,6 +22,7 @@ Based on the Schur Vector approach outlined in this paper: - "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" - by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell - */ -+WPILIB_DLLEXPORT - Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, -@@ -71,6 +73,7 @@ J = Σ [uₖ] [0 R][uₖ] ΔT - @throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite. - @throws std::runtime_error if R is not positive definite. - */ -+WPILIB_DLLEXPORT - Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, diff --git a/upstream_utils/update_drake.py b/upstream_utils/update_drake.py deleted file mode 100755 index 3398d904a2..0000000000 --- a/upstream_utils/update_drake.py +++ /dev/null @@ -1,104 +0,0 @@ -#!/usr/bin/env python3 - -import os -import shutil - -from upstream_utils import ( - get_repo_root, - clone_repo, - comment_out_invalid_includes, - walk_cwd_and_copy_if, - git_am, -) - - -def main(): - upstream_root = clone_repo("https://github.com/RobotLocomotion/drake", "v1.15.0") - wpilib_root = get_repo_root() - wpimath = os.path.join(wpilib_root, "wpimath") - - # Apply patches to upstream Git repo - os.chdir(upstream_root) - for f in [ - "0001-Replace-Eigen-Dense-with-Eigen-Core.patch", - "0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch", - ]: - git_am(os.path.join(wpilib_root, "upstream_utils/drake_patches", f)) - - # Delete old install - for d in [ - "src/main/native/thirdparty/drake/src", - "src/main/native/thirdparty/drake/include", - "src/test/native/cpp/drake", - "src/test/native/include/drake", - ]: - shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True) - - # Copy drake source files into allwpilib - src_files = walk_cwd_and_copy_if( - lambda dp, f: f - in ["drake_assert_and_throw.cc", "discrete_algebraic_riccati_equation.cc"], - os.path.join(wpimath, "src/main/native/thirdparty/drake/src"), - ) - - # Copy drake header files into allwpilib - include_files = walk_cwd_and_copy_if( - lambda dp, f: f - in [ - "drake_assert.h", - "drake_assertion_error.h", - "is_approx_equal_abstol.h", - "never_destroyed.h", - "drake_copyable.h", - "drake_throw.h", - "discrete_algebraic_riccati_equation.h", - ], - os.path.join(wpimath, "src/main/native/thirdparty/drake/include/drake"), - ) - - # Copy drake test source files into allwpilib - os.chdir(os.path.join(upstream_root, "math/test")) - test_src_files = walk_cwd_and_copy_if( - lambda dp, f: f == "discrete_algebraic_riccati_equation_test.cc", - os.path.join(wpimath, "src/test/native/cpp/drake"), - ) - os.chdir(upstream_root) - test_src_files += walk_cwd_and_copy_if( - lambda dp, f: f == "fmt_eigen.cc", - os.path.join(wpimath, "src/test/native/cpp/drake"), - ) - - # Copy drake test header files into allwpilib - test_include_files = walk_cwd_and_copy_if( - lambda dp, f: f in ["eigen_matrix_compare.h", "fmt.h", "fmt_eigen.h"], - os.path.join(wpimath, "src/test/native/include/drake"), - ) - - for f in src_files: - comment_out_invalid_includes( - f, [os.path.join(wpimath, "src/main/native/thirdparty/drake/include")] - ) - for f in include_files: - comment_out_invalid_includes( - f, [os.path.join(wpimath, "src/main/native/thirdparty/drake/include")] - ) - for f in test_src_files: - comment_out_invalid_includes( - f, - [ - os.path.join(wpimath, "src/main/native/thirdparty/drake/include"), - os.path.join(wpimath, "src/test/native/include"), - ], - ) - for f in test_include_files: - comment_out_invalid_includes( - f, - [ - os.path.join(wpimath, "src/main/native/thirdparty/drake/include"), - os.path.join(wpimath, "src/test/native/include"), - ], - ) - - -if __name__ == "__main__": - main() diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/DARE.java similarity index 61% rename from wpimath/src/main/java/edu/wpi/first/math/Drake.java rename to wpimath/src/main/java/edu/wpi/first/math/DARE.java index 55bc5b21cb..c9757b778a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java +++ b/wpimath/src/main/java/edu/wpi/first/math/DARE.java @@ -6,8 +6,8 @@ package edu.wpi.first.math; import org.ejml.simple.SimpleMatrix; -public final class Drake { - private Drake() {} +public final class DARE { + private DARE() {} /** * Solves the discrete algebraic Riccati equation. @@ -18,10 +18,9 @@ public final class Drake { * @param R Input cost matrix. * @return Solution of DARE. */ - public static SimpleMatrix discreteAlgebraicRiccatiEquation( - SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) { + public static SimpleMatrix dare(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) { var S = new SimpleMatrix(A.numRows(), A.numCols()); - WPIMathJNI.discreteAlgebraicRiccatiEquation( + WPIMathJNI.dare( A.getDDRM().getData(), B.getDDRM().getData(), Q.getDDRM().getData(), @@ -43,15 +42,12 @@ public final class Drake { * @param R Input cost matrix. * @return Solution of DARE. */ - public static - Matrix discreteAlgebraicRiccatiEquation( - Matrix A, - Matrix B, - Matrix Q, - Matrix R) { - return new Matrix<>( - discreteAlgebraicRiccatiEquation( - A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage())); + public static Matrix dare( + Matrix A, + Matrix B, + Matrix Q, + Matrix R) { + return new Matrix<>(dare(A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage())); } /** @@ -64,7 +60,7 @@ public final class Drake { * @param N State-input cross-term cost matrix. * @return Solution of DARE. */ - public static SimpleMatrix discreteAlgebraicRiccatiEquation( + public static SimpleMatrix dare( 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 @@ -73,7 +69,7 @@ public final class Drake { var scrQ = Q.minus(N.mult(R.solve(N.transpose()))); var S = new SimpleMatrix(A.numRows(), A.numCols()); - WPIMathJNI.discreteAlgebraicRiccatiEquation( + WPIMathJNI.dare( scrA.getDDRM().getData(), B.getDDRM().getData(), scrQ.getDDRM().getData(), @@ -96,21 +92,28 @@ public final class Drake { * @param N State-input cross-term cost matrix. * @return Solution of DARE. */ - 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()))); - + public static Matrix dare( + Matrix A, + Matrix B, + Matrix Q, + Matrix R, + Matrix N) { + // This is a change of variables to make the DARE that includes Q, R, and N + // cost matrices fit the form of the DARE that includes only Q and R cost + // matrices. + // + // This is equivalent to solving the original DARE: + // + // A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 + // + // where A₂ and Q₂ are a change of variables: + // + // A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ return new Matrix<>( - discreteAlgebraicRiccatiEquation( - scrA.getStorage(), B.getStorage(), scrQ.getStorage(), R.getStorage())); + dare( + A.minus(B.times(R.solve(N.transpose()))).getStorage(), + B.getStorage(), + Q.minus(N.times(R.solve(N.transpose()))).getStorage(), + R.getStorage())); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 40a5c634d6..d86d141561 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -54,7 +54,7 @@ public final class WPIMathJNI { * @param inputs Number of inputs in B matrix. * @param S Array storage for DARE solution. */ - public static native void discreteAlgebraicRiccatiEquation( + public static native void dare( double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S); /** 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 dce1748587..69ef860b32 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 @@ -4,7 +4,7 @@ package edu.wpi.first.math.controller; -import edu.wpi.first.math.Drake; +import edu.wpi.first.math.DARE; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Num; @@ -111,7 +111,7 @@ public class LinearQuadraticRegulator(states, states); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java index 24d6d91479..94bb159fd3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java @@ -4,7 +4,7 @@ package edu.wpi.first.math.estimator; -import edu.wpi.first.math.Drake; +import edu.wpi.first.math.DARE; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -87,9 +87,7 @@ public class KalmanFilter( - Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR)); + var P = new Matrix<>(DARE.dare(discA.transpose(), C.transpose(), discQ, discR)); // S = CPCᵀ + R var S = C.times(P).times(C.transpose()).plus(discR); diff --git a/wpimath/src/main/native/cpp/DARE.cpp b/wpimath/src/main/native/cpp/DARE.cpp new file mode 100644 index 0000000000..ffe6172837 --- /dev/null +++ b/wpimath/src/main/native/cpp/DARE.cpp @@ -0,0 +1,265 @@ +// 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 "frc/DARE.h" + +#include +#include +#include + +#include "Eigen/Cholesky" +#include "Eigen/Core" +#include "Eigen/Eigenvalues" +#include "Eigen/QR" +#include "frc/fmt/Eigen.h" + +// Works cited: +// +// [1] E. K.-W. Chu, H.-Y. Fan, W.-W. Lin & C.-S. Wang "Structure-Preserving +// Algorithms for Periodic Discrete-Time Algebraic Riccati Equations", +// International Journal of Control, 77:8, 767-788, 2004. +// DOI: 10.1080/00207170410001714988 + +namespace frc { + +namespace { + +/** + * Returns true if (A, B) is a stabilizable pair. + * + * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if + * any, have absolute values less than one, where an eigenvalue is + * uncontrollable if rank([λI - A, B]) < n where n is the number of states. + * + * @param A System matrix. + * @param B Input matrix. + */ +bool IsStabilizable(const Eigen::Ref& A, + const Eigen::Ref& B) { + Eigen::EigenSolver es{A}; + + for (int i = 0; i < A.rows(); ++i) { + if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() + + es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() < + 1) { + continue; + } + + Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()}; + E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(A.rows(), A.rows()) - + A, + B; + + Eigen::ColPivHouseholderQR qr{E}; + if (qr.rank() < A.rows()) { + return false; + } + } + return true; +} + +/** + * Returns true if (A, C) is a detectable pair. + * + * (A, C) is detectable if and only if the unobservable eigenvalues of A, if + * any, have absolute values less than one, where an eigenvalue is unobservable + * if rank([λI - A; C]) < n where n is the number of states. + * + * @param A System matrix. + * @param C Output matrix. + */ +bool IsDetectable(const Eigen::Ref& A, + const Eigen::Ref& C) { + return IsStabilizable(A.transpose(), C.transpose()); +} + +/** + * Returns true if all the matrix's eigenvalues are greater than or equal to + * zero. + * + * @param A The matrix. + */ +bool IsPositiveSemidefinite(const Eigen::Ref& A) { + Eigen::SelfAdjointEigenSolver es{A}; + for (int i = 0; i < A.rows(); ++i) { + if (es.eigenvalues()[i] < 0) { + return false; + } + } + + return true; +} + +/** + * Returns true if all the matrix's eigenvalues are greater than zero. + * + * @param A The matrix. + */ +bool IsPositiveDefinite(const Eigen::Ref& A) { + Eigen::SelfAdjointEigenSolver es{A}; + for (int i = 0; i < A.rows(); ++i) { + if (es.eigenvalues()[i] <= 0) { + return false; + } + } + + return true; +} + +} // namespace + +Eigen::MatrixXd DARE(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R) { + // These are unused if assertions aren't compiled in + [[maybe_unused]] int states = A.rows(); + [[maybe_unused]] int inputs = B.cols(); + + // Check argument dimensions + assert(A.rows() == states && A.cols() == states); + assert(B.rows() == states && B.cols() == inputs); + assert(Q.rows() == states && Q.cols() == states); + assert(R.rows() == inputs && R.cols() == inputs); + + // Require the number of inputs be less than or equal to the number of states + if (inputs > states) { + std::string msg = fmt::format( + "Number of inputs ({}) is greater than number of states ({})!", inputs, + states); + throw std::invalid_argument(msg); + } + + // Require Q be symmetric + if ((Q - Q.transpose()).norm() > 1e-10) { + std::string msg = + fmt::format("Q isn't symmetric!\n\nQ =\n{}\n", Eigen::MatrixXd{Q}); + throw std::invalid_argument(msg); + } + + // Require Q be positive semidefinite + if (!IsPositiveSemidefinite(Q)) { + std::string msg = fmt::format("Q isn't positive semidefinite!\n\nQ =\n{}\n", + Eigen::MatrixXd{Q}); + throw std::invalid_argument(msg); + } + + // Require R be symmetric + if ((R - R.transpose()).norm() > 1e-10) { + std::string msg = + fmt::format("R isn't symmetric!\n\nR =\n{}\n", Eigen::MatrixXd{R}); + throw std::invalid_argument(msg); + } + + // Require R be positive definite + if (!IsPositiveDefinite(R)) { + std::string msg = fmt::format("R isn't positive definite!\n\nR =\n{}\n", + Eigen::MatrixXd{R}); + throw std::invalid_argument(msg); + } + + // Require (A, B) pair be stabilizable + if (!IsStabilizable(A, B)) { + std::string msg = + fmt::format("The (A, B) pair isn't stabilizable!\n\nA =\n{}\nB =\n{}\n", + Eigen::MatrixXd{A}, Eigen::MatrixXd{B}); + throw std::invalid_argument(msg); + } + + // Require (A, C) pair be detectable where Q = CᵀC + { + Eigen::LDLT ldlt{Q}; + Eigen::MatrixXd C = Eigen::MatrixXd{ldlt.matrixL()} * + ldlt.vectorD().cwiseSqrt().asDiagonal(); + + if (!IsDetectable(A, C)) { + std::string msg = fmt::format( + "The (A, C) pair where Q = CᵀC isn't detectable!\n\nA =\n{}\nQ " + "=\n{}\n", + Eigen::MatrixXd{A}, Eigen::MatrixXd{Q}); + throw std::invalid_argument(msg); + } + } + + // Implements the SSCA algorithm on page 12 of [1]. + + // A₀ = A + Eigen::MatrixXd A_k = A; + Eigen::MatrixXd A_k1 = A; + + // G₀ = BR⁻¹Bᵀ + // + // See equation (4) of [1]. + Eigen::MatrixXd G_k = B * R.llt().solve(B.transpose()); + + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(A.rows(), A.cols()); + + // H₀ = Q + // + // See equation (4) of [1]. + Eigen::MatrixXd H_k = Q; + Eigen::MatrixXd H_k1 = Q; + + do { + A_k = A_k1; + H_k = H_k1; + + // W = I + HₖGₖ + Eigen::MatrixXd W = I + H_k * G_k; + + // W is symmetric positive definite, so the LLT decomposition would work + // here and is faster than the householder QR decomposition [2]. However, + // it's not accurate enough. Experimentation showed that so many iterations + // of iterative refinement [3] were required to fix the accuracy that the + // total system solve time was much higher than householder QR. + // + // [2] https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html + // [3] https://en.wikipedia.org/wiki/Iterative_refinement + auto W_solver = W.householderQr(); + + // Solve WV₁ = Aₖᵀ for V₁ + Eigen::MatrixXd V_1 = W_solver.solve(A_k.transpose()); + + // Solve WV₂ = Hₖ for V₂ + Eigen::MatrixXd V_2 = W_solver.solve(H_k); + + // Aₖ₊₁ = V₁ᵀAₖ + A_k1 = V_1.transpose() * A_k; + + // Gₖ₊₁ = Gₖ + AₖGₖV₁ + G_k += A_k * G_k * V_1; + + // Hₖ₊₁ = Hₖ + AₖᵀV₂Aₖ + H_k1 = H_k + A_k.transpose() * V_2 * A_k; + + // while |Hₖ₊₁ − Hₖ| > ε |Hₖ₊₁| + } while ((H_k1 - H_k).norm() > 1e-10 * H_k1.norm()); + + return H_k1; +} + +Eigen::MatrixXd DARE(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& N) { + // Check argument dimensions + assert(N.rows() == B.rows() && N.cols() == B.cols()); + + // This is a change of variables to make the DARE that includes Q, R, and N + // cost matrices fit the form of the DARE that includes only Q and R cost + // matrices. + // + // This is equivalent to solving the original DARE: + // + // A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 + // + // where A₂ and Q₂ are a change of variables: + // + // A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ + return DARE(A - B * R.llt().solve(N.transpose()), B, + Q - N * R.llt().solve(N.transpose()), R); +} + +} // namespace frc diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index fd8a223646..584c443d15 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -12,47 +12,51 @@ #include "Eigen/Core" #include "Eigen/Eigenvalues" #include "Eigen/QR" -#include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" +#include "frc/DARE.h" #include "frc/trajectory/TrajectoryUtil.h" #include "unsupported/Eigen/MatrixFunctions" using namespace wpi::java; +namespace { + /** * Returns true if (A, B) is a stabilizable pair. * * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if * any, have absolute values less than one, where an eigenvalue is - * uncontrollable if rank(λI - A, B) < n where n is the number of states. + * uncontrollable if rank([λI - A, B]) < n where n is the number of states. * * @param A System matrix. * @param B Input matrix. */ -bool check_stabilizable(const Eigen::Ref& A, - const Eigen::Ref& B) { - int states = B.rows(); - int inputs = B.cols(); +bool IsStabilizable(const Eigen::Ref& A, + const Eigen::Ref& B) { Eigen::EigenSolver es{A}; - for (int i = 0; i < states; ++i) { + + for (int i = 0; i < A.rows(); ++i) { if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() + es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() < 1) { continue; } - Eigen::MatrixXcd E{states, states + inputs}; - E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A, + Eigen::MatrixXcd E{A.rows(), A.rows() + B.cols()}; + E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(A.rows(), A.rows()) - + A, B; + Eigen::ColPivHouseholderQR qr{E}; - if (qr.rank() < states) { + if (qr.rank() < A.rows()) { return false; } } - return true; } +} // namespace + std::vector GetElementsFromTrajectory( const frc::Trajectory& trajectory) { std::vector elements; @@ -97,11 +101,11 @@ extern "C" { /* * Class: edu_wpi_first_math_WPIMathJNI - * Method: discreteAlgebraicRiccatiEquation + * Method: dare * Signature: ([D[D[D[DII[D)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation +Java_edu_wpi_first_math_WPIMathJNI_dare (JNIEnv* env, jclass, jdoubleArray A, jdoubleArray B, jdoubleArray Q, jdoubleArray R, jint states, jint inputs, jdoubleArray S) { @@ -124,8 +128,7 @@ Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation Rmat{nativeR, inputs, inputs}; try { - Eigen::MatrixXd result = - drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat); + Eigen::MatrixXd result = frc::DARE(Amat, Bmat, Qmat, Rmat); env->ReleaseDoubleArrayElements(A, nativeA, 0); env->ReleaseDoubleArrayElements(B, nativeB, 0); @@ -205,7 +208,7 @@ Java_edu_wpi_first_math_WPIMathJNI_isStabilizable Eigen::Matrix> B{nativeB, states, inputs}; - bool isStabilizable = check_stabilizable(A, B); + bool isStabilizable = IsStabilizable(A, B); env->ReleaseDoubleArrayElements(aSrc, nativeA, 0); env->ReleaseDoubleArrayElements(bSrc, nativeB, 0); diff --git a/wpimath/src/main/native/include/frc/DARE.h b/wpimath/src/main/native/include/frc/DARE.h new file mode 100644 index 0000000000..b6c3de5b06 --- /dev/null +++ b/wpimath/src/main/native/include/frc/DARE.h @@ -0,0 +1,91 @@ +// 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. + +#pragma once + +#include + +#include "Eigen/Core" + +namespace frc { + +/** + * + * Computes the unique stabilizing solution X to the discrete-time algebraic + * Riccati equation: + * + * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 + * + * @param A The system matrix. + * @param B The input matrix. + * @param Q The state cost matrix. + * @param R The input cost matrix. + * @throws std::invalid_argument if number of inputs is greater than number of + * states. + * @throws std::invalid_argument if Q isn't symmetric positive semidefinite. + * @throws std::invalid_argument if R isn't symmetric positive definite. + * @throws std::invalid_argument if the (A, B) pair isn't stabilizable. + * @throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't + * detectable. + */ +WPILIB_DLLEXPORT +Eigen::MatrixXd DARE(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R); + +/** +Computes the unique stabilizing solution X to the discrete-time algebraic +Riccati equation: + + AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 + +This overload of the DARE is useful for finding the control law uₖ that +minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ. + +@verbatim + ∞ [xₖ]ᵀ[Q N][xₖ] +J = Σ [uₖ] [Nᵀ R][uₖ] ΔT + k=0 +@endverbatim + +This is a more general form of the following. The linear-quadratic regulator +is the feedback control law uₖ that minimizes the following cost function +subject to xₖ₊₁ = Axₖ + Buₖ: + +@verbatim + ∞ +J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT + k=0 +@endverbatim + +This can be refactored as: + +@verbatim + ∞ [xₖ]ᵀ[Q 0][xₖ] +J = Σ [uₖ] [0 R][uₖ] ΔT + k=0 +@endverbatim + +@param A The system matrix. +@param B The input matrix. +@param Q The state cost matrix. +@param R The input cost matrix. +@param N The state-input cross cost matrix. +@throws std::invalid_argument if number of inputs is greater than number of + states. +@throws std::invalid_argument if Q − NR⁻¹Nᵀ isn't symmetric positive + semidefinite. +@throws std::invalid_argument if R isn't symmetric positive definite. +@throws std::invalid_argument if the (A, B) pair isn't stabilizable. +@throws std::invalid_argument if the (A, C) pair where Q = CᵀC isn't detectable. +*/ +WPILIB_DLLEXPORT +Eigen::MatrixXd DARE(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& N); + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc index 87d37ec425..caa20f2ec5 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc @@ -8,8 +8,7 @@ #include #include "Eigen/Cholesky" -#include "Eigen/Eigenvalues" -#include "drake/math/discrete_algebraic_riccati_equation.h" +#include "frc/DARE.h" #include "frc/StateSpaceUtil.h" #include "frc/controller/LinearQuadraticRegulator.h" #include "frc/fmt/Eigen.h" @@ -52,8 +51,7 @@ LinearQuadraticRegulator::LinearQuadraticRegulator( throw std::invalid_argument(msg); } - Matrixd S = - drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R); + Matrixd S = DARE(discA, discB, Q, R); // K = (BᵀSB + R)⁻¹BᵀSA m_K = (discB.transpose() * S * discB + R) @@ -72,8 +70,7 @@ LinearQuadraticRegulator::LinearQuadraticRegulator( Matrixd discB; DiscretizeAB(A, B, dt, &discA, &discB); - Matrixd S = - drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N); + Matrixd S = DARE(discA, discB, Q, R, N); // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ) m_K = (discB.transpose() * S * discB + R) diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc index a56b8b5c66..a0a0e100e0 100644 --- a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc @@ -5,7 +5,7 @@ #pragma once #include "Eigen/Cholesky" -#include "drake/math/discrete_algebraic_riccati_equation.h" +#include "frc/DARE.h" #include "frc/StateSpaceUtil.h" #include "frc/estimator/ExtendedKalmanFilter.h" #include "frc/system/Discretization.h" @@ -41,8 +41,7 @@ ExtendedKalmanFilter::ExtendedKalmanFilter( Matrixd discR = DiscretizeR(m_contR, dt); if (IsDetectable(discA, C) && Outputs <= States) { - m_initP = drake::math::DiscreteAlgebraicRiccatiEquation( - discA.transpose(), C.transpose(), discQ, discR); + m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR); } else { m_initP = StateMatrix::Zero(); } @@ -77,8 +76,7 @@ ExtendedKalmanFilter::ExtendedKalmanFilter( Matrixd discR = DiscretizeR(m_contR, dt); if (IsDetectable(discA, C) && Outputs <= States) { - m_initP = drake::math::DiscreteAlgebraicRiccatiEquation( - discA.transpose(), C.transpose(), discQ, discR); + m_initP = DARE(discA.transpose(), C.transpose(), discQ, discR); } else { m_initP = StateMatrix::Zero(); } diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc index ca4f37c942..ba266934ed 100644 --- a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc @@ -11,7 +11,7 @@ #include #include "Eigen/Cholesky" -#include "drake/math/discrete_algebraic_riccati_equation.h" +#include "frc/DARE.h" #include "frc/StateSpaceUtil.h" #include "frc/estimator/KalmanFilter.h" #include "frc/system/Discretization.h" @@ -47,8 +47,8 @@ KalmanFilter::KalmanFilter( throw std::invalid_argument(msg); } - Matrixd P = drake::math::DiscreteAlgebraicRiccatiEquation( - discA.transpose(), C.transpose(), discQ, discR); + Matrixd P = + DARE(discA.transpose(), C.transpose(), discQ, discR); // S = CPCᵀ + R Matrixd S = C * P * C.transpose() + discR; diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h deleted file mode 100644 index 47097ede73..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h +++ /dev/null @@ -1,162 +0,0 @@ -#pragma once - -#include - -/// @file -/// Provides Drake's assertion implementation. This is intended to be used -/// both within Drake and by other software. Drake's asserts can be armed -/// and disarmed independently from the system-wide asserts. - -#ifdef DRAKE_DOXYGEN_CXX -/// @p DRAKE_ASSERT(condition) is similar to the built-in @p assert(condition) -/// from the C++ system header @p . Unless Drake's assertions are -/// disarmed by the pre-processor definitions listed below, @p DRAKE_ASSERT -/// will evaluate @p condition and iff the value is false will trigger an -/// assertion failure with a message showing at least the condition text, -/// function name, file, and line. -/// -/// By default, assertion failures will :abort() the program. However, when -/// using the pydrake python bindings, assertion failures will instead throw a -/// C++ exception that causes a python SystemExit exception. -/// -/// Assertions are enabled or disabled using the following pre-processor macros: -/// -/// - If @p DRAKE_ENABLE_ASSERTS is defined, then @p DRAKE_ASSERT is armed. -/// - If @p DRAKE_DISABLE_ASSERTS is defined, then @p DRAKE_ASSERT is disarmed. -/// - If both macros are defined, then it is a compile-time error. -/// - If neither are defined, then NDEBUG governs assertions as usual. -/// -/// This header will define exactly one of either @p DRAKE_ASSERT_IS_ARMED or -/// @p DRAKE_ASSERT_IS_DISARMED to indicate whether @p DRAKE_ASSERT is armed. -/// -/// This header will define both `constexpr bool drake::kDrakeAssertIsArmed` -/// and `constexpr bool drake::kDrakeAssertIsDisarmed` globals. -/// -/// One difference versus the standard @p assert(condition) is that the -/// @p condition within @p DRAKE_ASSERT is always syntax-checked, even if -/// Drake's assertions are disarmed. -/// -/// Treat @p DRAKE_ASSERT like a statement -- it must always be used -/// in block scope, and must always be followed by a semicolon. -#define DRAKE_ASSERT(condition) -/// Like @p DRAKE_ASSERT, except that the expression must be void-valued; this -/// allows for guarding expensive assertion-checking subroutines using the same -/// macros as stand-alone assertions. -#define DRAKE_ASSERT_VOID(expression) -/// Evaluates @p condition and iff the value is false will trigger an assertion -/// failure with a message showing at least the condition text, function name, -/// file, and line. -#define DRAKE_DEMAND(condition) -/// Silences a "no return value" compiler warning by calling a function that -/// always raises an exception or aborts (i.e., a function marked noreturn). -/// Only use this macro at a point where (1) a point in the code is truly -/// unreachable, (2) the fact that it's unreachable is knowable from only -/// reading the function itself (and not, e.g., some larger design invariant), -/// and (3) there is a compiler warning if this macro were removed. The most -/// common valid use is with a switch-case-return block where all cases are -/// accounted for but the enclosing function is supposed to return a value. Do -/// *not* use this macro as a "logic error" assertion; it should *only* be used -/// to silence false positive warnings. When in doubt, throw an exception -/// manually instead of using this macro. -#define DRAKE_UNREACHABLE() -#else // DRAKE_DOXYGEN_CXX - -// Users should NOT set these; only this header should set them. -#ifdef DRAKE_ASSERT_IS_ARMED -# error Unexpected DRAKE_ASSERT_IS_ARMED defined. -#endif -#ifdef DRAKE_ASSERT_IS_DISARMED -# error Unexpected DRAKE_ASSERT_IS_DISARMED defined. -#endif - -// Decide whether Drake assertions are enabled. -#if defined(DRAKE_ENABLE_ASSERTS) && defined(DRAKE_DISABLE_ASSERTS) -# error Conflicting assertion toggles. -#elif defined(DRAKE_ENABLE_ASSERTS) -# define DRAKE_ASSERT_IS_ARMED -#elif defined(DRAKE_DISABLE_ASSERTS) || defined(NDEBUG) -# define DRAKE_ASSERT_IS_DISARMED -#else -# define DRAKE_ASSERT_IS_ARMED -#endif - -namespace drake { -namespace internal { -// Abort the program with an error message. -[[noreturn]] void Abort(const char* condition, const char* func, - const char* file, int line); -// Report an assertion failure; will either Abort(...) or throw. -[[noreturn]] void AssertionFailed(const char* condition, const char* func, - const char* file, int line); -} // namespace internal -namespace assert { -// Allows for specialization of how to bool-convert Conditions used in -// assertions, in case they are not intrinsically convertible. See -// common/symbolic/expression/formula.h for an example use. This is a public -// interface to extend; it is intended to be specialized by unusual Scalar -// types that require special handling. -template -struct ConditionTraits { - static constexpr bool is_valid = std::is_convertible_v; - static bool Evaluate(const Condition& value) { - return value; - } -}; -} // namespace assert -} // namespace drake - -#define DRAKE_UNREACHABLE() \ - ::drake::internal::Abort( \ - "Unreachable code was reached?!", __func__, __FILE__, __LINE__) - -#define DRAKE_DEMAND(condition) \ - do { \ - typedef ::drake::assert::ConditionTraits< \ - typename std::remove_cv_t> Trait; \ - static_assert(Trait::is_valid, "Condition should be bool-convertible."); \ - static_assert( \ - !std::is_pointer_v, \ - "When using DRAKE_DEMAND on a raw pointer, always write out " \ - "DRAKE_DEMAND(foo != nullptr), do not write DRAKE_DEMAND(foo) " \ - "and rely on implicit pointer-to-bool conversion."); \ - if (!Trait::Evaluate(condition)) { \ - ::drake::internal::AssertionFailed( \ - #condition, __func__, __FILE__, __LINE__); \ - } \ - } while (0) - -#ifdef DRAKE_ASSERT_IS_ARMED -// Assertions are enabled. -namespace drake { -constexpr bool kDrakeAssertIsArmed = true; -constexpr bool kDrakeAssertIsDisarmed = false; -} // namespace drake -# define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition) -# define DRAKE_ASSERT_VOID(expression) do { \ - static_assert( \ - std::is_convertible_v, \ - "Expression should be void."); \ - expression; \ - } while (0) -#else -// Assertions are disabled, so just typecheck the expression. -namespace drake { -constexpr bool kDrakeAssertIsArmed = false; -constexpr bool kDrakeAssertIsDisarmed = true; -} // namespace drake -# define DRAKE_ASSERT(condition) do { \ - typedef ::drake::assert::ConditionTraits< \ - typename std::remove_cv_t> Trait; \ - static_assert(Trait::is_valid, "Condition should be bool-convertible."); \ - static_assert( \ - !std::is_pointer_v, \ - "When using DRAKE_ASSERT on a raw pointer, always write out " \ - "DRAKE_ASSERT(foo != nullptr), do not write DRAKE_ASSERT(foo) " \ - "and rely on implicit pointer-to-bool conversion."); \ - } while (0) -# define DRAKE_ASSERT_VOID(expression) static_assert( \ - std::is_convertible_v, \ - "Expression should be void.") -#endif - -#endif // DRAKE_DOXYGEN_CXX diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h deleted file mode 100644 index b42847453e..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - -#include -#include - -namespace drake { -namespace internal { - -// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are -// configured to throw. -class assertion_error : public std::runtime_error { - public: - explicit assertion_error(const std::string& what_arg) - : std::runtime_error(what_arg) {} -}; - -} // namespace internal -} // namespace drake diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h deleted file mode 100644 index a96a6fb7cf..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -// ============================================================================ -// N.B. The spelling of the macro names between doc/Doxyfile_CXX.in and this -// file must be kept in sync! -// ============================================================================ - -/** @file -Provides careful macros to selectively enable or disable the special member -functions for copy-construction, copy-assignment, move-construction, and -move-assignment. - -http://en.cppreference.com/w/cpp/language/member_functions#Special_member_functions - -When enabled via these macros, the `= default` implementation is provided. -Code that needs custom copy or move functions should not use these macros. -*/ - -/** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for -copy-construction, copy-assignment, move-construction, and move-assignment. -Drake's Doxygen is customized to render the deletions in detail, with -appropriate comments. Invoke this macro in the public section of the class -declaration, e.g.: -
-class Foo {
- public:
-  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
-
-  // ...
-};
-
-*/ -#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname) \ - Classname(const Classname&) = delete; \ - void operator=(const Classname&) = delete; \ - Classname(Classname&&) = delete; \ - void operator=(Classname&&) = delete; - -/** DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN defaults the special member -functions for copy-construction, copy-assignment, move-construction, and -move-assignment. This macro should be used only when copy-construction and -copy-assignment defaults are well-formed. Note that the defaulted move -functions could conceivably still be ill-formed, in which case they will -effectively not be declared or used -- but because the copy constructor exists -the type will still be MoveConstructible. Drake's Doxygen is customized to -render the functions in detail, with appropriate comments. Typically, you -should invoke this macro in the public section of the class declaration, e.g.: -
-class Foo {
- public:
-  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
-
-  // ...
-};
-
- -However, if Foo has a virtual destructor (i.e., is subclassable), then -typically you should use either DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN in the -public section or else DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN in the -protected section, to prevent -object slicing. - -The macro contains a built-in self-check that copy-assignment is well-formed. -This self-check proves that the Classname is CopyConstructible, CopyAssignable, -MoveConstructible, and MoveAssignable (in all but the most arcane cases where a -member of the Classname is somehow CopyAssignable but not CopyConstructible). -Therefore, classes that use this macro typically will not need to have any unit -tests that check for the presence nor correctness of these functions. - -However, the self-check does not provide any checks of the runtime efficiency -of the functions. If it is important for performance that the move functions -actually move (instead of making a copy), then you should consider capturing -that in a unit test. -*/ -#define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname) \ - Classname(const Classname&) = default; \ - Classname& operator=(const Classname&) = default; \ - Classname(Classname&&) = default; \ - Classname& operator=(Classname&&) = default; \ - /* Fails at compile-time if copy-assign doesn't compile. */ \ - /* Note that we do not test the copy-ctor here, because */ \ - /* it will not exist when Classname is abstract. */ \ - static void DrakeDefaultCopyAndMoveAndAssign_DoAssign( \ - Classname* a, const Classname& b) { *a = b; } \ - static_assert( \ - &DrakeDefaultCopyAndMoveAndAssign_DoAssign == \ - &DrakeDefaultCopyAndMoveAndAssign_DoAssign, \ - "This assertion is never false; its only purpose is to " \ - "generate 'use of deleted function: operator=' errors " \ - "when Classname is a template."); diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h deleted file mode 100644 index fdd07a2622..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include - -#include "drake/common/drake_assert.h" - -/// @file -/// Provides a convenient wrapper to throw an exception when a condition is -/// unmet. This is similar to an assertion, but uses exceptions instead of -/// ::abort(), and cannot be disabled. - -namespace drake { -namespace internal { -// Throw an error message. -[[noreturn]] void Throw(const char* condition, const char* func, - const char* file, int line); -} // namespace internal -} // namespace drake - -/// Evaluates @p condition and iff the value is false will throw an exception -/// with a message showing at least the condition text, function name, file, -/// and line. -/// -/// The condition must not be a pointer, where we'd implicitly rely on its -/// nullness. Instead, always write out "!= nullptr" to be precise. -/// -/// Correct: `DRAKE_THROW_UNLESS(foo != nullptr);` -/// Incorrect: `DRAKE_THROW_UNLESS(foo);` -/// -/// Because this macro is intended to provide a useful exception message to -/// users, we should err on the side of extra detail about the failure. The -/// meaning of "foo" isolated within error message text does not make it -/// clear that a null pointer is the proximate cause of the problem. -#define DRAKE_THROW_UNLESS(condition) \ - do { \ - typedef ::drake::assert::ConditionTraits< \ - typename std::remove_cv_t> Trait; \ - static_assert(Trait::is_valid, "Condition should be bool-convertible."); \ - static_assert( \ - !std::is_pointer_v, \ - "When using DRAKE_THROW_UNLESS on a raw pointer, always write out " \ - "DRAKE_THROW_UNLESS(foo != nullptr), do not write DRAKE_THROW_UNLESS" \ - "(foo) and rely on implicit pointer-to-bool conversion."); \ - if (!Trait::Evaluate(condition)) { \ - ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__); \ - } \ - } while (0) diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h deleted file mode 100644 index b3f369ca01..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include - -#include - -namespace drake { - -/// Returns true if and only if the two matrices are equal to within a certain -/// absolute elementwise @p tolerance. Special values (infinities, NaN, etc.) -/// do not compare as equal elements. -template -bool is_approx_equal_abstol(const Eigen::MatrixBase& m1, - const Eigen::MatrixBase& m2, - double tolerance) { - return ( - (m1.rows() == m2.rows()) && - (m1.cols() == m2.cols()) && - ((m1 - m2).template lpNorm() <= tolerance)); -} - -/// Returns true if and only if a simple greedy search reveals a permutation -/// of the columns of m2 to make the matrix equal to m1 to within a certain -/// absolute elementwise @p tolerance. E.g., there exists a P such that -/// 
-///    forall i,j,  |m1 - m2*P|_{i,j} <= tolerance
-///    where P is a permutation matrix:
-///       P(i,j)={0,1}, sum_i P(i,j)=1, sum_j P(i,j)=1.
-///
-/// Note: Returns false for matrices of different sizes. -/// Note: The current implementation is O(n^2) in the number of columns. -/// Note: In marginal cases (with similar but not identical columns) this -/// algorithm can fail to find a permutation P even if it exists because it -/// accepts the first column match (m1(i),m2(j)) and removes m2(j) from the -/// pool. It is possible that other columns of m2 would also match m1(i) but -/// that m2(j) is the only match possible for a later column of m1. -template -bool IsApproxEqualAbsTolWithPermutedColumns( - const Eigen::MatrixBase& m1, - const Eigen::MatrixBase& m2, double tolerance) { - if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false; - - std::vector available(m2.cols()); - for (int i = 0; i < m2.cols(); i++) available[i] = true; - - for (int i = 0; i < m1.cols(); i++) { - bool found_match = false; - for (int j = 0; j < m2.cols(); j++) { - if (available[j] && - is_approx_equal_abstol(m1.col(i), m2.col(j), tolerance)) { - found_match = true; - available[j] = false; - break; - } - } - if (!found_match) return false; - } - return true; -} - - -} // namespace drake diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h b/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h deleted file mode 100644 index 024b3552e2..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h +++ /dev/null @@ -1,103 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "drake/common/drake_copyable.h" - -namespace drake { - -/// Wraps an underlying type T such that its storage is a direct member field -/// of this object (i.e., without any indirection into the heap), but *unlike* -/// most member fields T's destructor is never invoked. -/// -/// This is especially useful for function-local static variables that are not -/// trivially destructable. We shouldn't call their destructor at program exit -/// because of the "indeterminate order of ... destruction" as mentioned in -/// cppguide's -/// Static -/// and Global Variables section, but other solutions to this problem place -/// the objects on the heap through an indirection. -/// -/// Compared with other approaches, this mechanism more clearly describes the -/// intent to readers, avoids "possible leak" warnings from memory-checking -/// tools, and is probably slightly faster. -/// -/// Example uses: -/// -/// The singleton pattern: -/// @code -/// class Singleton { -/// public: -/// DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Singleton) -/// static Singleton& getInstance() { -/// static never_destroyed instance; -/// return instance.access(); -/// } -/// private: -/// friend never_destroyed; -/// Singleton() = default; -/// }; -/// @endcode -/// -/// A lookup table, created on demand the first time its needed, and then -/// reused thereafter: -/// @code -/// enum class Foo { kBar, kBaz }; -/// Foo ParseFoo(const std::string& foo_string) { -/// using Dict = std::unordered_map; -/// static const drake::never_destroyed string_to_enum{ -/// std::initializer_list{ -/// {"bar", Foo::kBar}, -/// {"baz", Foo::kBaz}, -/// } -/// }; -/// return string_to_enum.access().at(foo_string); -/// } -/// @endcode -/// -/// In cases where computing the static data is more complicated than an -/// initializer_list, you can use a temporary lambda to populate the value: -/// @code -/// const std::vector& GetConstantMagicNumbers() { -/// static const drake::never_destroyed> result{[]() { -/// std::vector prototype; -/// std::mt19937 random_generator; -/// for (int i = 0; i < 10; ++i) { -/// double new_value = random_generator(); -/// prototype.push_back(new_value); -/// } -/// return prototype; -/// }()}; -/// return result.access(); -/// } -/// @endcode -/// -/// Note in particular the `()` after the lambda. That causes it to be invoked. -// -// The above examples are repeated in the unit test; keep them in sync. -template -class never_destroyed { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed) - - /// Passes the constructor arguments along to T using perfect forwarding. - template - explicit never_destroyed(Args&&... args) { - // Uses "placement new" to construct a `T` in `storage_`. - new (&storage_) T(std::forward(args)...); - } - - /// Does nothing. Guaranteed! - ~never_destroyed() = default; - - /// Returns the underlying T reference. - T& access() { return *reinterpret_cast(&storage_); } - const T& access() const { return *reinterpret_cast(&storage_); } - - private: - typename std::aligned_storage::type storage_; -}; - -} // namespace drake diff --git a/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h deleted file mode 100644 index 55b8442bf4..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h +++ /dev/null @@ -1,85 +0,0 @@ -#pragma once - -#include -#include - -#include -#include - -namespace drake { -namespace math { - -/** -Computes the unique stabilizing solution X to the discrete-time algebraic -Riccati equation: - -AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 - -@throws std::exception if Q is not positive semi-definite. -@throws std::exception if R is not positive definite. - -Based on the Schur Vector approach outlined in this paper: -"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" -by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell -*/ -WPILIB_DLLEXPORT -Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R); - -/** -Computes the unique stabilizing solution X to the discrete-time algebraic -Riccati equation: - -AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0 - -This is equivalent to solving the original DARE: - -A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0 - -where A₂ and Q₂ are a change of variables: - -A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ - -This overload of the DARE is useful for finding the control law uₖ that -minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ. - -@verbatim - ∞ [xₖ]ᵀ[Q N][xₖ] -J = Σ [uₖ] [Nᵀ R][uₖ] ΔT - k=0 -@endverbatim - -This is a more general form of the following. The linear-quadratic regulator -is the feedback control law uₖ that minimizes the following cost function -subject to xₖ₊₁ = Axₖ + Buₖ: - -@verbatim - ∞ -J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT - k=0 -@endverbatim - -This can be refactored as: - -@verbatim - ∞ [xₖ]ᵀ[Q 0][xₖ] -J = Σ [uₖ] [0 R][uₖ] ΔT - k=0 -@endverbatim - -@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite. -@throws std::runtime_error if R is not positive definite. -*/ -WPILIB_DLLEXPORT -Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R, - const Eigen::Ref& N); -} // namespace math -} // namespace drake - diff --git a/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp b/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp deleted file mode 100644 index 88e7e66289..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// This file contains the implementation of both drake_assert and drake_throw. -/* clang-format off to disable clang-format-includes */ -#include "drake/common/drake_assert.h" -#include "drake/common/drake_throw.h" -/* clang-format on */ - -#include -#include -#include -#include -#include -#include - -#include "drake/common/drake_assertion_error.h" -#include "drake/common/never_destroyed.h" - -namespace drake { -namespace internal { -namespace { - -// Singleton to manage assertion configuration. -struct AssertionConfig { - static AssertionConfig& singleton() { - static never_destroyed global; - return global.access(); - } - - std::atomic assertion_failures_are_exceptions; -}; - -// Stream into @p out the given failure details; only @p condition may be null. -void PrintFailureDetailTo(std::ostream& out, const char* condition, - const char* func, const char* file, int line) { - out << "Failure at " << file << ":" << line << " in " << func << "()"; - if (condition) { - out << ": condition '" << condition << "' failed."; - } else { - out << "."; - } -} -} // namespace - -// Declared in drake_assert.h. -void Abort(const char* condition, const char* func, const char* file, - int line) { - std::cerr << "abort: "; - PrintFailureDetailTo(std::cerr, condition, func, file, line); - std::cerr << std::endl; - std::abort(); -} - -// Declared in drake_throw.h. -void Throw(const char* condition, const char* func, const char* file, - int line) { - std::ostringstream what; - PrintFailureDetailTo(what, condition, func, file, line); - throw assertion_error(what.str().c_str()); -} - -// Declared in drake_assert.h. -void AssertionFailed(const char* condition, const char* func, const char* file, - int line) { - if (AssertionConfig::singleton().assertion_failures_are_exceptions) { - Throw(condition, func, file, line); - } else { - Abort(condition, func, file, line); - } -} - -} // namespace internal -} // namespace drake - -// Configures the DRAKE_ASSERT and DRAKE_DEMAND assertion failure handling -// behavior. -// -// By default, assertion failures will result in an ::abort(). If this method -// has ever been called, failures will result in a thrown exception instead. -// -// Assertion configuration has process-wide scope. Changes here will affect -// all assertions within the current process. -// -// This method is intended ONLY for use by pydrake bindings, and thus is not -// declared in any header file, to discourage anyone from using it. -extern "C" void drake_set_assertion_failure_to_throw_exception() { - drake::internal::AssertionConfig::singleton(). - assertion_failures_are_exceptions = true; -} diff --git a/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp deleted file mode 100644 index 20ea2b7bbe..0000000000 --- a/wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp +++ /dev/null @@ -1,475 +0,0 @@ -#include "drake/math/discrete_algebraic_riccati_equation.h" - -#include -#include - -#include "drake/common/drake_assert.h" -#include "drake/common/drake_throw.h" -#include "drake/common/is_approx_equal_abstol.h" - -namespace drake { -namespace math { -namespace { -/* helper functions */ -template -int sgn(T val) { - return (T(0) < val) - (val < T(0)); -} -void check_stabilizable(const Eigen::Ref& A, - const Eigen::Ref& B) { - // This function checks if (A,B) is a stabilizable pair. - // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of - // A, if any, have absolute values less than one, where an eigenvalue is - // uncontrollable if Rank[lambda * I - A, B] < n. - int n = B.rows(), m = B.cols(); - Eigen::EigenSolver es(A); - for (int i = 0; i < n; i++) { - if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() + - es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() < - 1) - continue; - Eigen::MatrixXcd E(n, n + m); - E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B; - Eigen::ColPivHouseholderQR qr(E); - DRAKE_THROW_UNLESS(qr.rank() == n); - } -} -void check_detectable(const Eigen::Ref& A, - const Eigen::Ref& Q) { - // This function check if (A,C) is a detectable pair, where Q = C' * C. - // (A,C) is detectable if and only if the unobservable eigenvalues of A, - // if any, have absolute values less than one, where an eigenvalue is - // unobservable if Rank[lambda * I - A; C] < n. - // Also, (A,C) is detectable if and only if (A',C') is stabilizable. - int n = A.rows(); - Eigen::LDLT ldlt(Q); - Eigen::MatrixXd L = ldlt.matrixL(); - Eigen::MatrixXd D = ldlt.vectorD(); - Eigen::MatrixXd D_sqrt = Eigen::MatrixXd::Zero(n, n); - for (int i = 0; i < n; i++) { - D_sqrt(i, i) = sqrt(D(i)); - } - Eigen::MatrixXd C = L * D_sqrt; - check_stabilizable(A.transpose(), C.transpose()); -} - -// "Givens rotation" computes an orthogonal 2x2 matrix R such that -// it eliminates the 2nd coordinate of the vector [a,b]', i.e., -// R * [ a ] = [ a_hat ] -// [ b ] [ 0 ] -// The implementation is based on -// https://en.wikipedia.org/wiki/Givens_rotation#Stable_calculation -void Givens_rotation(double a, double b, Eigen::Ref R, - double eps = 1e-10) { - double c, s; - if (fabs(b) < eps) { - c = (a < -eps ? -1 : 1); - s = 0; - } else if (fabs(a) < eps) { - c = 0; - s = -sgn(b); - } else if (fabs(a) > fabs(b)) { - double t = b / a; - double u = sgn(a) * fabs(sqrt(1 + t * t)); - c = 1 / u; - s = -c * t; - } else { - double t = a / b; - double u = sgn(b) * fabs(sqrt(1 + t * t)); - s = -1 / u; - c = -s * t; - } - R(0, 0) = c, R(0, 1) = -s, R(1, 0) = s, R(1, 1) = c; -} - -// The arguments S, T, and Z will be changed. -void swap_block_11(Eigen::Ref S, Eigen::Ref T, - Eigen::Ref Z, int p) { - // Dooren, Case I, p124-125. - int n2 = S.rows(); - Eigen::Matrix2d A = S.block<2, 2>(p, p); - Eigen::Matrix2d B = T.block<2, 2>(p, p); - Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::Matrix2d H = A(1, 1) * B - B(1, 1) * A; - Givens_rotation(H(0, 1), H(0, 0), Z1.block<2, 2>(p, p)); - S = (S * Z1).eval(); - T = (T * Z1).eval(); - Z = (Z * Z1).eval(); - Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(n2, n2); - Givens_rotation(T(p, p), T(p + 1, p), Q.block<2, 2>(p, p)); - S = (Q * S).eval(); - T = (Q * T).eval(); - S(p + 1, p) = 0; - T(p + 1, p) = 0; -} - -// The arguments S, T, and Z will be changed. -void swap_block_21(Eigen::Ref S, Eigen::Ref T, - Eigen::Ref Z, int p) { - // Dooren, Case II, p126-127. - int n2 = S.rows(); - Eigen::Matrix3d A = S.block<3, 3>(p, p); - Eigen::Matrix3d B = T.block<3, 3>(p, p); - // Compute H and eliminate H(1,0) by row operation. - Eigen::Matrix3d H = A(2, 2) * B - B(2, 2) * A; - Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3); - Givens_rotation(H(0, 0), H(1, 0), R.block<2, 2>(0, 0)); - H = (R * H).eval(); - Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2); - // Compute Z1, Z2, Q1, Q2. - Givens_rotation(H(1, 2), H(1, 1), Z1.block<2, 2>(p + 1, p + 1)); - H = (H * Z1.block<3, 3>(p, p)).eval(); - Givens_rotation(H(0, 1), H(0, 0), Z2.block<2, 2>(p, p)); - S = (S * Z1).eval(); - T = (T * Z1).eval(); - Z = (Z * Z1 * Z2).eval(); - Givens_rotation(T(p + 1, p + 1), T(p + 2, p + 1), - Q1.block<2, 2>(p + 1, p + 1)); - S = (Q1 * S * Z2).eval(); - T = (Q1 * T * Z2).eval(); - Givens_rotation(T(p, p), T(p + 1, p), Q2.block<2, 2>(p, p)); - S = (Q2 * S).eval(); - T = (Q2 * T).eval(); - S(p + 1, p) = 0; - S(p + 2, p) = 0; - T(p + 1, p) = 0; - T(p + 2, p) = 0; - T(p + 2, p + 1) = 0; -} - -// The arguments S, T, and Z will be changed. -void swap_block_12(Eigen::Ref S, Eigen::Ref T, - Eigen::Ref Z, int p) { - int n2 = S.rows(); - // Swap the role of S and T. - Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Q0 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2); - Givens_rotation(S(p + 1, p + 1), S(p + 2, p + 1), - Q0.block<2, 2>(p + 1, p + 1)); - S = (Q0 * S).eval(); - T = (Q0 * T).eval(); - Eigen::Matrix3d A = S.block<3, 3>(p, p); - Eigen::Matrix3d B = T.block<3, 3>(p, p); - // Compute H and eliminate H(2,1) by column operation. - Eigen::Matrix3d H = B(0, 0) * A - A(0, 0) * B; - Eigen::Matrix3d R = Eigen::MatrixXd::Identity(3, 3); - Givens_rotation(H(2, 2), H(2, 1), R.block<2, 2>(1, 1)); - H = (H * R).eval(); - // Compute Q1, Q2, Z1, Z2. - Givens_rotation(H(0, 1), H(1, 1), Q1.block<2, 2>(p, p)); - H = (Q1.block<3, 3>(p, p) * H).eval(); - Givens_rotation(H(1, 2), H(2, 2), Q2.block<2, 2>(p + 1, p + 1)); - S = (Q1 * S).eval(); - T = (Q1 * T).eval(); - Givens_rotation(S(p + 1, p + 1), S(p + 1, p), Z1.block<2, 2>(p, p)); - S = (Q2 * S * Z1).eval(); - T = (Q2 * T * Z1).eval(); - Givens_rotation(S(p + 2, p + 2), S(p + 2, p + 1), - Z2.block<2, 2>(p + 1, p + 1)); - S = (S * Z2).eval(); - T = (T * Z2).eval(); - Z = (Z * Z1 * Z2).eval(); - // Swap back the role of S and T. - Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p)); - S = (Q3 * S).eval(); - T = (Q3 * T).eval(); - S(p + 2, p) = 0; - S(p + 2, p + 1) = 0; - T(p + 1, p) = 0; - T(p + 2, p) = 0; - T(p + 2, p + 1) = 0; -} - -// The arguments S, T, and Z will be changed. -void swap_block_22(Eigen::Ref S, Eigen::Ref T, - Eigen::Ref Z, int p) { - // Direct Swapping Algorithm based on - // "Numerical Methods for General and Structured Eigenvalue Problems" by - // Daniel Kressner, p108-111. - // ( http://sma.epfl.ch/~anchpcommon/publications/kressner_eigenvalues.pdf ). - // Also relevant but not applicable here: - // "On Swapping Diagonal Blocks in Real Schur Form" by Zhaojun Bai and James - // W. Demmelt; - int n2 = S.rows(); - Eigen::MatrixXd A = S.block<4, 4>(p, p); - Eigen::MatrixXd B = T.block<4, 4>(p, p); - // Solve - // A11 * X - Y A22 = A12 - // B11 * X - Y B22 = B12 - // Reduce to solve Cx=D, where x=[x1;...;x4;y1;...;y4]. - Eigen::Matrix C = Eigen::Matrix::Zero(); - Eigen::Matrix D; - // clang-format off - C << A(0, 0), 0, A(0, 1), 0, -A(2, 2), -A(3, 2), 0, 0, - 0, A(0, 0), 0, A(0, 1), -A(2, 3), -A(3, 3), 0, 0, - A(1, 0), 0, A(1, 1), 0, 0, 0, -A(2, 2), -A(3, 2), - 0, A(1, 0), 0, A(1, 1), 0, 0, -A(2, 3), -A(3, 3), - B(0, 0), 0, B(0, 1), 0, -B(2, 2), -B(3, 2), 0, 0, - 0, B(0, 0), 0, B(0, 1), -B(2, 3), -B(3, 3), 0, 0, - B(1, 0), 0, B(1, 1), 0, 0, 0, -B(2, 2), -B(3, 2), - 0, B(1, 0), 0, B(1, 1), 0, 0, -B(2, 3), -B(3, 3); - // clang-format on - D << A(0, 2), A(0, 3), A(1, 2), A(1, 3), B(0, 2), B(0, 3), B(1, 2), B(1, 3); - Eigen::MatrixXd x = C.colPivHouseholderQr().solve(D); - // Q * [ -Y ] = [ R_Y ] , Z' * [ -X ] = [ R_X ] . - // [ I ] [ 0 ] [ I ] = [ 0 ] - Eigen::Matrix X, Y; - X << -x(0, 0), -x(1, 0), -x(2, 0), -x(3, 0), Eigen::Matrix2d::Identity(); - Y << -x(4, 0), -x(5, 0), -x(6, 0), -x(7, 0), Eigen::Matrix2d::Identity(); - Eigen::MatrixXd Q1 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::MatrixXd Z1 = Eigen::MatrixXd::Identity(n2, n2); - Eigen::ColPivHouseholderQR > qr1(X); - Z1.block<4, 4>(p, p) = qr1.householderQ(); - Eigen::ColPivHouseholderQR > qr2(Y); - Q1.block<4, 4>(p, p) = qr2.householderQ().adjoint(); - // Apply transform Q1 * (S,T) * Z1. - S = (Q1 * S * Z1).eval(); - T = (Q1 * T * Z1).eval(); - Z = (Z * Z1).eval(); - // Eliminate the T(p+3,p+2) entry. - Eigen::MatrixXd Q2 = Eigen::MatrixXd::Identity(n2, n2); - Givens_rotation(T(p + 2, p + 2), T(p + 3, p + 2), - Q2.block<2, 2>(p + 2, p + 2)); - S = (Q2 * S).eval(); - T = (Q2 * T).eval(); - // Eliminate the T(p+1,p) entry. - Eigen::MatrixXd Q3 = Eigen::MatrixXd::Identity(n2, n2); - Givens_rotation(T(p, p), T(p + 1, p), Q3.block<2, 2>(p, p)); - S = (Q3 * S).eval(); - T = (Q3 * T).eval(); - S(p + 2, p) = 0; - S(p + 2, p + 1) = 0; - S(p + 3, p) = 0; - S(p + 3, p + 1) = 0; - T(p + 1, p) = 0; - T(p + 2, p) = 0; - T(p + 2, p + 1) = 0; - T(p + 3, p) = 0; - T(p + 3, p + 1) = 0; - T(p + 3, p + 2) = 0; -} - -// Functionality of "swap_block" function: -// swap the 1x1 or 2x2 blocks pointed by p and q. -// There are four cases: swapping 1x1 and 1x1 matrices, swapping 2x2 and 1x1 -// matrices, swapping 1x1 and 2x2 matrices, and swapping 2x2 and 2x2 matrices. -// Algorithms are described in the papers -// "A generalized eigenvalue approach for solving Riccati equations" by P. Van -// Dooren, 1981 ( http://epubs.siam.org/doi/pdf/10.1137/0902010 ), and -// "Numerical Methods for General and Structured Eigenvalue Problems" by -// Daniel Kressner, 2005. -void swap_block(Eigen::Ref S, Eigen::Ref T, - Eigen::Ref Z, int p, int q, int q_block_size, - double eps = 1e-10) { - int p_tmp = q, p_block_size; - while (p_tmp-- > p) { - p_block_size = 1; - if (p_tmp >= 1 && fabs(S(p_tmp, p_tmp - 1)) > eps) { - p_block_size = 2; - p_tmp--; - } - switch (p_block_size * 10 + q_block_size) { - case 11: - swap_block_11(S, T, Z, p_tmp); - break; - case 21: - swap_block_21(S, T, Z, p_tmp); - break; - case 12: - swap_block_12(S, T, Z, p_tmp); - break; - case 22: - swap_block_22(S, T, Z, p_tmp); - break; - } - } -} - -// Functionality of "reorder_eigen" function: -// Reorder the eigenvalues of (S,T) such that the top-left n by n matrix has -// stable eigenvalues by multiplying Q's and Z's on the left and the right, -// respectively. -// Stable eigenvalues are inside the unit disk. -// -// Algorithm: -// Go along the diagonals of (S,T) from the top left to the bottom right. -// Once find a stable eigenvalue, push it to top left. -// In implementation, use two pointers, p and q. -// p points to the current block (1x1 or 2x2) and q points to the block with the -// stable eigenvalue(s). -// Push the block pointed by q to the position pointed by p. -// Finish when n stable eigenvalues are placed at the top-left n by n matrix. -// The algorithm for swapping blocks is described in the papers -// "A generalized eigenvalue approach for solving Riccati equations" by P. Van -// Dooren, 1981, and "Numerical Methods for General and Structured Eigenvalue -// Problems" by Daniel Kressner, 2005. -void reorder_eigen(Eigen::Ref S, Eigen::Ref T, - Eigen::Ref Z, double eps = 1e-10) { - // abs(a) < eps => a = 0 - int n2 = S.rows(); - int n = n2 / 2, p = 0, q = 0; - - // Find the first unstable p block. - while (p < n) { - if (fabs(S(p + 1, p)) < eps) { // p block size = 1 - if (fabs(T(p, p)) > eps && fabs(S(p, p)) <= fabs(T(p, p))) { // stable - p++; - continue; - } - } else { // p block size = 2 - const double det_T = - T(p, p) * T(p + 1, p + 1) - T(p + 1, p) * T(p, p + 1); - if (fabs(det_T) > eps) { - const double det_S = - S(p, p) * S(p + 1, p + 1) - S(p + 1, p) * S(p, p + 1); - if (fabs(det_S) <= fabs(det_T)) { // stable - p += 2; - continue; - } - } - } - break; - } - q = p; - - // Make the first n generalized eigenvalues stable. - while (p < n && q < n2) { - // Update q. - int q_block_size = 0; - while (q < n2) { - if (q == n2 - 1 || fabs(S(q + 1, q)) < eps) { // block size = 1 - if (fabs(T(q, q)) > eps && fabs(S(q, q)) <= fabs(T(q, q))) { - q_block_size = 1; - break; - } - q++; - } else { // block size = 2 - const double det_T = - T(q, q) * T(q + 1, q + 1) - T(q + 1, q) * T(q, q + 1); - if (fabs(det_T) > eps) { - const double det_S = - S(q, q) * S(q + 1, q + 1) - S(q + 1, q) * S(q, q + 1); - if (fabs(det_S) <= fabs(det_T)) { - q_block_size = 2; - break; - } - } - q += 2; - } - } - if (q >= n2) - throw std::runtime_error("fail to find enough stable eigenvalues"); - // Swap blocks pointed by p and q. - if (p != q) { - swap_block(S, T, Z, p, q, q_block_size); - p += q_block_size; - q += q_block_size; - } - } - if (p < n && q >= n2) - throw std::runtime_error("fail to find enough stable eigenvalues"); -} -} // namespace - -/** - * DiscreteAlgebraicRiccatiEquation function - * computes the unique stabilizing solution X to the discrete-time algebraic - * Riccati equation: - * - * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 - * - * @throws std::exception if Q is not positive semi-definite. - * @throws std::exception if R is not positive definite. - * - * Based on the Schur Vector approach outlined in this paper: - * "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation" - * by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell, in TAC, 1980, - * http://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1102434 - * - * Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half, - * R_half are sampled from standard normal distributions, where - * Q = Q_halfᵀ Q_half and similar for R, the absolute error of the solution - * is 10⁻⁶, while the absolute error of the solution computed by Matlab is - * 10⁻⁸. - * - * TODO(weiqiao.han): I may overwrite the RealQZ function to improve the - * accuracy, together with more thorough tests. - */ - -Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation( - const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R) { - int n = B.rows(), m = B.cols(); - - DRAKE_DEMAND(m <= n); - DRAKE_DEMAND(A.rows() == n && A.cols() == n); - DRAKE_DEMAND(Q.rows() == n && Q.cols() == n); - DRAKE_DEMAND(R.rows() == m && R.cols() == m); - DRAKE_DEMAND(is_approx_equal_abstol(Q, Q.transpose(), 1e-10)); - Eigen::SelfAdjointEigenSolver es(Q); - for (int i = 0; i < n; i++) { - DRAKE_THROW_UNLESS(es.eigenvalues()[i] >= 0); - } - DRAKE_DEMAND(is_approx_equal_abstol(R, R.transpose(), 1e-10)); - Eigen::LLT R_cholesky(R); - DRAKE_THROW_UNLESS(R_cholesky.info() == Eigen::Success); - check_stabilizable(A, B); - check_detectable(A, Q); - - Eigen::MatrixXd M(2 * n, 2 * n), L(2 * n, 2 * n); - M << A, Eigen::MatrixXd::Zero(n, n), -Q, Eigen::MatrixXd::Identity(n, n); - L << Eigen::MatrixXd::Identity(n, n), B * R.inverse() * B.transpose(), - Eigen::MatrixXd::Zero(n, n), A.transpose(); - - // QZ decomposition of M and L - // QMZ = S, QLZ = T - // where Q and Z are real orthogonal matrixes - // T is upper-triangular matrix, and S is upper quasi-triangular matrix - Eigen::RealQZ qz(2 * n); - qz.compute(M, L); // M = Q S Z, L = Q T Z (Q and Z computed by Eigen package - // are adjoints of Q and Z above) - Eigen::MatrixXd S = qz.matrixS(), T = qz.matrixT(), - Z = qz.matrixZ().adjoint(); - - // Reorder the generalized eigenvalues of (S,T). - Eigen::MatrixXd Z2 = Eigen::MatrixXd::Identity(2 * n, 2 * n); - reorder_eigen(S, T, Z2); - Z = (Z * Z2).eval(); - - // The first n columns of Z is ( U1 ) . - // ( U2 ) - // -1 - // X = U2 * U1 is a solution of the discrete time Riccati equation. - Eigen::MatrixXd U1 = Z.block(0, 0, n, n), U2 = Z.block(n, 0, n, n); - Eigen::MatrixXd X = U2 * U1.inverse(); - X = (X + X.adjoint().eval()) / 2.0; - 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()); - - // This is a change of variables to make the DARE that includes Q, R, and N - // cost matrices fit the form of the DARE that includes only Q and R cost - // matrices. - Eigen::MatrixXd A2 = A - B * R.llt().solve(N.transpose()); - Eigen::MatrixXd Q2 = Q - N * R.llt().solve(N.transpose()); - return DiscreteAlgebraicRiccatiEquation(A2, B, Q2, R); -} - -} // namespace math -} // namespace drake diff --git a/wpimath/src/test/java/edu/wpi/first/math/DARETest.java b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java new file mode 100644 index 0000000000..082ecf14e1 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/DARETest.java @@ -0,0 +1,185 @@ +// 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. + +package edu.wpi.first.math; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.ejml.simple.SimpleMatrix; +import org.junit.jupiter.api.Test; + +class DARETest { + public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) { + for (int i = 0; i < A.numRows(); i++) { + for (int j = 0; j < A.numCols(); j++) { + assertEquals(A.get(i, j), B.get(i, j), 1e-4); + } + } + } + + void assertDARESolution( + SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix X) { + // Check that X is the solution to the DARE + // Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0 + var Y = + (A.transpose().mult(X).mult(A)) + .minus(X) + .minus( + (A.transpose().mult(X).mult(B)) + .mult((B.transpose().mult(X).mult(B).plus(R)).invert()) + .mult(B.transpose().mult(X).mult(A))) + .plus(Q); + assertMatrixEqual(new SimpleMatrix(Y.numRows(), Y.numCols()), Y); + } + + void assertDARESolution( + SimpleMatrix A, + SimpleMatrix B, + SimpleMatrix Q, + SimpleMatrix R, + SimpleMatrix N, + SimpleMatrix 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 + var Y = + (A.transpose().mult(X).mult(A)) + .minus(X) + .minus( + (A.transpose().mult(X).mult(B).plus(N)) + .mult((B.transpose().mult(X).mult(B).plus(R)).invert()) + .mult(B.transpose().mult(X).mult(A).plus(N.transpose()))) + .plus(Q); + assertMatrixEqual(new SimpleMatrix(Y.numRows(), Y.numCols()), Y); + } + + @Test + void testNonInvertibleA_ABQR() { + // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic + // Riccati Equation" + + var A = + new SimpleMatrix( + 4, 4, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0}); + var B = new SimpleMatrix(4, 1, true, new double[] {0, 0, 0, 1}); + var Q = + new SimpleMatrix(4, 4, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}); + var R = new SimpleMatrix(1, 1, true, new double[] {0.25}); + + var X = DARE.dare(A, B, Q, R); + assertMatrixEqual(X, X.transpose()); + assertDARESolution(A, B, Q, R, X); + } + + @Test + void testNonInvertibleA_ABQRN() { + // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic + // Riccati Equation" + + var A = + new SimpleMatrix( + 4, 4, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0}); + var B = new SimpleMatrix(4, 1, true, new double[] {0, 0, 0, 1}); + var Q = + new SimpleMatrix(4, 4, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}); + var R = new SimpleMatrix(1, 1, true, new double[] {0.25}); + + var Aref = + new SimpleMatrix( + 4, 4, true, new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0}); + Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref)); + R = B.transpose().mult(Q).mult(B).plus(R); + var N = A.minus(Aref).transpose().mult(Q).mult(B); + + var X = DARE.dare(A, B, Q, R, N); + assertMatrixEqual(X, X.transpose()); + assertDARESolution(A, B, Q, R, N, X); + } + + @Test + void testInvertibleA_ABQR() { + var A = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1}); + var B = new SimpleMatrix(2, 1, true, new double[] {0, 1}); + var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0}); + var R = new SimpleMatrix(1, 1, true, new double[] {0.3}); + + var X = DARE.dare(A, B, Q, R); + assertMatrixEqual(X, X.transpose()); + assertDARESolution(A, B, Q, R, X); + } + + @Test + void testInvertibleA_ABQRN() { + var A = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1}); + var B = new SimpleMatrix(2, 1, true, new double[] {0, 1}); + var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0}); + var R = new SimpleMatrix(1, 1, true, new double[] {0.3}); + + var Aref = new SimpleMatrix(2, 2, true, new double[] {0.5, 1, 0, 1}); + Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref)); + R = B.transpose().mult(Q).mult(B).plus(R); + var N = A.minus(Aref).transpose().mult(Q).mult(B); + + var X = DARE.dare(A, B, Q, R, N); + assertMatrixEqual(X, X.transpose()); + assertDARESolution(A, B, Q, R, N, X); + } + + @Test + void testFirstGeneralizedEigenvalueOfSTIsStable_ABQR() { + // The first generalized eigenvalue of (S, T) is stable + + var A = new SimpleMatrix(2, 2, true, new double[] {0, 1, 0, 0}); + var B = new SimpleMatrix(2, 1, true, new double[] {0, 1}); + var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 1}); + var R = new SimpleMatrix(1, 1, true, new double[] {1}); + + var X = DARE.dare(A, B, Q, R); + assertMatrixEqual(X, X.transpose()); + assertDARESolution(A, B, Q, R, X); + } + + @Test + void testFirstGeneralizedEigenvalueOfSTIsStable_ABQRN() { + // The first generalized eigenvalue of (S, T) is stable + + var A = new SimpleMatrix(2, 2, true, new double[] {0, 1, 0, 0}); + var B = new SimpleMatrix(2, 1, true, new double[] {0, 1}); + var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 1}); + var R = new SimpleMatrix(1, 1, true, new double[] {1}); + + var Aref = new SimpleMatrix(2, 2, true, new double[] {0, 0.5, 0, 0}); + Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref)); + R = B.transpose().mult(Q).mult(B).plus(R); + var N = A.minus(Aref).transpose().mult(Q).mult(B); + + var X = DARE.dare(A, B, Q, R, N); + assertMatrixEqual(X, X.transpose()); + assertDARESolution(A, B, Q, R, N, X); + } + + @Test + void testIdentitySystem_ABQR() { + var A = SimpleMatrix.identity(2); + var B = SimpleMatrix.identity(2); + var Q = SimpleMatrix.identity(2); + var R = SimpleMatrix.identity(2); + + var X = DARE.dare(A, B, Q, R); + assertMatrixEqual(X, X.transpose()); + assertDARESolution(A, B, Q, R, X); + } + + @Test + void testIdentitySystem_ABQRN() { + var A = SimpleMatrix.identity(2); + var B = SimpleMatrix.identity(2); + var Q = SimpleMatrix.identity(2); + var R = SimpleMatrix.identity(2); + var N = SimpleMatrix.identity(2); + + var X = DARE.dare(A, B, Q, R, N); + assertMatrixEqual(X, X.transpose()); + assertDARESolution(A, B, Q, R, N, X); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java deleted file mode 100644 index 3050867226..0000000000 --- a/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java +++ /dev/null @@ -1,73 +0,0 @@ -// 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. - -package edu.wpi.first.math; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertTrue; - -import org.ejml.simple.SimpleMatrix; -import org.junit.jupiter.api.Test; - -class DrakeTest { - public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) { - for (int i = 0; i < A.numRows(); i++) { - for (int j = 0; j < A.numCols(); j++) { - assertEquals(A.get(i, j), B.get(i, j), 1e-4); - } - } - } - - private boolean solveDAREandVerify( - SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) { - var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R); - - // expect that x is the same as it's transpose - assertEquals(X.numRows(), X.numCols()); - assertMatrixEqual(X, X.transpose()); - - // Verify that this is a solution to the DARE. - SimpleMatrix Y = - A.transpose() - .mult(X) - .mult(A) - .minus(X) - .minus( - A.transpose() - .mult(X) - .mult(B) - .mult(((B.transpose().mult(X).mult(B)).plus(R)).invert()) - .mult(B.transpose()) - .mult(X) - .mult(A)) - .plus(Q); - assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols())); - - return true; - } - - @Test - void testDiscreteAlgebraicRicattiEquation() { - int n1 = 4; - int m1 = 1; - - // we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]] - SimpleMatrix A1 = - new SimpleMatrix( - n1, n1, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0}) - .transpose(); - SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[] {0, 0, 0, 1}); - SimpleMatrix Q1 = - new SimpleMatrix( - n1, n1, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}); - SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[] {0.25}); - assertTrue(solveDAREandVerify(A1, B1, Q1, R1)); - - SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1}); - SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[] {0, 1}); - SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0}); - SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[] {0.3}); - assertTrue(solveDAREandVerify(A2, B2, Q2, R2)); - } -} diff --git a/wpimath/src/test/native/cpp/DARETest.cpp b/wpimath/src/test/native/cpp/DARETest.cpp new file mode 100644 index 0000000000..3671c25546 --- /dev/null +++ b/wpimath/src/test/native/cpp/DARETest.cpp @@ -0,0 +1,220 @@ +// 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 + +#include "Eigen/Core" +#include "Eigen/Eigenvalues" +#include "frc/DARE.h" +#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& X) { + Eigen::SelfAdjointEigenSolver eigX(X); + for (int i = 0; i < X.rows(); ++i) { + EXPECT_GE(eigX.eigenvalues()[i], 0.0); + } +} + +void ExpectDARESolution(const Eigen::Ref& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& 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& A, + const Eigen::Ref& B, + const Eigen::Ref& Q, + const Eigen::Ref& R, + const Eigen::Ref& N, + const Eigen::Ref& 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); +} + +TEST(DARETest, NonInvertibleA_ABQR) { + // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic + // Riccati Equation" + + Eigen::MatrixXd A{4, 4}; + A << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; + Eigen::MatrixXd B{4, 1}; + B << 0, 0, 0, 1; + Eigen::MatrixXd Q{4, 4}; + Q << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + Eigen::MatrixXd R{1, 1}; + R << 0.25; + + Eigen::MatrixXd X = frc::DARE(A, B, Q, R); + ExpectMatrixEqual(X, X.transpose(), 1e-10); + ExpectPositiveSemidefinite(X); + ExpectDARESolution(A, B, Q, R, X); +} + +TEST(DARETest, NonInvertibleA_ABQRN) { + // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic + // Riccati Equation" + + Eigen::MatrixXd A{4, 4}; + A << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; + Eigen::MatrixXd B{4, 1}; + B << 0, 0, 0, 1; + Eigen::MatrixXd Q{4, 4}; + Q << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; + Eigen::MatrixXd R{1, 1}; + R << 0.25; + + Eigen::MatrixXd Aref{4, 4}; + Aref << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; + Q = (A - Aref).transpose() * Q * (A - Aref); + R = B.transpose() * Q * B + R; + Eigen::MatrixXd N = (A - Aref).transpose() * Q * B; + + Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N); + ExpectMatrixEqual(X, X.transpose(), 1e-10); + ExpectPositiveSemidefinite(X); + ExpectDARESolution(A, B, Q, R, N, X); +} + +TEST(DARETest, InvertibleA_ABQR) { + Eigen::MatrixXd A{2, 2}; + A << 1, 1, 0, 1; + Eigen::MatrixXd B{2, 1}; + B << 0, 1; + Eigen::MatrixXd Q{2, 2}; + Q << 1, 0, 0, 0; + Eigen::MatrixXd R{1, 1}; + R << 0.3; + + Eigen::MatrixXd X = frc::DARE(A, B, Q, R); + ExpectMatrixEqual(X, X.transpose(), 1e-10); + ExpectPositiveSemidefinite(X); + ExpectDARESolution(A, B, Q, R, X); +} + +TEST(DARETest, InvertibleA_ABQRN) { + Eigen::MatrixXd A{2, 2}; + A << 1, 1, 0, 1; + Eigen::MatrixXd B{2, 1}; + B << 0, 1; + Eigen::MatrixXd Q{2, 2}; + Q << 1, 0, 0, 0; + Eigen::MatrixXd R{1, 1}; + R << 0.3; + + Eigen::MatrixXd Aref{2, 2}; + Aref << 0.5, 1, 0, 1; + Q = (A - Aref).transpose() * Q * (A - Aref); + R = B.transpose() * Q * B + R; + Eigen::MatrixXd N = (A - Aref).transpose() * Q * B; + + Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N); + ExpectMatrixEqual(X, X.transpose(), 1e-10); + ExpectPositiveSemidefinite(X); + ExpectDARESolution(A, B, Q, R, N, X); +} + +TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQR) { + // The first generalized eigenvalue of (S, T) is stable + + Eigen::MatrixXd A{2, 2}; + A << 0, 1, 0, 0; + Eigen::MatrixXd B{2, 1}; + B << 0, 1; + Eigen::MatrixXd Q{2, 2}; + Q << 1, 0, 0, 1; + Eigen::MatrixXd R{1, 1}; + R << 1; + + Eigen::MatrixXd X = frc::DARE(A, B, Q, R); + ExpectMatrixEqual(X, X.transpose(), 1e-10); + ExpectPositiveSemidefinite(X); + ExpectDARESolution(A, B, Q, R, X); +} + +TEST(DARETest, FirstGeneralizedEigenvalueOfSTIsStable_ABQRN) { + // The first generalized eigenvalue of (S, T) is stable + + Eigen::MatrixXd A{2, 2}; + A << 0, 1, 0, 0; + Eigen::MatrixXd B{2, 1}; + B << 0, 1; + Eigen::MatrixXd Q{2, 2}; + Q << 1, 0, 0, 1; + Eigen::MatrixXd R{1, 1}; + R << 1; + + Eigen::MatrixXd Aref{2, 2}; + Aref << 0, 0.5, 0, 0; + Q = (A - Aref).transpose() * Q * (A - Aref); + R = B.transpose() * Q * B + R; + Eigen::MatrixXd N = (A - Aref).transpose() * Q * B; + + Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N); + ExpectMatrixEqual(X, X.transpose(), 1e-10); + ExpectPositiveSemidefinite(X); + ExpectDARESolution(A, B, Q, R, N, X); +} + +TEST(DARETest, IdentitySystem_ABQR) { + const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; + + Eigen::MatrixXd X = frc::DARE(A, B, Q, R); + ExpectMatrixEqual(X, X.transpose(), 1e-10); + ExpectPositiveSemidefinite(X); + ExpectDARESolution(A, B, Q, R, X); +} + +TEST(DARETest, IdentitySystem_ABQRN) { + const Eigen::MatrixXd A{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd B{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd Q{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd R{Eigen::Matrix2d::Identity()}; + const Eigen::MatrixXd N{Eigen::Matrix2d::Identity()}; + + Eigen::MatrixXd X = frc::DARE(A, B, Q, R, N); + ExpectMatrixEqual(X, X.transpose(), 1e-10); + ExpectPositiveSemidefinite(X); + ExpectDARESolution(A, B, Q, R, N, X); +} diff --git a/wpimath/src/test/native/cpp/drake/common/fmt_eigen.cpp b/wpimath/src/test/native/cpp/drake/common/fmt_eigen.cpp deleted file mode 100644 index 7ec8cd0f7e..0000000000 --- a/wpimath/src/test/native/cpp/drake/common/fmt_eigen.cpp +++ /dev/null @@ -1,41 +0,0 @@ -// TODO(jwnimmer-tri) Write our own formatting logic instead of using Eigen IO, -// and add customization flags for how to display the matrix data. -#undef EIGEN_NO_IO -#include "drake/common/fmt_eigen.h" - -#include -#include - -namespace drake { -namespace internal { - -template -using FormatterEigenRef = - Eigen::Ref>; - -template -std::string FormatEigenMatrix(const FormatterEigenRef& matrix) { - std::stringstream stream; - // We'll print our matrix data using as much precision as we can, so that - // console log output and/or error messages paint the full picture. Sadly, - // the ostream family of floating-point formatters doesn't know how to do - // "shortest round-trip precision". If we set the precision to max_digits, - // then simple numbers like "1.1" print as "1.1000000000000001"; instead, - // we'll use max_digits - 1 to avoid that problem, with the risk of losing - // the last ulps in the printout in case we needed that last digit. This - // will all be fixed once we stop using Eigen IO. - stream.precision(std::numeric_limits::max_digits10 - 1); - stream << matrix; - return stream.str(); -} - -// Explicitly instantiate for the allowed scalar types in our header. -template std::string FormatEigenMatrix( - const FormatterEigenRef& matrix); -template std::string FormatEigenMatrix( - const FormatterEigenRef& matrix); -template std::string FormatEigenMatrix( - const FormatterEigenRef& matrix); - -} // namespace internal -} // namespace drake diff --git a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp deleted file mode 100644 index 8631d6f3c0..0000000000 --- a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp +++ /dev/null @@ -1,124 +0,0 @@ -#include "drake/math/discrete_algebraic_riccati_equation.h" - -#include -#include - -#include "drake/common/test_utilities/eigen_matrix_compare.h" -// #include "drake/math/autodiff.h" - -using Eigen::MatrixXd; - -namespace drake { -namespace math { -namespace { -void SolveDAREandVerify(const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R) { - MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R); - // Check that X is positive semi-definite. - EXPECT_TRUE( - CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute)); - int n = X.rows(); - Eigen::SelfAdjointEigenSolver es(X); - for (int i = 0; i < n; i++) { - EXPECT_GE(es.eigenvalues()[i], 0); - } - // Check that X is the solution to the discrete time ARE. - // clang-format off - 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 - EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10, - MatrixCompareType::absolute)); -} - -void SolveDAREandVerify(const Eigen::Ref& A, - const Eigen::Ref& B, - const Eigen::Ref& Q, - const Eigen::Ref& R, - const Eigen::Ref& N) { - MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N); - // Check that X is positive semi-definite. - EXPECT_TRUE( - CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute)); - int n = X.rows(); - Eigen::SelfAdjointEigenSolver es(X); - for (int i = 0; i < n; i++) { - EXPECT_GE(es.eigenvalues()[i], 0); - } - // Check that X is the solution to the discrete time ARE. - // clang-format off - 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 - EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10, - MatrixCompareType::absolute)); -} - -GTEST_TEST(DARE, SolveDAREandVerify) { - // Test 1: non-invertible A - // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic - // Riccati Equation" - int n1 = 4, m1 = 1; - MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1); - A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; - B1 << 0, 0, 0, 1; - Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; - R1 << 0.25; - SolveDAREandVerify(A1, B1, Q1, R1); - - MatrixXd Aref1(n1, n1); - Aref1 << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0; - SolveDAREandVerify(A1, B1, (A1 - Aref1).transpose() * Q1 * (A1 - Aref1), - B1.transpose() * Q1 * B1 + R1, (A1 - Aref1).transpose() * Q1 * B1); - - // Test 2: invertible A - int n2 = 2, m2 = 1; - MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2); - A2 << 1, 1, 0, 1; - B2 << 0, 1; - Q2 << 1, 0, 0, 0; - R2 << 0.3; - SolveDAREandVerify(A2, B2, Q2, R2); - - MatrixXd Aref2(n2, n2); - Aref2 << 0.5, 1, 0, 1; - SolveDAREandVerify(A2, B2, (A2 - Aref2).transpose() * Q2 * (A2 - Aref2), - B2.transpose() * Q2 * B2 + R2, (A2 - Aref2).transpose() * Q2 * B2); - - // Test 3: the first generalized eigenvalue of (S,T) is stable - int n3 = 2, m3 = 1; - MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3); - A3 << 0, 1, 0, 0; - B3 << 0, 1; - Q3 << 1, 0, 0, 1; - R3 << 1; - SolveDAREandVerify(A3, B3, Q3, R3); - - MatrixXd Aref3(n3, n3); - Aref3 << 0, 0.5, 0, 0; - SolveDAREandVerify(A3, B3, (A3 - Aref3).transpose() * Q3 * (A3 - Aref3), - B3.transpose() * Q3 * B3 + R3, (A3 - Aref3).transpose() * Q3 * B3); - - // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix) - const Eigen::MatrixXd A4{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd B4{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd Q4{Eigen::Matrix2d::Identity()}; - const Eigen::MatrixXd R4{Eigen::Matrix2d::Identity()}; - SolveDAREandVerify(A4, B4, Q4, R4); - - const Eigen::MatrixXd N4{Eigen::Matrix2d::Identity()}; - SolveDAREandVerify(A4, B4, Q4, R4, N4); -} -} // namespace -} // namespace math -} // namespace drake diff --git a/wpimath/src/test/native/include/drake/common/fmt.h b/wpimath/src/test/native/include/drake/common/fmt.h deleted file mode 100644 index 7211bc6d4e..0000000000 --- a/wpimath/src/test/native/include/drake/common/fmt.h +++ /dev/null @@ -1,164 +0,0 @@ -#pragma once - -#include - -#include - -// This file contains the essentials of fmt support in Drake, mainly -// compatibility code to inter-operate with different versions of fmt. - -namespace drake { - -#if FMT_VERSION >= 80000 || defined(DRAKE_DOXYGEN_CXX) -/** When using fmt >= 8, this is an alias for -fmt::runtime. -When using fmt < 8, this is a no-op. */ -inline auto fmt_runtime(std::string_view s) { - return fmt::runtime(s); -} -/** When using fmt >= 8, this is defined to be `const` to indicate that the -`fmt::formatter::format(...)` function should be object-const. -When using fmt < 8, the function signature was incorrect (lacking the const), -so this macro will be empty. */ -#define DRAKE_FMT8_CONST const -#else // FMT_VERSION -inline auto fmt_runtime(std::string_view s) { - return s; -} -#define DRAKE_FMT8_CONST -#endif // FMT_VERSION - -namespace internal::formatter_as { - -/* The DRAKE_FORMATTER_AS macro specializes this for it's format_as types. -This struct is a functor that performs a conversion. See below for details. -@tparam T is the TYPE to be formatted. */ -template -struct Converter; - -/* Provides convenient type shortcuts for the TYPE in DRAKE_FORMATTER_AS. */ -template -struct Traits { - /* The type being formatted. */ - using InputType = T; - /* The format_as functor that provides the alternative object to format. */ - using Functor = Converter; - /* The type of the alternative object. */ - using OutputType = decltype(Functor::call(std::declval())); - /* The fmt::formatter<> for the alternative object. */ - using OutputTypeFormatter = fmt::formatter; -}; - -/* Implements fmt::formatter for DRAKE_FORMATER_AS types. The macro -specializes this for it's format_as types. See below for details. -@tparam T is the TYPE to be formatted. */ -template -struct Formatter; - -} // namespace internal::formatter_as - -} // namespace drake - -/** Adds a `fmt::formatter` template specialization that -formats the `TYPE` by delegating the formatting using a transformed expression, -as if a conversion function like this were interposed during formatting: - -@code{cpp} -template -auto format_as(const NAMESPACE::TYPE& ARG) { - return EXPR; -} -@endcode - -For example, this declaration ... - -@code{cpp} -DRAKE_FORMATTER_AS(, my_namespace, MyType, x, x.to_string()) -@endcode - -... changes this code ... - -@code{cpp} -MyType foo; -fmt::print(foo); -@endcode - -... to be equivalent to ... - -@code{cpp} -MyType foo; -fmt::print(foo.to_string()); -@endcode - -... allowing user to format `my_namespace::MyType` objects without manually -adding the `to_string` call every time. - -This provides a convenient mechanism to add formatters for classes that already -have a `to_string` function, or classes that are just thin wrappers over simple -types (ints, enums, etc.). - -Always use this macro in the global namespace, and do not use a semicolon (`;`) -afterward. - -@param TEMPLATE_ARGS The optional first argument `TEMPLATE_ARGS` can be used in -case the `TYPE` is templated, e.g., it might commonly be set to `typename T`. It -should be left empty when the TYPE is not templated. In case `TYPE` has multiple -template arguments, note that macros will fight with commas so you should use -`typename... Ts` instead of writing them all out. - -@param NAMESPACE The namespace that encloses the `TYPE` being formatted. Cannot -be empty. For nested namespaces, use intemediate colons, e.g., `%drake::common`. -Do not place _leading_ colons on the `NAMESPACE`. - -@param TYPE The class name (or struct name, or enum name, etc.) being formatted. -Do not place _leading_ double-colons on the `TYPE`. If the type is templated, -use the template arguments here, e.g., `MyOptional` if `TEMPLATE_ARGS` was -chosen as `typename T`. - -@param ARG A placeholder variable name to use for the value (i.e., object) -being formatted within the `EXPR` expression. - -@param EXPR An expression to `return` from the format_as function; it can -refer to the given `ARG` name which will be of type `const TYPE& ARG`. - -@note In future versions of fmt (perhaps fmt >= 10) there might be an ADL -`format_as` customization point with this feature built-in. If so, then we can -update this macro to use that spelling, and eventually deprecate the macro once -Drake drops support for earlier version of fmt. */ -#define DRAKE_FORMATTER_AS(TEMPLATE_ARGS, NAMESPACE, TYPE, ARG, EXPR) \ - /* Specializes the Converter<> class template for our TYPE. */ \ - namespace drake::internal::formatter_as { \ - template \ - struct Converter { \ - using InputType = NAMESPACE::TYPE; \ - static auto call(const InputType& ARG) { return EXPR; } \ - }; \ - \ - /* Provides the fmt::formatter implementation. */ \ - template \ - struct Formatter \ - : fmt::formatter::OutputType> { \ - using MyTraits = Traits; \ - /* Shadow our base class member function template of the same name. */ \ - template \ - auto format(const typename MyTraits::InputType& x, \ - FormatContext& ctx) DRAKE_FMT8_CONST { \ - /* Call the base class member function after laundering the object */ \ - /* through the user's provided format_as function. Older versions of */ \ - /* fmt have const-correctness bugs, which we can fix with some good */ \ - /* old fashioned const_cast-ing here. */ \ - using Base = typename MyTraits::OutputTypeFormatter; \ - const Base* const self = this; \ - return const_cast(self)->format( \ - MyTraits::Functor::call(x), \ - ctx); \ - } \ - }; \ - } /* namespace drake::internal::formatter_as */ \ - \ - /* Specializes the fmt::formatter<> class template for TYPE. */ \ - namespace fmt { \ - template \ - struct formatter \ - : drake::internal::formatter_as::Formatter {}; \ - } /* namespace fmt */ diff --git a/wpimath/src/test/native/include/drake/common/fmt_eigen.h b/wpimath/src/test/native/include/drake/common/fmt_eigen.h deleted file mode 100644 index e56d66417b..0000000000 --- a/wpimath/src/test/native/include/drake/common/fmt_eigen.h +++ /dev/null @@ -1,87 +0,0 @@ -#pragma once - -#include -#include - -#include - -#include "drake/common/fmt.h" - -namespace drake { -namespace internal { - -/* A tag type to be used in fmt::format("{}", fmt_eigen(...)) calls. -Below we'll add a fmt::formatter<> specialization for this tag. */ -template -struct fmt_eigen_ref { - const Eigen::MatrixBase& matrix; -}; - -/* Returns the string formatting of the given matrix. -@tparam T must be either double, float, or string */ -template -std::string FormatEigenMatrix( - const Eigen::Ref>& - matrix); - -} // namespace internal - -/** When passing an Eigen::Matrix to fmt, use this wrapper function to instruct -fmt to use Drake's custom formatter for Eigen types. - -Within Drake, when formatting an Eigen matrix into a string you must wrap the -Eigen object as `fmt_eigen(M)`. This holds true whether it be for logging, error -messages, debugging, or etc. - -For example: -@code -if (!CheckValid(M)) { - throw std::logic_error(fmt::format("Invalid M = {}", fmt_eigen(M))); -} -@endcode - -@warning The return value of this function should only ever be used as a -temporary object, i.e., in a fmt argument list or a logging statement argument -list. Never store it as a local variable, member field, etc. - -@note To ensure floating-point data is formatted without losing any digits, -Drake's code is compiled using -DEIGEN_NO_IO, which enforces that nothing within -Drake is allowed to use Eigen's `operator<<`. Downstream code that calls into -Drake is not required to use that option; it is only enforced by Drake's build -system, not by Drake's headers. */ -template -internal::fmt_eigen_ref fmt_eigen( - const Eigen::MatrixBase& matrix) { - return {matrix}; -} - -} // namespace drake - -#ifndef DRAKE_DOXYGEN_CXX -// Formatter specialization for drake::fmt_eigen. -namespace fmt { -template -struct formatter> - : formatter { - template - auto format(const drake::internal::fmt_eigen_ref& ref, - // NOLINTNEXTLINE(runtime/references) To match fmt API. - FormatContext& ctx) DRAKE_FMT8_CONST -> decltype(ctx.out()) { - using Scalar = typename Derived::Scalar; - const auto& matrix = ref.matrix; - if constexpr (std::is_same_v || - std::is_same_v) { - return formatter{}.format( - drake::internal::FormatEigenMatrix(matrix), ctx); - } else { - return formatter{}.format( - drake::internal::FormatEigenMatrix( - matrix.unaryExpr([](const auto& element) -> std::string { - return fmt::to_string(element); - })), - ctx); - } - } -}; -} // namespace fmt -#endif diff --git a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h deleted file mode 100644 index cd5dae8fd7..0000000000 --- a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h +++ /dev/null @@ -1,113 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include -#include - -#include "drake/common/fmt_eigen.h" -// #include "drake/common/text_logging.h" - -namespace drake { - -enum class MatrixCompareType { absolute, relative }; - -/** - * Compares two matrices to determine whether they are equal to within a certain - * threshold. - * - * @param m1 The first matrix to compare. - * @param m2 The second matrix to compare. - * @param tolerance The tolerance for determining equivalence. - * @param compare_type Whether the tolereance is absolute or relative. - * @param explanation A pointer to a string variable for saving an explanation - * of why @p m1 and @p m2 are unequal. This parameter is optional and defaults - * to `nullptr`. If this is `nullptr` and @p m1 and @p m2 are not equal, an - * explanation is logged as an error message. - * @return true if the two matrices are equal based on the specified tolerance. - */ -template -[[nodiscard]] ::testing::AssertionResult CompareMatrices( - const Eigen::MatrixBase& m1, - const Eigen::MatrixBase& m2, double tolerance = 0.0, - MatrixCompareType compare_type = MatrixCompareType::absolute) { - if (m1.rows() != m2.rows() || m1.cols() != m2.cols()) { - const std::string message = - fmt::format("Matrix size mismatch: ({} x {} vs. {} x {})", m1.rows(), - m1.cols(), m2.rows(), m2.cols()); - return ::testing::AssertionFailure() << message; - } - - for (int ii = 0; ii < m1.rows(); ii++) { - for (int jj = 0; jj < m1.cols(); jj++) { - // First handle the corner cases of positive infinity, negative infinity, - // and NaN - const auto both_positive_infinity = - m1(ii, jj) == std::numeric_limits::infinity() && - m2(ii, jj) == std::numeric_limits::infinity(); - - const auto both_negative_infinity = - m1(ii, jj) == -std::numeric_limits::infinity() && - m2(ii, jj) == -std::numeric_limits::infinity(); - - using std::isnan; - const auto both_nan = isnan(m1(ii, jj)) && isnan(m2(ii, jj)); - - if (both_positive_infinity || both_negative_infinity || both_nan) - continue; - - // Check for case where one value is NaN and the other is not - if ((isnan(m1(ii, jj)) && !isnan(m2(ii, jj))) || - (!isnan(m1(ii, jj)) && isnan(m2(ii, jj)))) { - const std::string message = - fmt::format("NaN mismatch at ({}, {}):\nm1 =\n{}\nm2 =\n{}", ii, jj, - fmt_eigen(m1), fmt_eigen(m2)); - return ::testing::AssertionFailure() << message; - } - - // Determine whether the difference between the two matrices is less than - // the tolerance. - using std::abs; - const auto delta = abs(m1(ii, jj) - m2(ii, jj)); - - if (compare_type == MatrixCompareType::absolute) { - // Perform comparison using absolute tolerance. - if (delta > tolerance) { - const std::string message = fmt::format( - "Values at ({}, {}) exceed tolerance: {} vs. {}, diff = {}, " - "tolerance = {}\nm1 =\n{}\nm2 =\n{}\ndelta=\n{}", - ii, jj, m1(ii, jj), m2(ii, jj), delta, tolerance, fmt_eigen(m1), - fmt_eigen(m2), fmt_eigen(m1 - m2)); - return ::testing::AssertionFailure() << message; - } - } else { - // Perform comparison using relative tolerance, see: - // http://realtimecollisiondetection.net/blog/?p=89 - using std::max; - const auto max_value = max(abs(m1(ii, jj)), abs(m2(ii, jj))); - const auto relative_tolerance = - tolerance * max(decltype(max_value){1}, max_value); - if (delta > relative_tolerance) { - const std::string message = fmt::format( - "Values at ({}, {}) exceed tolerance: {} vs. {}, diff = {}, " - "tolerance = {}, relative tolerance = {}\nm1 =\n{}\nm2 " - "=\n{}\ndelta=\n{}", - ii, jj, m1(ii, jj), m2(ii, jj), delta, tolerance, - relative_tolerance, fmt_eigen(m1), fmt_eigen(m2), - fmt_eigen(m1 - m2)); - return ::testing::AssertionFailure() << message; - } - } - } - } - - const std::string message = - fmt::format("m1 =\n{}\nis approximately equal to m2 =\n{}", fmt_eigen(m1), - fmt_eigen(m2)); - return ::testing::AssertionSuccess() << message; -} - -} // namespace drake