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