diff --git a/multibody/plant/tamsi_solver.cc b/multibody/plant/tamsi_solver.cc index 0293d984e00f..6ab0c7d04d12 100644 --- a/multibody/plant/tamsi_solver.cc +++ b/multibody/plant/tamsi_solver.cc @@ -13,7 +13,7 @@ namespace drake { namespace multibody { namespace internal { template -T TalsLimiter::CalcAlpha( +std::optional TalsLimiter::CalcAlpha( const Eigen::Ref>& v, const Eigen::Ref>& dv, double cos_theta_max, double v_stiction, double relative_tolerance) { @@ -147,7 +147,13 @@ T TalsLimiter::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 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::CrossesTheStictionRegion( } template -T TalsLimiter::SolveQuadraticForTheSmallestPositiveRoot( +std::optional TalsLimiter::SolveQuadraticForTheSmallestPositiveRoot( const T& a, const T& b, const T& c) { using std::abs; using std::max; @@ -214,8 +220,13 @@ T TalsLimiter::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::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::CalcJacobian( } template -T TamsiSolver::CalcAlpha( +std::optional TamsiSolver::CalcAlpha( const Eigen::Ref>& vt, const Eigen::Ref>& Delta_vt) const { using std::min; @@ -593,11 +604,13 @@ T TamsiSolver::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::CalcAlpha( - vt_ic, dvt_ic, - cos_theta_max_, v_stiction, parameters_.relative_tolerance)); + const std::optional tals_alpha = internal::TalsLimiter::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::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 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); diff --git a/multibody/plant/tamsi_solver.h b/multibody/plant/tamsi_solver.h index 68bd556f9005..c2cf26dea2db 100644 --- a/multibody/plant/tamsi_solver.h +++ b/multibody/plant/tamsi_solver.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #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>& v, - const Eigen::Ref>& 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 CalcAlpha(const Eigen::Ref>& v, + const Eigen::Ref>& 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 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>& vt, - const Eigen::Ref>& Delta_vt) const; + // Returns nullopt in case no solution was found. + std::optional CalcAlpha( + const Eigen::Ref>& vt, + const Eigen::Ref>& Delta_vt) const; // Dimensionless regularized friction function defined as: // ms(s) = ⌈ mu * s * (2 − s), s < 1 diff --git a/multibody/plant/test/tamsi_solver_test.cc b/multibody/plant/test/tamsi_solver_test.cc index ee21b0544d28..a233d3f2c215 100644 --- a/multibody/plant/test/tamsi_solver_test.cc +++ b/multibody/plant/test/tamsi_solver_test.cc @@ -95,7 +95,7 @@ TEST_F(DirectionLimiter, ZeroVandZeroDv) { const Vector2 vt = Vector2::Zero(); const Vector2 dvt = Vector2::Zero(); const double alpha = internal::TalsLimiter::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 vt = Vector2::Zero(); const Vector2 dvt = Vector2(-0.5, 0.7) * v_stiction; const double alpha = internal::TalsLimiter::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 vt = Vector2::Zero(); const Vector2 dvt = Vector2(0.3, -0.1); const double alpha = internal::TalsLimiter::CalcAlpha( - vt, dvt, cos_min, v_stiction, tolerance); + vt, dvt, cos_min, v_stiction, tolerance).value(); const Vector2 vt_alpha_expected = dvt.normalized() * v_stiction / 2.0; const Vector2 vt_alpha = vt + alpha * dvt; EXPECT_TRUE(CompareMatrices( @@ -126,7 +126,7 @@ TEST_F(DirectionLimiter, SlidingRegiontoZero) { const Vector2 vt = Vector2(0.3, -0.1); const Vector2 dvt = -vt; const double alpha = internal::TalsLimiter::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(-0.3, 0.45) * v_stiction; const Vector2 dvt = vt_alpha_expected - vt; const double alpha = internal::TalsLimiter::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 vt = Vector2(-0.5, 0.7) * v_stiction; const Vector2 dvt = Vector2(0.9, -0.3); const double alpha = internal::TalsLimiter::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 dvt(0.3, 0.15); const double alpha = internal::TalsLimiter::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 dvt = Vector2(-0.5, 0.3) * v_stiction * tolerance; const double alpha = internal::TalsLimiter::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 dvt(-0.3, -0.15); // dvt = -3 * vt. const double alpha = internal::TalsLimiter::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 dvt = v1 - vt; const double alpha = internal::TalsLimiter::CalcAlpha( - vt, dvt, cos_min, v_stiction, tolerance); + vt, dvt, cos_min, v_stiction, tolerance).value(); // Verify the result from the limiter. const Vector2 vt_alpha = vt + alpha * dvt; @@ -263,7 +263,7 @@ TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion) { const Vector2 dvt = v1 - vt; const double alpha = internal::TalsLimiter::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 dvt = v1 - vt; const double alpha = internal::TalsLimiter::CalcAlpha( - vt, dvt, cos_min, v_stiction, tolerance); + vt, dvt, cos_min, v_stiction, tolerance).value(); const Vector2 vt_alpha = vt + alpha * dvt; @@ -322,7 +322,7 @@ TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion_VeryLargeTheta) { const Vector2 dvt = v1 - vt; const double alpha = internal::TalsLimiter::CalcAlpha( - vt, dvt, cos_min, v_stiction, tolerance); + vt, dvt, cos_min, v_stiction, tolerance).value(); const Vector2 vt_alpha = vt + alpha * dvt; @@ -355,7 +355,7 @@ TEST_F(DirectionLimiter, ChangesWithinTheSlidingRegion_SingleSolution) { ASSERT_GT(theta1, theta_max); const double alpha = internal::TalsLimiter::CalcAlpha( - vt, dvt, cos_min, v_stiction, tolerance); + vt, dvt, cos_min, v_stiction, tolerance).value(); const Vector2 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 vt = Vector2(-0.5, 0.7); + Vector2 dvt = -3.0 * Rotation(theta_max) * vt; + dvt[1] = std::numeric_limits::infinity(); + const std::optional alpha = internal::TalsLimiter::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 vt = Vector2(-0.5, 0.7); + Vector2 dvt = -3.0 * Rotation(theta_max) * vt; + vt[1] = std::numeric_limits::quiet_NaN(); + const std::optional alpha = internal::TalsLimiter::CalcAlpha( + vt, dvt, cos_min, v_stiction, tolerance); + EXPECT_EQ(alpha, std::nullopt); +} + /* Top view of the pizza saver: ^ y C