Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Update drake with upstream (#3484)

Browse Source 
Our patches for the DARE and [[noreturn]] attributes were merged
upstream. We missed their monthly release window by a day, so we'll use
a commit hash for now.
This commit is contained in:
Tyler Veness
2021-07-22 17:48:48 -07:00
committed by GitHub
parent 1ef826d1da
commit ab8e8aa2a1
10 changed files with 97 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
wpimath/drake-dare-qrn-tests.patch
View File

@@ -1,82 +0,0 @@
diff --git a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
index 2deb039a0..2ee42a19c 100644
--- a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
+++ b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
@@ -33,6 +33,29 @@ void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
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.
+ 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;
+ 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
@@ -44,6 +67,12 @@ GTEST_TEST(DARE, SolveDAREandVerify) {
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);
@@ -52,6 +81,12 @@ GTEST_TEST(DARE, SolveDAREandVerify) {
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);
@@ -60,12 +95,21 @@ GTEST_TEST(DARE, SolveDAREandVerify) {
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

55
wpimath/drake-dare-qrn.patch
View File

@@ -1,55 +0,0 @@
diff --git a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
index 9585c4dae..d53fbdddf 100644
--- a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
+++ b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
@@ -453,5 +453,18 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
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());
+
+ Eigen::MatrixXd scrA = A - B * R.llt().solve(N.transpose());
+ Eigen::MatrixXd scrQ = Q - N * R.llt().solve(N.transpose());
+ return DiscreteAlgebraicRiccatiEquation(scrA, B, scrQ, R);
+}
+
} // namespace math
} // namespace drake
diff --git a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
index e3fea30c5..9b9fe97ec 100644
--- a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
+++ b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
@@ -27,6 +27,27 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& B,
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R);
+
+/// DiscreteAlgebraicRiccatiEquation function
+/// computes the unique stabilizing solution X to the discrete-time algebraic
+/// Riccati equation:
+/// \f[
+/// A'XA - X - (A'XB + N)(B'XB + R)^{-1}(B'XA + N') + Q = 0
+/// \f]
+///
+/// See
+/// https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
+/// for the change of variables used here.
+///
+/// @throws std::runtime_error if Q is not positive semi-definite.
+/// @throws std::runtime_error if R is not positive definite.
+///
+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

30
wpimath/drake-replace-noreturn-attributes.patch
View File

