Make sure DoCalcNextUpdateTime() timed trigger is accompanied by an E…

…vent object. (RobotLocomotion#14663)

* Make sure overridden DoCalcNextUpdateTime() is properly implemented.

systems/analysis/integrator_base.h

@@ -126,7 +126,7 @@ class IntegratorBase {
   DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IntegratorBase)
 
   /**
-   Status returned by StepOnceAtMost().
+   Status returned by IntegrateNoFurtherThanTime().
    When a step is successful, it will return an indication of what caused it
    to stop where it did. When unsuccessful it will throw an exception so you
    won't see any return value. When return of control is due ONLY to reaching
@@ -552,8 +552,8 @@ class IntegratorBase {
    to make the values mutable without permitting changing the size of
    the vector. Requires re-initializing the integrator after calling
    this method; if Initialize() is not called afterward, an exception will be
-   thrown when attempting to call StepOnceAtMost(). If the caller sets
-   one of the entries to a negative value, an exception will be thrown
+   thrown when attempting to call IntegrateNoFurtherThanTime(). If the caller
+   sets one of the entries to a negative value, an exception will be thrown
    when the integrator is initialized.
    */
   Eigen::VectorBlock<Eigen::VectorXd>
@@ -578,8 +578,8 @@ class IntegratorBase {
    to make the values mutable without permitting changing the size of
    the vector. Requires re-initializing the integrator after calling this
    method. If Initialize() is not called afterward, an exception will be
-   thrown when attempting to call StepOnceAtMost(). If the caller sets
-   one of the entries to a negative value, an exception will be thrown
+   thrown when attempting to call IntegrateNoFurtherThanTime(). If the caller
+   sets one of the entries to a negative value, an exception will be thrown
    when the integrator is initialized.
    */
   Eigen::VectorBlock<Eigen::VectorXd> get_mutable_misc_state_weight_vector() {
@@ -886,8 +886,8 @@ class IntegratorBase {
    An integrator must be initialized before being used. The pointer to the
    context must be set before Initialize() is called (or an std::logic_error
    will be thrown). If Initialize() is not called, an exception will be
-   thrown when attempting to call StepOnceAtMost(). To reinitialize the
-   integrator, Reset() should be called followed by Initialize().
+   thrown when attempting to call IntegrateNoFurtherThanTime(). To reinitialize
+   the integrator, Reset() should be called followed by Initialize().
    @throws std::logic_error If the context has not been set or a user-set
            parameter has been set illogically (i.e., one of the
            weighting matrix coefficients is set to a negative value- this
@@ -994,9 +994,9 @@ class IntegratorBase {
                             t_final is in the past.
    @sa IntegrateNoFurtherThanTime(), which is designed to be operated by
        Simulator and accounts for publishing and state reinitialization.
-   @sa IntegrateWithSingleStepToTime(), which is also designed to be operated
-       *outside of* Simulator, but throws an exception if the integrator
-       cannot advance time to `t_final` in a single step.
+   @sa IntegrateWithSingleFixedStepToTime(), which is also designed to be
+       operated *outside of* Simulator, but throws an exception if the
+       integrator cannot advance time to `t_final` in a single step.
 
    This method at a glance:
    - For integrating ODEs/DAEs not using Simulator

systems/analysis/simulator.cc

@@ -102,14 +102,14 @@ SimulatorStatus Simulator<T>::Initialize(const InitializeParams& params) {
   context_->PerturbTime(slightly_before_current_time, current_time);
 
   // Get the next timed event.
-  const T next_timed_event_time =
+  const T time_of_next_timed_event =
       system_.CalcNextUpdateTime(*context_, timed_events_.get());
 
   // Reset the context time.
   context_->SetTime(current_time);
 
   // Indicate a timed event is to be handled, if appropriate.
-  if (next_timed_event_time == current_time) {
+  if (time_of_next_timed_event == current_time) {
     time_or_witness_triggered_ = kTimeTriggered;
   } else {
     time_or_witness_triggered_ = kNothingTriggered;
@@ -240,29 +240,39 @@ SimulatorStatus Simulator<T>::AdvanceTo(const T& boundary_time) {
     // Do restricted (discrete variable) updates next.
     HandleDiscreteUpdate(merged_events->get_discrete_update_events());
 
-    // How far can we go before we have to handle timed events?
-    const T next_timed_event_time =
+    // How far can we go before we have to handle timed events? This can return
+    // infinity, meaning we don't see any timed events coming. When an earlier
+    // event trigger time is returned, at least one Event object must be
+    // returned. Note that if the returned time is the current time, we handle
+    // the Events and then restart at the same time, possibly discovering more
+    // events.
+    const T time_of_next_timed_event =
         system_.CalcNextUpdateTime(*context_, timed_events_.get());
-    DRAKE_DEMAND(next_timed_event_time >= step_start_time);
+    DRAKE_DEMAND(time_of_next_timed_event >= step_start_time);
+
+    using std::isfinite;
+    DRAKE_DEMAND(!isfinite(time_of_next_timed_event) ||
+                 timed_events_->HasEvents());
 
     // Determine whether the set of events requested by the System at
-    // next_timed_event_time includes an Update action, a Publish action, or
+    // time_of_next_timed_event includes an Update action, a Publish action, or
     // both.
     T next_update_time = std::numeric_limits<double>::infinity();
     T next_publish_time = std::numeric_limits<double>::infinity();
     if (timed_events_->HasDiscreteUpdateEvents() ||
         timed_events_->HasUnrestrictedUpdateEvents()) {
-      next_update_time = next_timed_event_time;
+      next_update_time = time_of_next_timed_event;
     }
     if (timed_events_->HasPublishEvents()) {
-      next_publish_time = next_timed_event_time;
+      next_publish_time = time_of_next_timed_event;
     }
 
-    // Integrate the continuous state forward in time.
+    // Integrate the continuous state forward in time. Note that if
+    // time_of_next_timed_event is the current time, this will return
+    // immediately without time having advanced. That still counts as a step.
     time_or_witness_triggered_ = IntegrateContinuousState(
         next_publish_time,
         next_update_time,
-        next_timed_event_time,
         boundary_time,
         witnessed_events_.get());
 
@@ -541,25 +551,25 @@ void Simulator<T>::RedetermineActiveWitnessFunctionsIfNecessary() {
 }
 
 // Integrates the continuous state forward in time while also locating
-// the first zero of any triggered witness functions.
+// the first zero of any triggered witness functions. Any of these times may
+// be set to infinity to indicate that nothing is scheduled.
+//
 // @param next_publish_time the time at which the next publish event occurs.
 // @param next_update_time the time at which the next update event occurs.
-// @param time_of_next_event the time at which the next timed event occurs.
 // @param boundary_time the maximum time to advance to.
-// @param events a non-null collection of events, which the method will clear
-//        on entry.
-// @returns the event triggers that terminated integration.
+// @param witnessed_events a non-null collection of events, which the method
+//     will clear on entry.
+// @returns the kind of event triggers that terminated integration.
 template <class T>
 typename Simulator<T>::TimeOrWitnessTriggered
 Simulator<T>::IntegrateContinuousState(
     const T& next_publish_time, const T& next_update_time,
-    const T&, const T& boundary_time,
-    CompositeEventCollection<T>* events) {
+    const T& boundary_time, CompositeEventCollection<T>* witnessed_events) {
   using std::abs;
 
   // Clear the composite event collection.
-  DRAKE_ASSERT(events);
-  events->Clear();
+  DRAKE_ASSERT(witnessed_events);
+  witnessed_events->Clear();
 
   // Save the time and current state.
   const Context<T>& context = get_context();
@@ -575,7 +585,7 @@ Simulator<T>::IntegrateContinuousState(
 
   // Attempt to integrate. Updates and boundary times are consciously
   // distinguished between. See internal documentation for
-  // IntegratorBase::StepOnceAtMost() for more information.
+  // IntegratorBase::IntegrateNoFurtherThanTime() for more information.
   typename IntegratorBase<T>::StepResult result =
       integrator_->IntegrateNoFurtherThanTime(
           next_publish_time, next_update_time, boundary_time);
@@ -618,7 +628,8 @@ Simulator<T>::IntegrateContinuousState(
         event->set_trigger_type(TriggerType::kWitness);
         event->set_event_data(std::make_unique<WitnessTriggeredEventData<T>>());
       }
-      PopulateEventDataForTriggeredWitness(t0, tf, fn, event.get(), events);
+      PopulateEventDataForTriggeredWitness(t0, tf, fn, event.get(),
+                                           witnessed_events);
     }
 
     // When successful, the isolation process produces a vector of witnesses
@@ -658,8 +669,8 @@ Simulator<T>::IntegrateContinuousState(
 
   // No witness function triggered; handle integration as usual.
   // Updates and boundary times are consciously distinguished between. See
-  // internal documentation for IntegratorBase::StepOnceAtMost() for more
-  // information.
+  // internal documentation for IntegratorBase::IntegrateNoFurtherThanTime() for
+  // more information.
   switch (result) {
     case IntegratorBase<T>::kReachedUpdateTime:
     case IntegratorBase<T>::kReachedPublishTime:

systems/analysis/simulator.h

@@ -729,11 +729,10 @@ class Simulator {
   }
 
   TimeOrWitnessTriggered IntegrateContinuousState(
-      const T& next_publish_dt,
-      const T& next_update_dt,
-      const T& time_of_next_timed_event,
-      const T& boundary_dt,
-      CompositeEventCollection<T>* events);
+      const T& next_publish_time,
+      const T& next_update_time,
+      const T& boundary_time,
+      CompositeEventCollection<T>* witnessed_events);
 
   // Private methods related to witness functions.
   void IsolateWitnessTriggers(

systems/framework/system.cc

@@ -338,8 +338,28 @@ T System<T>::CalcNextUpdateTime(const Context<T>& context,
   events->Clear();
   T time{NAN};
   DoCalcNextUpdateTime(context, events, &time);
-  using std::isnan;
-  DRAKE_ASSERT(!isnan(time));
+  using std::isnan, std::isfinite;
+
+  if (isnan(time)) {
+    throw std::logic_error(
+        fmt::format("System::CalcNextUpdateTime(): {} system '{}' overrode "
+                    "DoCalcNextUpdateTime() but at time={} it returned with no "
+                    "update time set (or the update time was set to NaN). "
+                    "Return infinity to indicate no next update time.",
+                    this->GetSystemType(), this->GetSystemPathname(),
+                    ExtractDoubleOrThrow(context.get_time())));
+  }
+
+  if (isfinite(time) && !events->HasEvents()) {
+    throw std::logic_error(fmt::format(
+        "System::CalcNextUpdateTime(): {} system '{}' overrode "
+        "DoCalcNextUpdateTime() but at time={} it returned update "
+        "time {} with an empty Event collection. Return infinity "
+        "to indicate no next update time; otherwise at least one "
+        "Event object must be provided even if it does nothing.",
+        this->GetSystemType(), this->GetSystemPathname(),
+        ExtractDoubleOrThrow(context.get_time()), ExtractDoubleOrThrow(time)));
+  }
 
   // If the context contains a perturbed current time, and
   // DoCalcNextUpdateTime() returned "right now" (which would be the

systems/framework/system.h

@@ -640,6 +640,10 @@ class System : public SystemBase {
   merged CompositeEventCollection will be passed to all event handling
   mechanisms.
 
+  If there is no timed event coming, the return value is Infinity. If
+  a finite update time is returned, there will be at least one Event object
+  in the returned event collection.
+
   @p events cannot be null. @p events will be cleared on entry. */
   T CalcNextUpdateTime(const Context<T>& context,
                        CompositeEventCollection<T>* events) const;
@@ -1441,6 +1445,11 @@ class System : public SystemBase {
   @p context has already been validated and @p events pointer is not
   null.
 
+  If you override this method, you _must_ set the returned @p time. Set it to
+  Infinity if there are no upcoming timed events. If you return a finite update
+  time, you _must_ put at least one Event object in the @p events collection.
+  These requirements are enforced by the public CalcNextUpdateTime() method.
+
   The default implementation returns with the next sample time being
   Infinity and no events added to @p events. */
   virtual void DoCalcNextUpdateTime(const Context<T>& context,

systems/framework/test/leaf_system_test.cc

@@ -3028,6 +3028,86 @@ GTEST_TEST(ImplicitTimeDerivatives, ResetToDefaultResidualSize) {
   EXPECT_EQ(dut.implicit_time_derivatives_residual_size(), 3);
 }
 
+// Simulator::AdvanceTo() could miss an event that was triggered by a
+// DoCalcNextUpdateTime() override that returned a finite next-trigger time but
+// no Event object to handle the event. See issues #12620 and #14644.
+//
+// PR #14663 redefined this as an error -- an Event must be returned if there
+// is a finite trigger time. That PR also replaced an assert with a real error
+// message in the case the time is returned NaN; we check that here also.
+//
+// Note that this is really just a System test, but we need a LeafSystem
+// to satisfy all the uninteresting pure virtuals.
+GTEST_TEST(SystemTest, MissedEventIssue12620) {
+  class TriggerTimeButNoEventSystem : public LeafSystem<double> {
+   public:
+    explicit TriggerTimeButNoEventSystem(double trigger_time)
+        : trigger_time_(trigger_time) {
+      this->set_name("MyTriggerSystem");
+    }
+
+   private:
+    void DoCalcNextUpdateTime(const Context<double>& context,
+                              CompositeEventCollection<double>*,
+                              double* next_update_time) const final {
+      *next_update_time = trigger_time_;
+      // Don't push anything to the EventCollection.
+    }
+
+    const double trigger_time_;
+  };
+
+  LeafCompositeEventCollection<double> events;
+
+  // First test returns NaN, which should be detected.
+  TriggerTimeButNoEventSystem nan_system(NAN);
+  auto nan_context = nan_system.AllocateContext();
+  nan_context->SetTime(0.25);
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      nan_system.CalcNextUpdateTime(*nan_context, &events), std::exception,
+      ".*CalcNextUpdateTime.*TriggerTimeButNoEventSystem.*MyTriggerSystem.*"
+      "time=0.25.*no update time.*NaN.*Return infinity.*");
+
+  // Second test returns a trigger time but no Event object.
+  TriggerTimeButNoEventSystem trigger_system(0.375);
+  auto trigger_context = trigger_system.AllocateContext();
+  trigger_context->SetTime(0.25);
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      trigger_system.CalcNextUpdateTime(*trigger_context, &events),
+      std::exception,
+      ".*CalcNextUpdateTime.*TriggerTimeButNoEventSystem.*MyTriggerSystem.*"
+      "time=0.25.*update time 0.375.*empty Event collection.*"
+      "at least one Event object must be provided.*");
+}
+
+// Check that a DoCalcNextUpdateTime() override that fails to set the update
+// time at all gets an error message. (This is detected as a returned time of
+// NaN because calling code initializes the time that way.)
+GTEST_TEST(SystemTest, ForgotToSetTheUpdateTime) {
+  class ForgotToSetTimeSystem : public LeafSystem<double> {
+   public:
+    ForgotToSetTimeSystem() { this->set_name("MyForgetfulSystem"); }
+
+   private:
+    void DoCalcNextUpdateTime(const Context<double>& context,
+                              CompositeEventCollection<double>*,
+                              double* next_update_time) const final {
+      // Oops -- forgot to set the time.
+    }
+  };
+
+  LeafCompositeEventCollection<double> events;
+
+  ForgotToSetTimeSystem forgot_system;
+  auto forgot_context = forgot_system.AllocateContext();
+  forgot_context->SetTime(0.25);
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      forgot_system.CalcNextUpdateTime(*forgot_context, &events),
+      std::exception,
+      ".*CalcNextUpdateTime.*ForgotToSetTimeSystem.*MyForgetfulSystem.*"
+      "time=0.25.*no update time.*NaN.*Return infinity.*");
+}
+
 }  // namespace
 }  // namespace systems
 }  // namespace drake

systems/framework/test/system_test.cc

@@ -1,6 +1,5 @@
 #include "drake/systems/framework/system.h"
 
-#include <cctype>
 #include <memory>
 #include <stdexcept>
 
@@ -16,7 +15,6 @@
 #include "drake/systems/framework/context.h"
 #include "drake/systems/framework/leaf_context.h"
 #include "drake/systems/framework/leaf_output_port.h"
-#include "drake/systems/framework/output_port.h"
 #include "drake/systems/framework/system_output.h"
 #include "drake/systems/framework/test_utilities/my_vector.h"
 
@@ -29,7 +27,9 @@ const int kSize = 3;
 // Note that Systems in this file are derived directly from drake::System for
 // testing purposes. User Systems should be derived only from LeafSystem which
 // handles much of the bookkeeping you'll see here, and won't need to call
-// methods that are intended only for internal use.
+// methods that are intended only for internal use. Some additional System tests
+// are found in leaf_system_test.cc in order to exploit LeafSystem to satisfy
+// the many pure virtuals in System.
 
 // A shell System to test the default implementations.
 class TestSystem : public System<double> {

systems/lcm/lcm_interface_system.cc

@@ -56,7 +56,7 @@ void LcmInterfaceSystem::OnHandleSubscriptionsError(
 // change as a direct result of this method.
 void LcmInterfaceSystem::DoCalcNextUpdateTime(
     const Context<double>& context,
-    systems::CompositeEventCollection<double>*,
+    systems::CompositeEventCollection<double>* events,
     double* time) const {
   const int timeout_millis = 0;  // Do not block.
   const int num_handled = lcm_->HandleSubscriptions(timeout_millis);
@@ -67,6 +67,10 @@ void LcmInterfaceSystem::DoCalcNextUpdateTime(
     // LcmSubscriberSystem might have had a (meaningful) update, without
     // simulation time advancing any further.
     *time = context.get_time();
+
+    // At least one Event object must be returned when time ≠ ∞.
+    PublishEvent<double> event(TriggerType::kTimed);
+    event.AddToComposite(events);
   } else {
     *time = std::numeric_limits<double>::infinity();
   }