diff --git a/wpimath/drake-dare-qrn-tests.patch b/wpimath/drake-dare-qrn-tests.patch deleted file mode 100644 index 16b6d20dd8..0000000000 --- a/wpimath/drake-dare-qrn-tests.patch +++ /dev/null @@ -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& A, - MatrixCompareType::absolute)); - } - -+void SolveDAREandVerify(const Eigen::Ref& A, -+ const Eigen::Ref& B, -+ const Eigen::Ref& Q, -+ const Eigen::Ref& R, -+ const Eigen::Ref& N) { -+ MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N); -+ // Check that X is positive semi-definite. -+ EXPECT_TRUE( -+ CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute)); -+ int n = X.rows(); -+ Eigen::SelfAdjointEigenSolver es(X); -+ for (int i = 0; i < n; i++) { -+ EXPECT_GE(es.eigenvalues()[i], 0); -+ } -+ // Check that X is the solution to the discrete time ARE. -+ 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 diff --git a/wpimath/drake-dare-qrn.patch b/wpimath/drake-dare-qrn.patch deleted file mode 100644 index 8033d2dbc8..0000000000 --- a/wpimath/drake-dare-qrn.patch +++ /dev/null @@ -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& A, -+ const Eigen::Ref& B, -+ const Eigen::Ref& Q, -+ const Eigen::Ref& R, -+ const Eigen::Ref& N) { -+ DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols()); -+ -+ 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& B, - const Eigen::Ref& Q, - const Eigen::Ref& 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& A, -+ const Eigen::Ref& B, -+ const Eigen::Ref& Q, -+ const Eigen::Ref& R, -+ const Eigen::Ref& N); - } // namespace math - } // namespace drake - diff --git a/wpimath/drake-replace-noreturn-attributes.patch b/wpimath/drake-replace-noreturn-attributes.patch deleted file mode 100644 index 20fd8b9d80..0000000000 --- a/wpimath/drake-replace-noreturn-attributes.patch +++ /dev/null @@ -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 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 268de596af..20ea2b7bbe 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 @@ -382,12 +382,11 @@ void reorder_eigen(Eigen::Ref S, Eigen::Ref 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 S, Eigen::Ref 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& 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 diff --git a/wpimath/src/main/native/include/drake/common/drake_assert.h b/wpimath/src/main/native/include/drake/common/drake_assert.h index 6263259c6d..51fdfad468 100644 --- a/wpimath/src/main/native/include/drake/common/drake_assert.h +++ b/wpimath/src/main/native/include/drake/common/drake_assert.h @@ -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 diff --git a/wpimath/src/main/native/include/drake/common/drake_throw.h b/wpimath/src/main/native/include/drake/common/drake_throw.h index aa992a16c7..9833d8b62a 100644 --- a/wpimath/src/main/native/include/drake/common/drake_throw.h +++ b/wpimath/src/main/native/include/drake/common/drake_throw.h @@ -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 diff --git a/wpimath/src/main/native/include/drake/common/never_destroyed.h b/wpimath/src/main/native/include/drake/common/never_destroyed.h index 2033fd0738..024b3552e2 100644 --- a/wpimath/src/main/native/include/drake/common/never_destroyed.h +++ b/wpimath/src/main/native/include/drake/common/never_destroyed.h @@ -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& GetConstantMagicNumbers() { +/// static const drake::never_destroyed> result{[]() { +/// std::vector prototype; +/// std::mt19937 random_generator; +/// for (int i = 0; i < 10; ++i) { +/// double new_value = random_generator(); +/// prototype.push_back(new_value); +/// } +/// return prototype; +/// }()}; +/// return result.access(); +/// } +/// @endcode +/// +/// Note in particular the `()` after the lambda. That causes it to be invoked. // // The above examples are repeated in the unit test; keep them in sync. template 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 0f2a2d78ec..cb0a4ee131 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 @@ -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& Q, const Eigen::Ref& 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( 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 2ee42a19cd..8631d6f3c0 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 @@ -25,10 +25,14 @@ void SolveDAREandVerify(const Eigen::Ref& 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& 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)); } diff --git a/wpimath/update_drake.py b/wpimath/update_drake.py index 296e85f278..79e9e90532 100755 --- a/wpimath/update_drake.py +++ b/wpimath/update_drake.py @@ -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"])