[multibody] Improve error message for !(Delta > 0) (#19407)

multibody/plant/tamsi_solver.cc

@@ -13,7 +13,7 @@ namespace drake {
 namespace multibody {
 namespace internal {
 template <typename T>
-T TalsLimiter<T>::CalcAlpha(
+std::optional<T> TalsLimiter<T>::CalcAlpha(
     const Eigen::Ref<const Vector2<T>>& v,
     const Eigen::Ref<const Vector2<T>>& dv,
     double cos_theta_max, double v_stiction, double relative_tolerance) {
@@ -147,7 +147,13 @@ T TalsLimiter<T>::CalcAlpha(
       // quadratic equation (Δ = b² - 4ac) must be positive.
       // We use a very specialized quadratic solver for this case where we know
       // there must exist a positive (i.e. real) root.
-      alpha = SolveQuadraticForTheSmallestPositiveRoot(a, b, c);
+      // However, in case of a diverging simulation it might not hold true.
+      std::optional<T> maybe_alpha =
+          SolveQuadraticForTheSmallestPositiveRoot(a, b, c);
+      if (!maybe_alpha.has_value()) {
+        return std::nullopt;
+      }
+      alpha = *maybe_alpha;
 
       // The geometry of the problem tells us that α ≤ 1.0
       DRAKE_ASSERT(alpha <= 1.0);
@@ -195,7 +201,7 @@ bool TalsLimiter<T>::CrossesTheStictionRegion(
 }
 
 template <typename T>
-T TalsLimiter<T>::SolveQuadraticForTheSmallestPositiveRoot(
+std::optional<T> TalsLimiter<T>::SolveQuadraticForTheSmallestPositiveRoot(
     const T& a, const T& b, const T& c) {
   using std::abs;
   using std::max;
@@ -214,8 +220,13 @@ T TalsLimiter<T>::SolveQuadraticForTheSmallestPositiveRoot(
   } else {
     // The determinant, Δ = b² - 4ac, of the quadratic equation.
     const T Delta = b * b - 4 * a * c;  // Uppercase, as in Δ.
-    // Geometry tell us that a real solution does exist i.e. Delta > 0.
-    DRAKE_THROW_UNLESS(Delta > 0);
+    // Geometry tell us that a real solution does exist, i.e., Delta > 0.
+    // However, in case of a diverging simulation it might not hold true.
+    if constexpr (scalar_predicate<T>::is_bool) {
+      if (!(Delta > 0)) {
+        return std::nullopt;
+      }
+    }
     const T sqrt_Delta = sqrt(Delta);
 
     // To avoid loss of significance, when 4ac is relatively small compared
@@ -583,7 +594,7 @@ void TamsiSolver<T>::CalcJacobian(
 }
 
 template <typename T>
-T TamsiSolver<T>::CalcAlpha(
+std::optional<T> TamsiSolver<T>::CalcAlpha(
     const Eigen::Ref<const VectorX<T>>& vt,
     const Eigen::Ref<const VectorX<T>>& Delta_vt) const {
   using std::min;
@@ -593,11 +604,13 @@ T TamsiSolver<T>::CalcAlpha(
     const int ik = 2 * ic;  // Index ik scans contact vector quantities.
     auto vt_ic = vt.template segment<2>(ik);
     const auto dvt_ic = Delta_vt.template segment<2>(ik);
-    alpha = min(
-        alpha,
-        internal::TalsLimiter<T>::CalcAlpha(
-            vt_ic, dvt_ic,
-            cos_theta_max_, v_stiction, parameters_.relative_tolerance));
+    const std::optional<T> tals_alpha = internal::TalsLimiter<T>::CalcAlpha(
+        vt_ic, dvt_ic, cos_theta_max_, v_stiction,
+        parameters_.relative_tolerance);
+    if (!tals_alpha.has_value()) {
+      return std::nullopt;
+    }
+    alpha = min(alpha, *tals_alpha);
   }
   DRAKE_DEMAND(0 < alpha && alpha <= 1.0);
   return alpha;
@@ -747,10 +760,13 @@ TamsiSolverResult TamsiSolver<T>::SolveWithGuess(
     vt_error = ExtractDoubleOrThrow(Delta_vt.norm());
 
     // Limit the angle change between vₜᵏ⁺¹ and vₜᵏ for all contact points.
-    T alpha = CalcAlpha(vt, Delta_vt);
+    std::optional<T> alpha = CalcAlpha(vt, Delta_vt);
+    if (!alpha.has_value()) {
+      return TamsiSolverResult::kAlphaSolverFailed;
+    }
 
     // Update generalized velocity vector.
-    v = v + alpha * Delta_v;
+    v = v + alpha.value() * Delta_v;
 
     // Save iteration statistics.
     statistics_.Update(vt_error);

multibody/plant/tamsi_solver.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <memory>
+#include <optional>
 #include <vector>
 
 #include "drake/common/default_scalars.h"
@@ -132,11 +133,12 @@ struct TalsLimiter {
   // (see class's documentation) and to detect values close to
   // zero, ‖vₜ‖ < εᵥ. A value close to one could cause the solver to miss
   // transitions from/to stiction.
-  // @retval α the limit in [0, 1] so that vₜᵏ⁺¹ = vₜᵏ + αΔvₜᵏ.
-  static T CalcAlpha(const Eigen::Ref<const Vector2<T>>& v,
-                     const Eigen::Ref<const Vector2<T>>& dv,
-                     double cos_theta_max, double v_stiction,
-                     double relative_tolerance);
+  // @retval α the limit in [0, 1] so that vₜᵏ⁺¹ = vₜᵏ + αΔvₜᵏ, or nullopt in
+  // case no solution was found.
+  static std::optional<T> CalcAlpha(const Eigen::Ref<const Vector2<T>>& v,
+                                    const Eigen::Ref<const Vector2<T>>& dv,
+                                    double cos_theta_max, double v_stiction,
+                                    double relative_tolerance);
 
   // Helper method for detecting when the line connecting v with v1 = v + dv
   // crosses the stiction region, a circle of radius `v_stiction`.
@@ -155,8 +157,9 @@ struct TalsLimiter {
 
   // Helper method to solve the quadratic equation aα² + bα + c = 0 for the
   // very particular case we know we have real roots (Δ = b² - 4ac > 0) and we
-  // are interested in the smallest positive root.
-  static T SolveQuadraticForTheSmallestPositiveRoot(
+  // are interested in the smallest positive root. Returns nullopt when Δ is not
+  // positive.
+  static std::optional<T> SolveQuadraticForTheSmallestPositiveRoot(
       const T& a, const T& b, const T& c);
 };
 }  // namespace internal
@@ -173,7 +176,10 @@ enum class TamsiSolverResult {
   /// The linear solver used within the Newton-Raphson loop failed.
   /// This might be caused by a divergent iteration that led to an invalid
   /// Jacobian matrix.
-  kLinearSolverFailed = 2
+  kLinearSolverFailed = 2,
+
+  /// Could not solve for the α coefficient for per-iteration angle change.
+  kAlphaSolverFailed = 3,
 };
 
 /// These are the parameters controlling the iteration process of the
@@ -1157,8 +1163,10 @@ class TamsiSolver {
   // We'll do so by computing a coefficient 0 < α ≤ 1 so that if the
   // generalized velocities are updated as vᵏ⁺¹ = vᵏ + α Δvᵏ then θ ≤ θₘₐₓ
   // for all contact points.
-  T CalcAlpha(const Eigen::Ref<const VectorX<T>>& vt,
-              const Eigen::Ref<const VectorX<T>>& Delta_vt) const;
+  // Returns nullopt in case no solution was found.
+  std::optional<T> CalcAlpha(
+      const Eigen::Ref<const VectorX<T>>& vt,
+      const Eigen::Ref<const VectorX<T>>& Delta_vt) const;
 
   // Dimensionless regularized friction function defined as:
   // ms(s) = ⌈ mu * s * (2 − s),  s  < 1

multibody/plant/test/tamsi_solver_test.cc

@@ -95,7 +95,7 @@ TEST_F(DirectionLimiter, ZeroVandZeroDv) {
   const Vector2<double> vt = Vector2<double>::Zero();
   const Vector2<double> dvt = Vector2<double>::Zero();
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
   EXPECT_NEAR(alpha, 1.0, kTolerance);
 }
 
@@ -105,7 +105,7 @@ TEST_F(DirectionLimiter, ZeroVtoWithinStictionRegion) {
   const Vector2<double> vt = Vector2<double>::Zero();
   const Vector2<double> dvt = Vector2<double>(-0.5, 0.7) * v_stiction;
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
   EXPECT_NEAR(alpha, 1.0, kTolerance);
 }
 
@@ -114,7 +114,7 @@ TEST_F(DirectionLimiter, ZeroVtoSlidingRegion) {
   const Vector2<double> vt = Vector2<double>::Zero();
   const Vector2<double> dvt = Vector2<double>(0.3, -0.1);
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
   const Vector2<double> vt_alpha_expected = dvt.normalized() * v_stiction / 2.0;
   const Vector2<double> vt_alpha = vt + alpha * dvt;
   EXPECT_TRUE(CompareMatrices(
@@ -126,7 +126,7 @@ TEST_F(DirectionLimiter, SlidingRegiontoZero) {
   const Vector2<double> vt = Vector2<double>(0.3, -0.1);
   const Vector2<double> dvt = -vt;
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
   // TalsLimiter does not allow changes from outside the stiction region
   // (where friction is constant) to exactly zero velocity, since this
   // would imply leaving the solver in a state where gradients are negligible
@@ -148,7 +148,7 @@ TEST_F(DirectionLimiter, SlidingRegionToStictionRegion) {
       Vector2<double>(-0.3, 0.45) * v_stiction;
   const Vector2<double> dvt = vt_alpha_expected - vt;
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
   EXPECT_NEAR(alpha, 1.0, kTolerance);
 }
 
@@ -159,7 +159,7 @@ TEST_F(DirectionLimiter, WithinStictionRegionToSlidingRegion) {
   const Vector2<double> vt = Vector2<double>(-0.5, 0.7) * v_stiction;
   const Vector2<double> dvt = Vector2<double>(0.9, -0.3);
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
   EXPECT_NEAR(alpha, 1.0, kTolerance);
 }
 
@@ -171,7 +171,7 @@ TEST_F(DirectionLimiter, StictionToSliding) {
   const Vector2<double> dvt(0.3, 0.15);
 
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
 
   // For this case TalsLimiter neglects the very small initial vt
   // (since we always have tolerance << 1.0) so that:
@@ -188,7 +188,7 @@ TEST_F(DirectionLimiter, VerySmallDeltaV) {
   const Vector2<double> dvt =
       Vector2<double>(-0.5, 0.3) * v_stiction * tolerance;
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
   EXPECT_NEAR(alpha, 1.0, kTolerance);
 }
 
@@ -200,7 +200,7 @@ TEST_F(DirectionLimiter, StraightCrossThroughZero) {
   const Vector2<double> dvt(-0.3, -0.15);  // dvt = -3 * vt.
 
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
 
   // Since the change crosses zero exactly, we expect
   // v_alpha = v + alpha * dv = v/‖v‖⋅vₛ/2.
@@ -238,7 +238,7 @@ TEST_F(DirectionLimiter, CrossStictionRegionFromTheOutside) {
   const Vector2<double> dvt = v1 - vt;
 
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
 
   // Verify the result from the limiter.
   const Vector2<double> vt_alpha = vt + alpha * dvt;
@@ -263,7 +263,7 @@ TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion) {
   const Vector2<double> dvt = v1 - vt;
 
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
 
   EXPECT_NEAR(alpha, 1.0, kTolerance);
 }
@@ -289,7 +289,7 @@ TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion_LargeTheta) {
   const Vector2<double> dvt = v1 - vt;
 
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
 
   const Vector2<double> vt_alpha = vt + alpha * dvt;
 
@@ -322,7 +322,7 @@ TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion_VeryLargeTheta) {
   const Vector2<double> dvt = v1 - vt;
 
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
 
   const Vector2<double> vt_alpha = vt + alpha * dvt;
 
@@ -355,7 +355,7 @@ TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion_SingleSolution) {
   ASSERT_GT(theta1, theta_max);
 
   const double alpha = internal::TalsLimiter<double>::CalcAlpha(
-      vt, dvt, cos_min, v_stiction, tolerance);
+      vt, dvt, cos_min, v_stiction, tolerance).value();
 
   const Vector2<double> vt_alpha = vt + alpha * dvt;
 
@@ -367,6 +367,28 @@ TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion_SingleSolution) {
   EXPECT_NEAR(theta, theta_max, kTolerance);
 }
 
+TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion_QuadraticNonPositive1) {
+  // Use the same input as SingleSolution, but corrupt one value to be infinity.
+  // That should trigger the nullopt code paths.
+  Vector2<double> vt = Vector2<double>(-0.5, 0.7);
+  Vector2<double> dvt = -3.0 * Rotation(theta_max) * vt;
+  dvt[1] = std::numeric_limits<double>::infinity();
+  const std::optional<double> alpha = internal::TalsLimiter<double>::CalcAlpha(
+      vt, dvt, cos_min, v_stiction, tolerance);
+  EXPECT_EQ(alpha, std::nullopt);
+}
+
+TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion_QuadraticNonPositive2) {
+  // Use the same input as SingleSolution, but corrupt one value to be NaN.
+  // That should also trigger the nullopt code paths.
+  Vector2<double> vt = Vector2<double>(-0.5, 0.7);
+  Vector2<double> dvt = -3.0 * Rotation(theta_max) * vt;
+  vt[1] = std::numeric_limits<double>::quiet_NaN();
+  const std::optional<double> alpha = internal::TalsLimiter<double>::CalcAlpha(
+      vt, dvt, cos_min, v_stiction, tolerance);
+  EXPECT_EQ(alpha, std::nullopt);
+}
+
 /* Top view of the pizza saver:
 
   ^ y               C