diff --git a/systems/analysis/integrator_base.h b/systems/analysis/integrator_base.h index eec4486d56a0..278a9cae268c 100644 --- a/systems/analysis/integrator_base.h +++ b/systems/analysis/integrator_base.h @@ -1940,13 +1940,17 @@ std::pair IntegratorBase::CalcAdjustedStepSize( // First, make a guess at the next step size to use based on // the supplied error norm. Watch out for NaN. Further adjustments will be // made in blocks of code that follow. - if (isnan(err) || isinf(err)) // e.g., integrand returned NaN. + if (isnan(err) || isinf(err)) { // e.g., integrand returned NaN. new_step_size = kMinShrink * step_taken; - else if (err == 0) // A "perfect" step; can happen if no dofs for example. - new_step_size = kMaxGrow * step_taken; - else // Choose best step for skating just below the desired accuracy. - new_step_size = kSafety * step_taken * - pow(get_accuracy_in_use() / err, 1.0 / err_order); + return std::make_pair(false, new_step_size); + } else { + if (err == 0) { // A "perfect" step; can happen if no dofs for example. + new_step_size = kMaxGrow * step_taken; + } else { // Choose best step for skating just below the desired accuracy. + new_step_size = kSafety * step_taken * + pow(get_accuracy_in_use() / err, 1.0 / err_order); + } + } // Error indicates that the step size can be increased. if (new_step_size > step_taken) { diff --git a/systems/analysis/test/integrator_base_test.cc b/systems/analysis/test/integrator_base_test.cc index 4ecb139daa0d..5d4e8ad353fa 100644 --- a/systems/analysis/test/integrator_base_test.cc +++ b/systems/analysis/test/integrator_base_test.cc @@ -20,15 +20,122 @@ class DummyIntegrator : public IntegratorBase { // Necessary implementations of pure virtual methods. bool supports_error_estimation() const override { return true; } int get_error_estimate_order() const override { return 1; } + std::pair CalcAdjustedStepSize(const T& err, const T& step_taken, + bool* at_minimum_step_size) const { + return IntegratorBase::CalcAdjustedStepSize(err, step_taken, + at_minimum_step_size); + } // Promote CalcStateChangeNorm() to public. using IntegratorBase::CalcStateChangeNorm; private: - // Should not be called. - bool DoStep(const T&) override { DRAKE_UNREACHABLE(); } + // We want the Step function to fail whenever the step size is greater than + // or equal to unity (see FixedStepFailureIndicatesSubstepFailure). + bool DoStep(const T& step_size) override { return (step_size < 1.0); } }; +// Tests that IntegratorBase::IntegrateNoFurtherThanTime(.) records a substep +// failure when running in fixed step mode and stepping fails. +GTEST_TEST(IntegratorBaseTest, FixedStepFailureIndicatesSubstepFailure) { + // Use the spring-mass system because we need some system (and this one will + // do as well as any other). + SpringMassSystem spring_mass(10.0, 1.0, false); + std::unique_ptr> context = spring_mass.CreateDefaultContext(); + DummyIntegrator integrator(spring_mass, context.get()); + + // Set the integrator to fixed step mode. + integrator.set_fixed_step_mode(true); + + // Verify the statistics are clear before integrating. + EXPECT_EQ(integrator.get_num_step_shrinkages_from_substep_failures(), 0); + EXPECT_EQ(integrator.get_num_substep_failures(), 0); + + // Call the integration function. + const double arbitrary_time = 1.0; + integrator.Initialize(); + integrator.IntegrateNoFurtherThanTime(arbitrary_time, arbitrary_time, + arbitrary_time); + + // Verify the step statistics have been updated. We expect DoStep() to be + // called just twice. + EXPECT_EQ(integrator.get_num_step_shrinkages_from_substep_failures(), 1); + EXPECT_EQ(integrator.get_num_substep_failures(), 1); +} + +// Tests that CalcAdjustedStepSize() shrinks the step size when encountering +// NaN and Inf. +GTEST_TEST(IntegratorBaseTest, CalcAdjustedStepSizeShrinksOnNaNAndInf) { + // We expect the shrinkage to be *at least* a factor of two. + const double kShrink = 0.5; + + // Various "errors" that will be passed into CalcAdjustedStepSize. + const double zero_error = 0.0; + const double nan_error = std::numeric_limits::quiet_NaN(); + const double inf_error = std::numeric_limits::infinity(); + + // Arbitrary step size taken. + const double step_taken = 1.0; + + // The two possible values that the at_minimum_step_size input/output + // parameter can take on entry. + bool at_minimum_step_size_true_on_entry = true; + bool at_minimum_step_size_false_on_entry = false; + + // Use the spring-mass system (system and context will be unused for this + // test). + SpringMassSystem spring_mass(10.0, 1.0, false); + std::unique_ptr> context = spring_mass.CreateDefaultContext(); + DummyIntegrator integrator(spring_mass, context.get()); + + // Verify that there is no shrinkage for zero error. + std::pair result; + result = integrator.CalcAdjustedStepSize(zero_error, step_taken, + &at_minimum_step_size_true_on_entry); + EXPECT_EQ(result.first, true); + EXPECT_GE(result.second, step_taken); + result = integrator.CalcAdjustedStepSize( + zero_error, step_taken, &at_minimum_step_size_false_on_entry); + EXPECT_EQ(result.first, true); + EXPECT_GE(result.second, step_taken); + + // Neither should be at the minimum step size. + EXPECT_EQ(at_minimum_step_size_true_on_entry, false); + EXPECT_EQ(at_minimum_step_size_false_on_entry, false); + + // Reset the minimum step size Booleans. + at_minimum_step_size_true_on_entry = true; + at_minimum_step_size_false_on_entry = false; + + // Verify shrinkage for NaN error. + result = integrator.CalcAdjustedStepSize(nan_error, step_taken, + &at_minimum_step_size_true_on_entry); + EXPECT_EQ(result.first, false); + EXPECT_LT(result.second, kShrink * step_taken); + result = integrator.CalcAdjustedStepSize( + nan_error, step_taken, &at_minimum_step_size_false_on_entry); + EXPECT_EQ(result.first, false); + EXPECT_LT(result.second, kShrink * step_taken); + + // Minimum step size should be unchanged. + EXPECT_EQ(at_minimum_step_size_true_on_entry, true); + EXPECT_EQ(at_minimum_step_size_false_on_entry, false); + + // Verify shrinkage for Inf error. + result = integrator.CalcAdjustedStepSize(inf_error, step_taken, + &at_minimum_step_size_true_on_entry); + EXPECT_EQ(result.first, false); + EXPECT_LT(result.second, kShrink * step_taken); + result = integrator.CalcAdjustedStepSize( + inf_error, step_taken, &at_minimum_step_size_false_on_entry); + EXPECT_EQ(result.first, false); + EXPECT_LT(result.second, kShrink * step_taken); + + // Minimum step size should be unchanged. + EXPECT_EQ(at_minimum_step_size_true_on_entry, true); + EXPECT_EQ(at_minimum_step_size_false_on_entry, false); +} + // Tests that CalcStateChangeNorm() propagates NaNs in state. GTEST_TEST(IntegratorBaseTest, DoubleStateChangeNormPropagatesNaN) { // We need a system with q, v, and z variables. Constants and absence of