Update primitives and examples to use modern event API (RobotLocomoti…

…on#20114)

Overriding the event dispatching will soon be deprecated.

examples/rod2d/rod2d.cc

@@ -30,7 +30,8 @@ Rod2D<T>::Rod2D(SystemType system_type, double dt)
 
     // Discretization approach requires three position variables and
     // three velocity variables, all discrete, and periodic update.
-    this->DeclarePeriodicDiscreteUpdateNoHandler(dt);
+    this->DeclarePeriodicDiscreteUpdateEvent(dt, 0.0,
+                                             &Rod2D<T>::CalcDiscreteUpdate);
     auto state_index = this->DeclareDiscreteState(6);
     state_output_port_ = &this->DeclareStateOutputPort(
         "state_output", state_index);
@@ -481,9 +482,8 @@ void Rod2D<T>::CalcImpactProblemData(
 /// Integrates the Rod 2D example forward in time using a
 /// half-explicit discretization scheme.
 template <class T>
-void Rod2D<T>::DoCalcDiscreteVariableUpdates(
+void Rod2D<T>::CalcDiscreteUpdate(
     const systems::Context<T>& context,
-    const std::vector<const systems::DiscreteUpdateEvent<T>*>&,
     systems::DiscreteValues<T>* discrete_state) const {
   using std::sin;
   using std::cos;

examples/rod2d/rod2d.h

@@ -504,10 +504,9 @@ class Rod2D : public systems::LeafSystem<T> {
   void DoCalcTimeDerivatives(const systems::Context<T>& context,
                              systems::ContinuousState<T>* derivatives)
                                const override;
-  void DoCalcDiscreteVariableUpdates(
+  void CalcDiscreteUpdate(
       const systems::Context<T>& context,
-      const std::vector<const systems::DiscreteUpdateEvent<T>*>& events,
-      systems::DiscreteValues<T>* discrete_state) const override;
+      systems::DiscreteValues<T>* discrete_state) const;
   void SetDefaultState(const systems::Context<T>& context,
                        systems::State<T>* state) const override;
 

manipulation/schunk_wsg/schunk_wsg_trajectory_generator.cc

@@ -10,6 +10,7 @@ namespace schunk_wsg {
 using systems::BasicVector;
 using systems::Context;
 using systems::DiscreteValues;
+using systems::EventStatus;
 
 SchunkWsgTrajectoryGenerator::SchunkWsgTrajectoryGenerator(int input_size,
                                                            int position_index)
@@ -33,7 +34,10 @@ SchunkWsgTrajectoryGenerator::SchunkWsgTrajectoryGenerator(int input_size,
       SchunkWsgTrajectoryGeneratorStateVector<double>());
   // The update period below matches the polling rate from
   // drake-schunk-driver.
-  this->DeclarePeriodicDiscreteUpdateNoHandler(0.05);
+  this->DeclarePeriodicDiscreteUpdateEvent(
+      0.05, 0.0, &SchunkWsgTrajectoryGenerator::CalcDiscreteUpdate);
+  this->DeclareForcedDiscreteUpdateEvent(
+      &SchunkWsgTrajectoryGenerator::CalcDiscreteUpdate);
 }
 
 void SchunkWsgTrajectoryGenerator::OutputTarget(
@@ -59,9 +63,8 @@ void SchunkWsgTrajectoryGenerator::OutputForce(
   output->get_mutable_value() = Vector1d(traj_state->max_force());
 }
 
-void SchunkWsgTrajectoryGenerator::DoCalcDiscreteVariableUpdates(
+EventStatus SchunkWsgTrajectoryGenerator::CalcDiscreteUpdate(
     const Context<double>& context,
-    const std::vector<const systems::DiscreteUpdateEvent<double>*>&,
     DiscreteValues<double>* discrete_state) const {
   const double desired_position =
       get_desired_position_input_port().Eval(context)[0];
@@ -97,6 +100,8 @@ void SchunkWsgTrajectoryGenerator::DoCalcDiscreteVariableUpdates(
     new_traj_state->set_trajectory_start_time(
         last_traj_state->trajectory_start_time());
   }
+
+  return EventStatus::Succeeded();
 }
 
 void SchunkWsgTrajectoryGenerator::UpdateTrajectory(

manipulation/schunk_wsg/schunk_wsg_trajectory_generator.h

@@ -78,10 +78,9 @@ class SchunkWsgTrajectoryGenerator : public systems::LeafSystem<double> {
                    systems::BasicVector<double>* output) const;
 
   /// Latches the input port into the discrete state.
-  void DoCalcDiscreteVariableUpdates(
+  systems::EventStatus CalcDiscreteUpdate(
       const systems::Context<double>& context,
-      const std::vector<const systems::DiscreteUpdateEvent<double>*>& events,
-      systems::DiscreteValues<double>* discrete_state) const override;
+      systems::DiscreteValues<double>* discrete_state) const;
 
   void UpdateTrajectory(double cur_position, double target_position) const;
 

planning/trajectory_optimization/test/direct_transcription_test.cc

@@ -52,7 +52,8 @@ class CubicPolynomialSystem final : public systems::LeafSystem<T> {
         time_step_(time_step) {
     // Zero inputs, zero outputs.
     this->DeclareDiscreteState(1);  // One state variable.
-    this->DeclarePeriodicDiscreteUpdateNoHandler(time_step);
+    this->DeclarePeriodicDiscreteUpdateEvent(
+        time_step, 0.0, &CubicPolynomialSystem<T>::CalcDiscreteUpdate);
   }
 
   // Scalar-converting copy constructor.
@@ -64,10 +65,8 @@ class CubicPolynomialSystem final : public systems::LeafSystem<T> {
 
  private:
   // x[n+1] = x³[n]
-  void DoCalcDiscreteVariableUpdates(
-      const Context<T>& context,
-      const std::vector<const DiscreteUpdateEvent<T>*>&,
-      DiscreteValues<T>* discrete_state) const final {
+  void CalcDiscreteUpdate(const Context<T>& context,
+                          DiscreteValues<T>* discrete_state) const {
     using std::pow;
     (*discrete_state)[0] =
         pow(context.get_discrete_state(0).GetAtIndex(0), 3.0);
@@ -86,7 +85,8 @@ class LinearSystemWParams final : public systems::LeafSystem<T> {
     // Zero inputs, zero outputs.
     this->DeclareDiscreteState(1);                     // One state variable.
     this->DeclareNumericParameter(BasicVector<T>(1));  // One parameter.
-    this->DeclarePeriodicDiscreteUpdateNoHandler(1.0);
+    this->DeclarePeriodicDiscreteUpdateEvent(
+        1.0, 0.0, &LinearSystemWParams<T>::CalcDiscreteUpdate);
   }
 
   // Scalar-converting copy constructor.
@@ -96,10 +96,8 @@ class LinearSystemWParams final : public systems::LeafSystem<T> {
 
  private:
   // x[n+1] = p0 * x[n]
-  void DoCalcDiscreteVariableUpdates(
-      const Context<T>& context,
-      const std::vector<const DiscreteUpdateEvent<T>*>&,
-      DiscreteValues<T>* discrete_state) const final {
+  void CalcDiscreteUpdate(const Context<T>& context,
+                          DiscreteValues<T>* discrete_state) const {
     (*discrete_state)[0] = context.get_numeric_parameter(0).GetAtIndex(0) *
                            context.get_discrete_state(0).GetAtIndex(0);
   }

systems/controllers/linear_model_predictive_controller.cc

@@ -63,7 +63,16 @@ LinearModelPredictiveController<T>::LinearModelPredictiveController(
     throw std::runtime_error("R must be positive definite");
   }
 
-  this->DeclarePeriodicDiscreteUpdateNoHandler(time_period_);
+  // TODO(jwnimmer-tri) This seems like a misunderstood attempt at implementing
+  // discrete dynamics. The intent *appears* to be that SetupAndSolveQp should
+  // be run once every time_step. However, both because its result is NOT stored
+  // as state and because the output is direct-feedthrough from the input, we do
+  // not actually embody any kind of discrete dynamics. Anytime a user evaluates
+  // the output port after the input port has changed, we'll redo the QP, even
+  // when the current time is not an integer multiple of the step.
+  this->DeclarePeriodicDiscreteUpdateEvent(
+      time_period_, 0.0,
+      &LinearModelPredictiveController<T>::DoNothingButPretendItWasSomething);
 
   if (base_context_ != nullptr) {
     linear_model_ = Linearize(*model_, *base_context_);
@@ -85,6 +94,13 @@ void LinearModelPredictiveController<T>::CalcControl(
   // TODO(jadecastro) Implement the time-varying case.
 }
 
+template <typename T>
+EventStatus
+LinearModelPredictiveController<T>::DoNothingButPretendItWasSomething(
+    const Context<T>&, DiscreteValues<T>*) const {
+  return EventStatus::Succeeded();
+}
+
 template <typename T>
 VectorX<T> LinearModelPredictiveController<T>::SetupAndSolveQp(
     const Context<T>& base_context, const VectorX<T>& current_state) const {

systems/controllers/linear_model_predictive_controller.h

@@ -84,6 +84,9 @@ class LinearModelPredictiveController : public LeafSystem<T> {
  private:
   void CalcControl(const Context<T>& context, BasicVector<T>* control) const;
 
+  EventStatus DoNothingButPretendItWasSomething(const Context<T>&,
+                                                DiscreteValues<T>*) const;
+
   // Sets up a DirectTranscription problem and solves for the current control
   // input.
   VectorX<T> SetupAndSolveQp(const Context<T>& base_context,

systems/controllers/test/linear_model_predictive_controller_test.cc

@@ -95,7 +95,8 @@ class CubicPolynomialSystem final : public LeafSystem<T> {
                                   &CubicPolynomialSystem::OutputState,
                                   {this->all_state_ticket()});
     this->DeclareDiscreteState(2);
-    this->DeclarePeriodicDiscreteUpdateNoHandler(time_step);
+    this->DeclarePeriodicDiscreteUpdateEvent(
+        time_step, 0.0, &CubicPolynomialSystem<T>::CalcDiscreteUpdate);
   }
 
   template <typename U>
@@ -107,10 +108,9 @@ class CubicPolynomialSystem final : public LeafSystem<T> {
 
   // x1(k+1) = u(k)
   // x2(k+1) = -x1³(k)
-  void DoCalcDiscreteVariableUpdates(
+  void CalcDiscreteUpdate(
       const Context<T>& context,
-      const std::vector<const DiscreteUpdateEvent<T>*>&,
-      DiscreteValues<T>* next_state) const final {
+      DiscreteValues<T>* next_state) const {
     using std::pow;
     const T& x1 = context.get_discrete_state(0).get_value()[0];
     const T& u = this->get_input_port(0).Eval(context)[0];

systems/framework/test/diagram_test.cc

@@ -51,13 +51,18 @@ class EmptySystem : public LeafSystem<T> {
   void AddPeriodicDiscreteUpdate() {
     const double default_period = 1.125;
     const double default_offset = 2.25;
-    this->DeclarePeriodicDiscreteUpdateNoHandler(default_period,
-                                                 default_offset);
+    this->DeclarePeriodicDiscreteUpdateEvent(default_period, default_offset,
+                                             &EmptySystem::Noop);
   }
 
   // Adds a specific periodic discrete update.
   void AddPeriodicDiscreteUpdate(double period, double offset) {
-    this->DeclarePeriodicDiscreteUpdateNoHandler(period, offset);
+    this->DeclarePeriodicDiscreteUpdateEvent(period, offset,
+                                             &EmptySystem::Noop);
+  }
+
+  EventStatus Noop(const Context<T>&, DiscreteValues<T>*) const {
+    return EventStatus::DidNothing();
   }
 };
 

systems/framework/test/system_symbolic_inspector_test.cc

@@ -29,6 +29,7 @@ class SparseSystem : public LeafSystem<symbolic::Expression> {
 
     this->DeclareContinuousState(kSize);
     this->DeclareDiscreteState(kSize);
+    this->DeclareForcedDiscreteUpdateEvent(&SparseSystem::CalcDiscreteUpdate);
 
     this->DeclareEqualityConstraint(&SparseSystem::CalcConstraint, kSize,
                                     "equality constraint");
@@ -41,7 +42,9 @@ class SparseSystem : public LeafSystem<symbolic::Expression> {
     this->DeclareAbstractInputPort(kUseDefaultName, Value<std::string>{});
   }
 
-  ~SparseSystem() override {}
+  void set_discrete_update_is_affine(bool value) {
+    discrete_update_is_affine_ = value;
+  }
 
  private:
   // Calculation function for output port 0.
@@ -95,12 +98,9 @@ class SparseSystem : public LeafSystem<symbolic::Expression> {
     derivatives->SetFromVector(xdot);
   }
 
-  void DoCalcDiscreteVariableUpdates(
+  EventStatus CalcDiscreteUpdate(
       const systems::Context<symbolic::Expression>& context,
-      const std::vector<
-          const systems::DiscreteUpdateEvent<symbolic::Expression>*>&,
-      systems::DiscreteValues<symbolic::Expression>* discrete_state)
-      const override {
+      systems::DiscreteValues<symbolic::Expression>* discrete_state) const {
     const auto& u0 = this->get_input_port(0).Eval(context);
     const auto& u1 = this->get_input_port(1).Eval(context);
     const Vector2<symbolic::Expression> xd =
@@ -109,10 +109,16 @@ class SparseSystem : public LeafSystem<symbolic::Expression> {
     const Eigen::Matrix2d B1 = 8 * Eigen::Matrix2d::Identity();
     const Eigen::Matrix2d B2 = 9 * Eigen::Matrix2d::Identity();
     const Eigen::Vector2d f0(10.0, 11.0);
-    const Vector2<symbolic::Expression> next_xd =
+    Vector2<symbolic::Expression> next_xd =
         A * xd + B1 * u0 + B2 * u1 + f0;
+    if (!discrete_update_is_affine_) {
+      next_xd(0) += cos(u0(0));
+    }
     discrete_state->set_value(0, next_xd);
+    return EventStatus::Succeeded();
   }
+
+  bool discrete_update_is_affine_{true};
 };
 
 class SystemSymbolicInspectorTest : public ::testing::Test {
@@ -189,10 +195,17 @@ TEST_F(PendulumInspectorTest, IsTimeInvariant) {
   EXPECT_TRUE(inspector_->IsTimeInvariant());
 }
 
-TEST_F(SystemSymbolicInspectorTest, HasAffineDynamics) {
+TEST_F(SystemSymbolicInspectorTest, HasAffineDynamicsYes) {
   EXPECT_TRUE(inspector_->HasAffineDynamics());
 }
 
+GTEST_TEST(SystemSymbolicInspectorTest2, HasAffineDynamicsNo) {
+  SparseSystem other;
+  other.set_discrete_update_is_affine(false);
+  SystemSymbolicInspector other_inspector(other);
+  EXPECT_FALSE(other_inspector.HasAffineDynamics());
+}
+
 TEST_F(PendulumInspectorTest, HasAffineDynamics) {
   EXPECT_FALSE(inspector_->HasAffineDynamics());
 }

systems/framework/test_utilities/initialization_test_system.h

@@ -14,50 +14,64 @@ namespace systems {
 class InitializationTestSystem : public LeafSystem<double> {
  public:
   InitializationTestSystem() {
+    // N.B. In the code below, we call the DeclareInitializationEvent() internal
+    // API to declare events, so that we can carefully inspect the contents
+    // during unit testing. Users should NOT use this event API, but instead the
+    // DeclareInitializationPublishEvent(), etc.
+
+    // Publish event.
     PublishEvent<double> pub_event(
         TriggerType::kInitialization,
         [this](const System<double>&, const Context<double>&,
                const PublishEvent<double>& event) {
-          this->InitPublish(event);
+          EXPECT_EQ(event.get_trigger_type(), TriggerType::kInitialization);
+          this->InitPublish();
           return EventStatus::Succeeded();
         });
     DeclareInitializationEvent(pub_event);
 
+    // Discrete state and update event.
     DeclareDiscreteState(1);
-    DeclareAbstractState(Value<bool>(false));
+    DiscreteUpdateEvent<double> discrete_update_event(
+        TriggerType::kInitialization,
+        [this](const System<double>&, const Context<double>&,
+               const DiscreteUpdateEvent<double>& event,
+               DiscreteValues<double>* discrete_state) {
+          EXPECT_EQ(event.get_trigger_type(), TriggerType::kInitialization);
+          this->InitDiscreteUpdate(discrete_state);
+          return EventStatus::Succeeded();
+        });
+    DeclareInitializationEvent(discrete_update_event);
 
-    DeclareInitializationEvent(
-        DiscreteUpdateEvent<double>(TriggerType::kInitialization));
-    DeclareInitializationEvent(
-        UnrestrictedUpdateEvent<double>(TriggerType::kInitialization));
+    // Abstract state and update event.
+    DeclareAbstractState(Value<bool>(false));
+    UnrestrictedUpdateEvent<double> unrestricted_update_event(
+        TriggerType::kInitialization,
+        [this](const System<double>&, const Context<double>&,
+               const UnrestrictedUpdateEvent<double>& event,
+               State<double>* state) {
+          EXPECT_EQ(event.get_trigger_type(), TriggerType::kInitialization);
+          this->InitUnrestrictedUpdate(state);
+          return EventStatus::Succeeded();
+        });
+    DeclareInitializationEvent(unrestricted_update_event);
   }
 
   bool get_pub_init() const { return pub_init_; }
   bool get_dis_update_init() const { return dis_update_init_; }
   bool get_unres_update_init() const { return unres_update_init_; }
 
  private:
-  void InitPublish(const PublishEvent<double>& event) const {
-    EXPECT_EQ(event.get_trigger_type(), TriggerType::kInitialization);
+  void InitPublish() const {
     pub_init_ = true;
   }
 
-  void DoCalcDiscreteVariableUpdates(
-      const Context<double>&,
-      const std::vector<const DiscreteUpdateEvent<double>*>& events,
-      DiscreteValues<double>* discrete_state) const final {
-    EXPECT_EQ(events.size(), 1);
-    EXPECT_EQ(events.front()->get_trigger_type(), TriggerType::kInitialization);
+  void InitDiscreteUpdate(DiscreteValues<double>* discrete_state) const {
     dis_update_init_ = true;
     discrete_state->set_value(Vector1d(1.23));
   }
 
-  void DoCalcUnrestrictedUpdate(
-      const Context<double>&,
-      const std::vector<const UnrestrictedUpdateEvent<double>*>& events,
-      State<double>* state) const final {
-    EXPECT_EQ(events.size(), 1);
-    EXPECT_EQ(events.front()->get_trigger_type(), TriggerType::kInitialization);
+  void InitUnrestrictedUpdate(State<double>* state) const {
     unres_update_init_ = true;
     state->get_mutable_abstract_state<bool>(0) = true;
   }