[wpimath] Update drake with upstream (#3484)

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.

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

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
 

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

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