@@ -1,30 +0,0 @@
diff --git b/wpimath/src/main/native/include/drake/common/drake_assert.h a/wpimath/src/main/native/include/drake/common/drake_assert.h
index acc1298fe..21e7bd100 100644
--- b/wpimath/src/main/native/include/drake/common/drake_assert.h
+++ a/wpimath/src/main/native/include/drake/common/drake_assert.h
@@ -83,10 +83,10 @@
namespace drake {
namespace internal {
// Abort the program with an error message.
-__attribute__((noreturn)) /* gcc is ok with [[noreturn]]; clang is not. */
+[[noreturn]]
void Abort(const char* condition, const char* func, const char* file, int line);
// Report an assertion failure; will either Abort(...) or throw.
-__attribute__((noreturn)) /* gcc is ok with [[noreturn]]; clang is not. */
+[[noreturn]]
void AssertionFailed(
const char* condition, const char* func, const char* file, int line);
} // namespace internal
diff --git b/wpimath/src/main/native/include/drake/common/drake_throw.h a/wpimath/src/main/native/include/drake/common/drake_throw.h
index ffa617c25..d19e4efb7 100644
--- b/wpimath/src/main/native/include/drake/common/drake_throw.h
+++ a/wpimath/src/main/native/include/drake/common/drake_throw.h
@@ -12,7 +12,7 @@
namespace drake {
namespace internal {
// Throw an error message.
-__attribute__((noreturn)) /* gcc is ok with [[noreturn]]; clang is not. */
+[[noreturn]]
void Throw(const char* condition, const char* func, const char* file, int line);
} // namespace internal
} // namespace drake

24
wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
View File

@@ -382,12 +382,11 @@ void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
* DiscreteAlgebraicRiccatiEquation function
* computes the unique stabilizing solution X to the discrete-time algebraic
* Riccati equation:
* \f[
* A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
* \f]
*
* @throws std::runtime_error if Q is not positive semi-definite.
* @throws std::runtime_error if R is not positive definite.
* 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"
@@ -396,9 +395,9 @@ void reorder_eigen(Eigen::Ref<Eigen::MatrixXd> S, Eigen::Ref<Eigen::MatrixXd> T,
*
* 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^{-6}, while the absolute error of the solution computed by Matlab is
* 10^{-8}.
* 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.
@@ -464,9 +463,12 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& N) {
DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
Eigen::MatrixXd scrA = A - B * R.llt().solve(N.transpose());
Eigen::MatrixXd scrQ = Q - N * R.llt().solve(N.transpose());
return DiscreteAlgebraicRiccatiEquation(scrA, B, scrQ, R);
// 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

9
wpimath/src/main/native/include/drake/common/drake_assert.h
View File

@@ -83,12 +83,11 @@
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);
[[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);
[[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

4
wpimath/src/main/native/include/drake/common/drake_throw.h
View File

@@ -12,8 +12,8 @@
namespace drake {
namespace internal {
// Throw an error message.
[[noreturn]]
void Throw(const char* condition, const char* func, const char* file, int line);
[[noreturn]] void Throw(const char* condition, const char* func,
const char* file, int line);
} // namespace internal
} // namespace drake

19
wpimath/src/main/native/include/drake/common/never_destroyed.h
View File

@@ -56,6 +56,25 @@ namespace drake {
/// 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>

55
wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
View File

@@ -11,12 +11,10 @@ namespace math {
/// Computes the unique stabilizing solution X to the discrete-time algebraic
/// Riccati equation:
///
/// @f[
/// A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
/// @f]
/// AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
///
/// @throws std::runtime_error if Q is not positive semi-definite.
/// @throws std::runtime_error if R is not positive definite.
/// @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"
@@ -28,18 +26,47 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
const Eigen::Ref<const Eigen::MatrixXd>& Q,
const Eigen::Ref<const Eigen::MatrixXd>& R);
/// DiscreteAlgebraicRiccatiEquation function
/// computes the unique stabilizing solution X to the discrete-time algebraic
/// Computes the unique stabilizing solution X to the discrete-time algebraic
/// Riccati equation:
/// \f[
/// A'XA - X - (A'XB + N)(B'XB + R)^{-1}(B'XA + N') + Q = 0
/// \f]
///
/// See
/// https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
/// for the change of variables used here.
/// AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
///
/// @throws std::runtime_error if Q is not positive semi-definite.
/// 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.
///
Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(

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

@@ -25,10 +25,14 @@ void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
EXPECT_GE(es.eigenvalues()[i], 0);
}
// Check that X is the solution to the discrete time ARE.
MatrixXd Y = A.transpose() * X * A - X -
A.transpose() * X * B * (B.transpose() * X * B + R).inverse() *
B.transpose() * X * A +
Q;
// 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));
}
@@ -48,10 +52,14 @@ void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
EXPECT_GE(es.eigenvalues()[i], 0);
}
// Check that X is the solution to the discrete time ARE.
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 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));
}

7
wpimath/update_drake.py
View File

@@ -59,7 +59,8 @@ def drake_test_h_inclusions(dp, f):
def main():
wpimath = os.getcwd()
clone_repo("https://github.com/RobotLocomotion/drake", "v0.31.0")
clone_repo("https://github.com/RobotLocomotion/drake",
"8b72428dce6959d077e17c3c3a7a5ef4a17107ee")
repo = os.getcwd()
# Copy drake source files into allwpilib
@@ -128,10 +129,6 @@ def main():
# Apply patches
os.chdir(wpimath)
subprocess.check_output(["git", "apply", "drake-dare-qrn.patch"])
subprocess.check_output(["git", "apply", "drake-dare-qrn-tests.patch"])
subprocess.check_output(
["git", "apply", "drake-replace-noreturn-attributes.patch"])
subprocess.check_output(
["git", "apply", "drake-replace-dense-with-core.patch"])