Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Rewrite DARE solver (#5328)

Browse Source 
I timed the DARE unit tests, and the new solver is 0 to 100% faster in
all cases (that is, it's at least as fast as Drake's and up to 2x faster
in some cases).

The new solver is also much simpler, takes less time to compile, and
drops the libwpimath.so size from 325 MB to 301 MB.

I think most of the compilation time is coming from the eigenvalue
decompositions used to enforce argument preconditions.
This commit is contained in:
Tyler Veness
2023-05-14 22:23:00 -07:00
committed by GitHub
parent 3876a2523a
commit 52bd5b972d
32 changed files with 831 additions and 2024 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.github/workflows/upstream-utils.yml vendored
View File

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

76
upstream_utils/drake_patches/0001-Replace-Eigen-Dense-with-Eigen-Core.patch
View File

@@ -1,76 +0,0 @@
From 32dd1aa796fbb45d02f8bd445cc962e6a91318e7 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Wed, 18 May 2022 11:13:21 -0700
Subject: [PATCH 1/2] Replace <Eigen/Dense> with <Eigen/Core>
---
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 <vector>
-#include <Eigen/Dense>
+#include <Eigen/Core>
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 <limits>
#include <string>
-#include <Eigen/Dense>
+#include <Eigen/Core>
#include <gtest/gtest.h>
#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 <Eigen/Eigenvalues>
+#include <Eigen/QR>
+
#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 <cmath>
#include <cstdlib>
-#include <Eigen/Dense>
+#include <Eigen/Core>
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 <Eigen/Eigenvalues>
#include <gtest/gtest.h>
#include "drake/common/test_utilities/eigen_matrix_compare.h"

37
upstream_utils/drake_patches/0002-Add-WPILIB_DLLEXPORT-to-DARE-function-declarations.patch
View File

@@ -1,37 +0,0 @@
From 48ed223a299304724a38ad9911f7a732bf574b71 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
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 <cstdlib>
#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& 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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,

104
upstream_utils/update_drake.py
View File

@@ -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()

65
wpimath/src/main/java/edu/wpi/first/math/Drake.java → wpimath/src/main/java/edu/wpi/first/math/DARE.java
View File

@@ -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 <States extends Num, Inputs extends Num>
Matrix<States, States> discreteAlgebraicRiccatiEquation(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R) {
return new Matrix<>(
discreteAlgebraicRiccatiEquation(
A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> 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 <States extends Num, Inputs extends Num>
Matrix<States, States> discreteAlgebraicRiccatiEquation(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N) {
// See
// https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
// for the change of variables used here.
var scrA = A.minus(B.times(R.solve(N.transpose())));
var scrQ = Q.minus(N.times(R.solve(N.transpose())));
public static <States extends Num, Inputs extends Num> Matrix<States, States> dare(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> 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()));
}
}

2
wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
View File

@@ -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);
/**

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

@@ -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 extends Num, Inputs extends Num, Ou
throw new IllegalArgumentException(msg);
}
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
var S = DARE.dare(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K =
@@ -150,7 +150,7 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Ou
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
var S = DARE.dare(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K =

5
wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
View File

@@ -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.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
@@ -145,8 +145,7 @@ public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Output
final var discR = Discretization.discretizeR(m_contR, dtSeconds);
if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
m_initP =
Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR);
m_initP = DARE.dare(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = new Matrix<>(states, states);
}

6
wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
View File

@@ -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<States extends Num, Inputs extends Num, Outputs extend
throw new IllegalArgumentException(msg);
}
var P =
new Matrix<>(
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);

265
wpimath/src/main/native/cpp/DARE.cpp Normal file
View File

@@ -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 <cassert>
#include <stdexcept>
#include <string>
#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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
Eigen::EigenSolver<Eigen::MatrixXd> 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<Eigen::MatrixXcd> 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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& 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<const Eigen::MatrixXd>& A) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> 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<const Eigen::MatrixXd>& A) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> 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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R) {
// 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<Eigen::MatrixXd> 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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N) {
// 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

35
wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
View File

@@ -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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
int states = B.rows();
int inputs = B.cols();
bool IsStabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B) {
Eigen::EigenSolver<Eigen::MatrixXd> 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<Eigen::MatrixXcd> qr{E};
if (qr.rank() < states) {
if (qr.rank() < A.rows()) {
return false;
}
}
return true;
}
} // namespace
std::vector<double> GetElementsFromTrajectory(
const frc::Trajectory& trajectory) {
std::vector<double> 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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
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);

91
wpimath/src/main/native/include/frc/DARE.h Normal file
View File

@@ -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 <wpi/SymbolExports.h>
#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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R);
/**
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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N);
} // namespace frc

9
wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc
View File

@@ -8,8 +8,7 @@
#include <string>
#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<States, Inputs>::LinearQuadraticRegulator(
throw std::invalid_argument(msg);
}
Matrixd<States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
Matrixd<States, States> S = DARE(discA, discB, Q, R);
// K = (BᵀSB + R)⁻¹BᵀSA
m_K = (discB.transpose() * S * discB + R)
@@ -72,8 +70,7 @@ LinearQuadraticRegulator<States, Inputs>::LinearQuadraticRegulator(
Matrixd<States, Inputs> discB;
DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
Matrixd<States, States> S =
drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
Matrixd<States, States> S = DARE(discA, discB, Q, R, N);
// K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
m_K = (discB.transpose() * S * discB + R)

8
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
View File

@@ -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<States, Inputs, Outputs>::ExtendedKalmanFilter(
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(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<States, Inputs, Outputs>::ExtendedKalmanFilter(
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(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();
}

6
wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
View File

@@ -11,7 +11,7 @@
#include <string>
#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<States, Inputs, Outputs>::KalmanFilter(
throw std::invalid_argument(msg);
}
Matrixd<States, States> P = drake::math::DiscreteAlgebraicRiccatiEquation(
discA.transpose(), C.transpose(), discQ, discR);
Matrixd<States, States> P =
DARE(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;

162
wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assert.h vendored
View File

@@ -1,162 +0,0 @@
#pragma once
#include <type_traits>
/// @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 <cassert>. 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 <typename Condition>
struct ConditionTraits {
static constexpr bool is_valid = std::is_convertible_v<Condition, bool>;
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<decltype(condition)>> Trait; \
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
static_assert( \
!std::is_pointer_v<decltype(condition)>, \
"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<decltype(expression), void>, \
"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<decltype(condition)>> Trait; \
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
static_assert( \
!std::is_pointer_v<decltype(condition)>, \
"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<decltype(expression), void>, \
"Expression should be void.")
#endif
#endif // DRAKE_DOXYGEN_CXX

18
wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_assertion_error.h vendored
View File

@@ -1,18 +0,0 @@
#pragma once
#include <stdexcept>
#include <string>
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

90
wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_copyable.h vendored
View File

@@ -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.:
<pre>
class Foo {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Foo)
// ...
};
</pre>
*/
#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.:
<pre>
class Foo {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Foo)
// ...
};
</pre>
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
<a href="https://en.wikipedia.org/wiki/Object_slicing">object slicing</a>.
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.");

47
wpimath/src/main/native/thirdparty/drake/include/drake/common/drake_throw.h vendored
View File

@@ -1,47 +0,0 @@
#pragma once
#include <type_traits>
#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<decltype(condition)>> Trait; \
static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
static_assert( \
!std::is_pointer_v<decltype(condition)>, \
"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)

62
wpimath/src/main/native/thirdparty/drake/include/drake/common/is_approx_equal_abstol.h vendored
View File

@@ -1,62 +0,0 @@
#pragma once
#include <vector>
#include <Eigen/Core>
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 <typename DerivedA, typename DerivedB>
bool is_approx_equal_abstol(const Eigen::MatrixBase<DerivedA>& m1,
const Eigen::MatrixBase<DerivedB>& m2,
double tolerance) {
return (
(m1.rows() == m2.rows()) &&
(m1.cols() == m2.cols()) &&
((m1 - m2).template lpNorm<Eigen::Infinity>() <= 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
/// <pre>
/// 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.
/// </pre>
/// 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 <typename DerivedA, typename DerivedB>
bool IsApproxEqualAbsTolWithPermutedColumns(
const Eigen::MatrixBase<DerivedA>& m1,
const Eigen::MatrixBase<DerivedB>& m2, double tolerance) {
if ((m1.cols() != m2.cols()) || (m1.rows() != m2.rows())) return false;
std::vector<bool> 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

103
wpimath/src/main/native/thirdparty/drake/include/drake/common/never_destroyed.h vendored
View File

@@ -1,103 +0,0 @@
#pragma once
#include <new>
#include <type_traits>
#include <utility>
#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
/// <a href="https://drake.mit.edu/styleguide/cppguide.html#Static_and_Global_Variables">Static
/// and Global Variables</a> 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<Singleton> instance;
/// return instance.access();
/// }
/// private:
/// friend never_destroyed<Singleton>;
/// 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<std::string, Foo>;
/// static const drake::never_destroyed<Dict> string_to_enum{
/// std::initializer_list<Dict::value_type>{
/// {"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<double>& GetConstantMagicNumbers() {
/// static const drake::never_destroyed<std::vector<double>> result{[]() {
/// std::vector<double> 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 <typename T>
class never_destroyed {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(never_destroyed)
/// Passes the constructor arguments along to T using perfect forwarding.
template <typename... Args>
explicit never_destroyed(Args&&... args) {
// Uses "placement new" to construct a `T` in `storage_`.
new (&storage_) T(std::forward<Args>(args)...);
}
/// Does nothing. Guaranteed!
~never_destroyed() = default;
/// Returns the underlying T reference.
T& access() { return *reinterpret_cast<T*>(&storage_); }
const T& access() const { return *reinterpret_cast<const T*>(&storage_); }
private:
typename std::aligned_storage<sizeof(T), alignof(T)>::type storage_;
};
} // namespace drake

85
wpimath/src/main/native/thirdparty/drake/include/drake/math/discrete_algebraic_riccati_equation.h vendored
View File

@@ -1,85 +0,0 @@
#pragma once
#include <cmath>
#include <cstdlib>
#include <Eigen/Core>
#include <wpi/SymbolExports.h>
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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R);
/**
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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N);
} // namespace math
} // namespace drake

87
wpimath/src/main/native/thirdparty/drake/src/common/drake_assert_and_throw.cpp vendored
View File

@@ -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 <atomic>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#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<AssertionConfig> global;
return global.access();
}
std::atomic<bool> 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;
}

475
wpimath/src/main/native/thirdparty/drake/src/math/discrete_algebraic_riccati_equation.cpp vendored
View File

@@ -1,475 +0,0 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#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 <typename T>
int sgn(T val) {
return (T(0) < val) - (val < T(0));
}
void check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& 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<Eigen::MatrixXd> 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<Eigen::MatrixXcd> qr(E);
DRAKE_THROW_UNLESS(qr.rank() == n);
}
}
void check_detectable(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& 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<Eigen::MatrixXd> 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<Eigen::Matrix2d> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<double, 8, 8> C = Eigen::Matrix<double, 8, 8>::Zero();
Eigen::Matrix<double, 8, 1> 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<double, 4, 2> 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<Eigen::Matrix<double, 4, 2> > qr1(X);
Z1.block<4, 4>(p, p) = qr1.householderQ();
Eigen::ColPivHouseholderQR<Eigen::Matrix<double, 4, 2> > 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
Eigen::Ref<Eigen::MatrixXd> 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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R) {
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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N) {
DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
// 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

185
wpimath/src/test/java/edu/wpi/first/math/DARETest.java Normal file
View File

@@ -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);
}
}

73
wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
View File

@@ -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));
}
}

220
wpimath/src/test/native/cpp/DARETest.cpp Normal file
View File

@@ -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 <fmt/core.h>
#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<const Eigen::MatrixXd>& X) {
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigX(X);
for (int i = 0; i < X.rows(); ++i) {
EXPECT_GE(eigX.eigenvalues()[i], 0.0);
}
}
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& X) {
// Check that X is the solution to the DARE
// Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
// clang-format off
Eigen::MatrixXd Y =
A.transpose() * X * A
- X
- (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
* B.transpose() * X * A)
+ Q;
// clang-format on
ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
}
void ExpectDARESolution(const Eigen::Ref<const Eigen::MatrixXd>& A,
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R,
const Eigen::Ref<const Eigen::MatrixXd>& N,
const Eigen::Ref<const Eigen::MatrixXd>& X) {
// Check that X is the solution to the DARE
// Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
// clang-format off
Eigen::MatrixXd Y =
A.transpose() * X * A
- X
- ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
* (B.transpose() * X * A + N.transpose()))
+ Q;
// clang-format on
ExpectMatrixEqual(Y, Eigen::MatrixXd::Zero(X.rows(), X.cols()), 1e-10);
}
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);
}

41
wpimath/src/test/native/cpp/drake/common/fmt_eigen.cpp
View File

@@ -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 <limits>
#include <sstream>
namespace drake {
namespace internal {
template <typename T>
using FormatterEigenRef =
Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>;
template <typename T>
std::string FormatEigenMatrix(const FormatterEigenRef<T>& 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<double>::max_digits10 - 1);
stream << matrix;
return stream.str();
}
// Explicitly instantiate for the allowed scalar types in our header.
template std::string FormatEigenMatrix<double>(
const FormatterEigenRef<double>& matrix);
template std::string FormatEigenMatrix<float>(
const FormatterEigenRef<float>& matrix);
template std::string FormatEigenMatrix<std::string>(
const FormatterEigenRef<std::string>& matrix);
} // namespace internal
} // namespace drake

124
wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
View File

@@ -1,124 +0,0 @@
#include "drake/math/discrete_algebraic_riccati_equation.h"
#include <Eigen/Eigenvalues>
#include <gtest/gtest.h>
#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<const MatrixXd>& A,
const Eigen::Ref<const MatrixXd>& B,
const Eigen::Ref<const MatrixXd>& Q,
const Eigen::Ref<const MatrixXd>& 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<MatrixXd> 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<const MatrixXd>& A,
const Eigen::Ref<const MatrixXd>& B,
const Eigen::Ref<const MatrixXd>& Q,
const Eigen::Ref<const MatrixXd>& R,
const Eigen::Ref<const MatrixXd>& 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<MatrixXd> 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

164
wpimath/src/test/native/include/drake/common/fmt.h
View File

@@ -1,164 +0,0 @@
#pragma once
#include <string_view>
#include <fmt/format.h>
// 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
<a href="https://fmt.dev/latest/api.html#compile-time-format-string-checks">fmt::runtime</a>.
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<T>::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 <typename T>
struct Converter;
/* Provides convenient type shortcuts for the TYPE in DRAKE_FORMATTER_AS. */
template <typename T>
struct Traits {
/* The type being formatted. */
using InputType = T;
/* The format_as functor that provides the alternative object to format. */
using Functor = Converter<T>;
/* The type of the alternative object. */
using OutputType = decltype(Functor::call(std::declval<InputType>()));
/* The fmt::formatter<> for the alternative object. */
using OutputTypeFormatter = fmt::formatter<OutputType>;
};
/* Implements fmt::formatter<TYPE> 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 <typename T>
struct Formatter;
} // namespace internal::formatter_as
} // namespace drake
/** Adds a `fmt::formatter<NAMESPACE::TYPE>` 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 <TEMPLATE_ARGS>
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<T>` 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 <TEMPLATE_ARGS> \
struct Converter<NAMESPACE::TYPE> { \
using InputType = NAMESPACE::TYPE; \
static auto call(const InputType& ARG) { return EXPR; } \
}; \
\
/* Provides the fmt::formatter<TYPE> implementation. */ \
template <TEMPLATE_ARGS> \
struct Formatter<NAMESPACE::TYPE> \
: fmt::formatter<typename Traits<NAMESPACE::TYPE>::OutputType> { \
using MyTraits = Traits<NAMESPACE::TYPE>; \
/* Shadow our base class member function template of the same name. */ \
template <typename FormatContext> \
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<Base*>(self)->format( \
MyTraits::Functor::call(x), \
ctx); \
} \
}; \
} /* namespace drake::internal::formatter_as */ \
\
/* Specializes the fmt::formatter<> class template for TYPE. */ \
namespace fmt { \
template <TEMPLATE_ARGS> \
struct formatter<NAMESPACE::TYPE> \
: drake::internal::formatter_as::Formatter<NAMESPACE::TYPE> {}; \
} /* namespace fmt */

87
wpimath/src/test/native/include/drake/common/fmt_eigen.h
View File

@@ -1,87 +0,0 @@
#pragma once
#include <string>
#include <string_view>
#include <Eigen/Core>
#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 <typename Derived>
struct fmt_eigen_ref {
const Eigen::MatrixBase<Derived>& matrix;
};
/* Returns the string formatting of the given matrix.
@tparam T must be either double, float, or string */
template <typename T>
std::string FormatEigenMatrix(
const Eigen::Ref<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>>&
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 <typename Derived>
internal::fmt_eigen_ref<Derived> fmt_eigen(
const Eigen::MatrixBase<Derived>& matrix) {
return {matrix};
}
} // namespace drake
#ifndef DRAKE_DOXYGEN_CXX
// Formatter specialization for drake::fmt_eigen.
namespace fmt {
template <typename Derived>
struct formatter<drake::internal::fmt_eigen_ref<Derived>>
: formatter<std::string_view> {
template <typename FormatContext>
auto format(const drake::internal::fmt_eigen_ref<Derived>& 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<Scalar, double> ||
std::is_same_v<Scalar, float>) {
return formatter<std::string_view>{}.format(
drake::internal::FormatEigenMatrix<Scalar>(matrix), ctx);
} else {
return formatter<std::string_view>{}.format(
drake::internal::FormatEigenMatrix<std::string>(
matrix.unaryExpr([](const auto& element) -> std::string {
return fmt::to_string(element);
})),
ctx);
}
}
};
} // namespace fmt
#endif

113
wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
View File

@@ -1,113 +0,0 @@
#pragma once
#include <algorithm>
#include <cmath>
#include <limits>
#include <string>
#include <Eigen/Core>
#include <gtest/gtest.h>
#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 <typename DerivedA, typename DerivedB>
[[nodiscard]] ::testing::AssertionResult CompareMatrices(
const Eigen::MatrixBase<DerivedA>& m1,
const Eigen::MatrixBase<DerivedB>& 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<double>::infinity() &&
m2(ii, jj) == std::numeric_limits<double>::infinity();
const auto both_negative_infinity =
m1(ii, jj) == -std::numeric_limits<double>::infinity() &&
m2(ii, jj) == -std::numeric_limits<double>::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