From 40391132f0080732db5854a4008efbf9b7cd1c88 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Wed, 6 Sep 2023 14:49:10 -0700 Subject: [PATCH] Update primitives and examples to use modern event API (#20114) Overriding the event dispatching will soon be deprecated. --- examples/rod2d/rod2d.cc | 6 +-- examples/rod2d/rod2d.h | 5 +- .../schunk_wsg_trajectory_generator.cc | 11 ++-- .../schunk_wsg_trajectory_generator.h | 5 +- .../test/direct_transcription_test.cc | 18 +++---- .../linear_model_predictive_controller.cc | 18 ++++++- .../linear_model_predictive_controller.h | 3 ++ ...linear_model_predictive_controller_test.cc | 8 +-- systems/framework/test/diagram_test.cc | 11 ++-- .../test/system_symbolic_inspector_test.cc | 29 +++++++--- .../initialization_test_system.h | 54 ++++++++++++------- systems/primitives/discrete_derivative.cc | 10 ++-- systems/primitives/discrete_derivative.h | 6 +-- systems/primitives/symbolic_vector_system.cc | 10 ++-- systems/primitives/symbolic_vector_system.h | 5 +- systems/primitives/test/linear_system_test.cc | 18 +++---- 16 files changed, 133 insertions(+), 84 deletions(-) diff --git a/examples/rod2d/rod2d.cc b/examples/rod2d/rod2d.cc index fb87472460ec..50414c185a90 100644 --- a/examples/rod2d/rod2d.cc +++ b/examples/rod2d/rod2d.cc @@ -30,7 +30,8 @@ Rod2D::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::CalcDiscreteUpdate); auto state_index = this->DeclareDiscreteState(6); state_output_port_ = &this->DeclareStateOutputPort( "state_output", state_index); @@ -481,9 +482,8 @@ void Rod2D::CalcImpactProblemData( /// Integrates the Rod 2D example forward in time using a /// half-explicit discretization scheme. template -void Rod2D::DoCalcDiscreteVariableUpdates( +void Rod2D::CalcDiscreteUpdate( const systems::Context& context, - const std::vector*>&, systems::DiscreteValues* discrete_state) const { using std::sin; using std::cos; diff --git a/examples/rod2d/rod2d.h b/examples/rod2d/rod2d.h index cc30202cc6ba..3197555dabe7 100644 --- a/examples/rod2d/rod2d.h +++ b/examples/rod2d/rod2d.h @@ -504,10 +504,9 @@ class Rod2D : public systems::LeafSystem { void DoCalcTimeDerivatives(const systems::Context& context, systems::ContinuousState* derivatives) const override; - void DoCalcDiscreteVariableUpdates( + void CalcDiscreteUpdate( const systems::Context& context, - const std::vector*>& events, - systems::DiscreteValues* discrete_state) const override; + systems::DiscreteValues* discrete_state) const; void SetDefaultState(const systems::Context& context, systems::State* state) const override; diff --git a/manipulation/schunk_wsg/schunk_wsg_trajectory_generator.cc b/manipulation/schunk_wsg/schunk_wsg_trajectory_generator.cc index 601a27187fd2..129a1b55433a 100644 --- a/manipulation/schunk_wsg/schunk_wsg_trajectory_generator.cc +++ b/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()); // 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& context, - const std::vector*>&, DiscreteValues* 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( diff --git a/manipulation/schunk_wsg/schunk_wsg_trajectory_generator.h b/manipulation/schunk_wsg/schunk_wsg_trajectory_generator.h index d30d5fd6a9fe..6d8797b34edf 100644 --- a/manipulation/schunk_wsg/schunk_wsg_trajectory_generator.h +++ b/manipulation/schunk_wsg/schunk_wsg_trajectory_generator.h @@ -78,10 +78,9 @@ class SchunkWsgTrajectoryGenerator : public systems::LeafSystem { systems::BasicVector* output) const; /// Latches the input port into the discrete state. - void DoCalcDiscreteVariableUpdates( + systems::EventStatus CalcDiscreteUpdate( const systems::Context& context, - const std::vector*>& events, - systems::DiscreteValues* discrete_state) const override; + systems::DiscreteValues* discrete_state) const; void UpdateTrajectory(double cur_position, double target_position) const; diff --git a/planning/trajectory_optimization/test/direct_transcription_test.cc b/planning/trajectory_optimization/test/direct_transcription_test.cc index 26eb6a8b9e62..d09c27d982a1 100644 --- a/planning/trajectory_optimization/test/direct_transcription_test.cc +++ b/planning/trajectory_optimization/test/direct_transcription_test.cc @@ -52,7 +52,8 @@ class CubicPolynomialSystem final : public systems::LeafSystem { 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::CalcDiscreteUpdate); } // Scalar-converting copy constructor. @@ -64,10 +65,8 @@ class CubicPolynomialSystem final : public systems::LeafSystem { private: // x[n+1] = x³[n] - void DoCalcDiscreteVariableUpdates( - const Context& context, - const std::vector*>&, - DiscreteValues* discrete_state) const final { + void CalcDiscreteUpdate(const Context& context, + DiscreteValues* 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 { // Zero inputs, zero outputs. this->DeclareDiscreteState(1); // One state variable. this->DeclareNumericParameter(BasicVector(1)); // One parameter. - this->DeclarePeriodicDiscreteUpdateNoHandler(1.0); + this->DeclarePeriodicDiscreteUpdateEvent( + 1.0, 0.0, &LinearSystemWParams::CalcDiscreteUpdate); } // Scalar-converting copy constructor. @@ -96,10 +96,8 @@ class LinearSystemWParams final : public systems::LeafSystem { private: // x[n+1] = p0 * x[n] - void DoCalcDiscreteVariableUpdates( - const Context& context, - const std::vector*>&, - DiscreteValues* discrete_state) const final { + void CalcDiscreteUpdate(const Context& context, + DiscreteValues* discrete_state) const { (*discrete_state)[0] = context.get_numeric_parameter(0).GetAtIndex(0) * context.get_discrete_state(0).GetAtIndex(0); } diff --git a/systems/controllers/linear_model_predictive_controller.cc b/systems/controllers/linear_model_predictive_controller.cc index 131a144293c9..3fd71fed5f95 100644 --- a/systems/controllers/linear_model_predictive_controller.cc +++ b/systems/controllers/linear_model_predictive_controller.cc @@ -63,7 +63,16 @@ LinearModelPredictiveController::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::DoNothingButPretendItWasSomething); if (base_context_ != nullptr) { linear_model_ = Linearize(*model_, *base_context_); @@ -85,6 +94,13 @@ void LinearModelPredictiveController::CalcControl( // TODO(jadecastro) Implement the time-varying case. } +template +EventStatus +LinearModelPredictiveController::DoNothingButPretendItWasSomething( + const Context&, DiscreteValues*) const { + return EventStatus::Succeeded(); +} + template VectorX LinearModelPredictiveController::SetupAndSolveQp( const Context& base_context, const VectorX& current_state) const { diff --git a/systems/controllers/linear_model_predictive_controller.h b/systems/controllers/linear_model_predictive_controller.h index f7b07d36f787..720117d1c2a2 100644 --- a/systems/controllers/linear_model_predictive_controller.h +++ b/systems/controllers/linear_model_predictive_controller.h @@ -84,6 +84,9 @@ class LinearModelPredictiveController : public LeafSystem { private: void CalcControl(const Context& context, BasicVector* control) const; + EventStatus DoNothingButPretendItWasSomething(const Context&, + DiscreteValues*) const; + // Sets up a DirectTranscription problem and solves for the current control // input. VectorX SetupAndSolveQp(const Context& base_context, diff --git a/systems/controllers/test/linear_model_predictive_controller_test.cc b/systems/controllers/test/linear_model_predictive_controller_test.cc index e7c110d4bcbd..7918cfac89a7 100644 --- a/systems/controllers/test/linear_model_predictive_controller_test.cc +++ b/systems/controllers/test/linear_model_predictive_controller_test.cc @@ -95,7 +95,8 @@ class CubicPolynomialSystem final : public LeafSystem { &CubicPolynomialSystem::OutputState, {this->all_state_ticket()}); this->DeclareDiscreteState(2); - this->DeclarePeriodicDiscreteUpdateNoHandler(time_step); + this->DeclarePeriodicDiscreteUpdateEvent( + time_step, 0.0, &CubicPolynomialSystem::CalcDiscreteUpdate); } template @@ -107,10 +108,9 @@ class CubicPolynomialSystem final : public LeafSystem { // x1(k+1) = u(k) // x2(k+1) = -x1³(k) - void DoCalcDiscreteVariableUpdates( + void CalcDiscreteUpdate( const Context& context, - const std::vector*>&, - DiscreteValues* next_state) const final { + DiscreteValues* 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]; diff --git a/systems/framework/test/diagram_test.cc b/systems/framework/test/diagram_test.cc index 40d662953220..68476a7a96dd 100644 --- a/systems/framework/test/diagram_test.cc +++ b/systems/framework/test/diagram_test.cc @@ -51,13 +51,18 @@ class EmptySystem : public LeafSystem { 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&, DiscreteValues*) const { + return EventStatus::DidNothing(); } }; diff --git a/systems/framework/test/system_symbolic_inspector_test.cc b/systems/framework/test/system_symbolic_inspector_test.cc index b294f45f9ffe..0928ca521019 100644 --- a/systems/framework/test/system_symbolic_inspector_test.cc +++ b/systems/framework/test/system_symbolic_inspector_test.cc @@ -29,6 +29,7 @@ class SparseSystem : public LeafSystem { 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 { this->DeclareAbstractInputPort(kUseDefaultName, Value{}); } - ~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 { derivatives->SetFromVector(xdot); } - void DoCalcDiscreteVariableUpdates( + EventStatus CalcDiscreteUpdate( const systems::Context& context, - const std::vector< - const systems::DiscreteUpdateEvent*>&, - systems::DiscreteValues* discrete_state) - const override { + systems::DiscreteValues* 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 xd = @@ -109,10 +109,16 @@ class SparseSystem : public LeafSystem { 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 next_xd = + Vector2 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()); } diff --git a/systems/framework/test_utilities/initialization_test_system.h b/systems/framework/test_utilities/initialization_test_system.h index b5b7c2c7b14c..563bafb60240 100644 --- a/systems/framework/test_utilities/initialization_test_system.h +++ b/systems/framework/test_utilities/initialization_test_system.h @@ -14,22 +14,47 @@ namespace systems { class InitializationTestSystem : public LeafSystem { 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 pub_event( TriggerType::kInitialization, [this](const System&, const Context&, const PublishEvent& 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(false)); + DiscreteUpdateEvent discrete_update_event( + TriggerType::kInitialization, + [this](const System&, const Context&, + const DiscreteUpdateEvent& event, + DiscreteValues* discrete_state) { + EXPECT_EQ(event.get_trigger_type(), TriggerType::kInitialization); + this->InitDiscreteUpdate(discrete_state); + return EventStatus::Succeeded(); + }); + DeclareInitializationEvent(discrete_update_event); - DeclareInitializationEvent( - DiscreteUpdateEvent(TriggerType::kInitialization)); - DeclareInitializationEvent( - UnrestrictedUpdateEvent(TriggerType::kInitialization)); + // Abstract state and update event. + DeclareAbstractState(Value(false)); + UnrestrictedUpdateEvent unrestricted_update_event( + TriggerType::kInitialization, + [this](const System&, const Context&, + const UnrestrictedUpdateEvent& event, + State* 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_; } @@ -37,27 +62,16 @@ class InitializationTestSystem : public LeafSystem { bool get_unres_update_init() const { return unres_update_init_; } private: - void InitPublish(const PublishEvent& event) const { - EXPECT_EQ(event.get_trigger_type(), TriggerType::kInitialization); + void InitPublish() const { pub_init_ = true; } - void DoCalcDiscreteVariableUpdates( - const Context&, - const std::vector*>& events, - DiscreteValues* discrete_state) const final { - EXPECT_EQ(events.size(), 1); - EXPECT_EQ(events.front()->get_trigger_type(), TriggerType::kInitialization); + void InitDiscreteUpdate(DiscreteValues* discrete_state) const { dis_update_init_ = true; discrete_state->set_value(Vector1d(1.23)); } - void DoCalcUnrestrictedUpdate( - const Context&, - const std::vector*>& events, - State* state) const final { - EXPECT_EQ(events.size(), 1); - EXPECT_EQ(events.front()->get_trigger_type(), TriggerType::kInitialization); + void InitUnrestrictedUpdate(State* state) const { unres_update_init_ = true; state->get_mutable_abstract_state(0) = true; } diff --git a/systems/primitives/discrete_derivative.cc b/systems/primitives/discrete_derivative.cc index d3b5d6d92446..3df726de19f2 100644 --- a/systems/primitives/discrete_derivative.cc +++ b/systems/primitives/discrete_derivative.cc @@ -32,7 +32,10 @@ DiscreteDerivative::DiscreteDerivative(int num_inputs, double time_step, // at time == 0.0. this->DeclareDiscreteState(1); } - this->DeclarePeriodicDiscreteUpdateNoHandler(time_step_); + this->DeclarePeriodicDiscreteUpdateEvent( + time_step_, 0.0, &DiscreteDerivative::CalcDiscreteUpdate); + this->DeclareForcedDiscreteUpdateEvent( + &DiscreteDerivative::CalcDiscreteUpdate); } template @@ -57,9 +60,8 @@ void DiscreteDerivative::set_input_history( } template -void DiscreteDerivative::DoCalcDiscreteVariableUpdates( +EventStatus DiscreteDerivative::CalcDiscreteUpdate( const drake::systems::Context& context, - const std::vector*>&, drake::systems::DiscreteValues* state) const { // x₀[n+1] = u[n]. state->set_value(0, this->get_input_port().Eval(context)); @@ -71,6 +73,8 @@ void DiscreteDerivative::DoCalcDiscreteVariableUpdates( if (suppress_initial_transient_) { state->get_mutable_vector(2)[0] = context.get_discrete_state(2)[0] + 1.0; } + + return EventStatus::Succeeded(); } template diff --git a/systems/primitives/discrete_derivative.h b/systems/primitives/discrete_derivative.h index 186189f5b4d7..a6f3ea6ff891 100644 --- a/systems/primitives/discrete_derivative.h +++ b/systems/primitives/discrete_derivative.h @@ -108,10 +108,8 @@ class DiscreteDerivative final : public LeafSystem { bool suppress_initial_transient() const; private: - void DoCalcDiscreteVariableUpdates( - const Context& context, - const std::vector*>&, - DiscreteValues* discrete_state) const final; + EventStatus CalcDiscreteUpdate(const Context& context, + DiscreteValues* discrete_state) const; void CalcOutput(const Context& context, BasicVector* output_vector) const; diff --git a/systems/primitives/symbolic_vector_system.cc b/systems/primitives/symbolic_vector_system.cc index ad28e11624bd..2ff9d09e6468 100644 --- a/systems/primitives/symbolic_vector_system.cc +++ b/systems/primitives/symbolic_vector_system.cc @@ -130,7 +130,10 @@ SymbolicVectorSystem::SymbolicVectorSystem( this->DeclareContinuousState(state_vars_.size()); } else { this->DeclareDiscreteState(state_vars_.size()); - this->DeclarePeriodicDiscreteUpdateNoHandler(time_period_, 0.0); + this->DeclarePeriodicDiscreteUpdateEvent( + time_period_, 0.0, &SymbolicVectorSystem::CalcDiscreteUpdate); + this->DeclareForcedDiscreteUpdateEvent( + &SymbolicVectorSystem::CalcDiscreteUpdate); } } if (parameter_vars_.size() > 0) { @@ -351,15 +354,14 @@ void SymbolicVectorSystem::DoCalcTimeDerivatives( } template -void SymbolicVectorSystem::DoCalcDiscreteVariableUpdates( +EventStatus SymbolicVectorSystem::CalcDiscreteUpdate( const drake::systems::Context& context, - const std::vector*>& events, drake::systems::DiscreteValues* updates) const { - unused(events); DRAKE_DEMAND(time_period_ > 0.0); DRAKE_DEMAND(dynamics_.size() > 0); EvaluateWithContext(context, dynamics_, dynamics_jacobian_, dynamics_needs_inputs_, &updates->get_mutable_vector()); + return EventStatus::Succeeded(); } } // namespace systems diff --git a/systems/primitives/symbolic_vector_system.h b/systems/primitives/symbolic_vector_system.h index 7a916aae7b58..484916623de3 100644 --- a/systems/primitives/symbolic_vector_system.h +++ b/systems/primitives/symbolic_vector_system.h @@ -196,10 +196,9 @@ class SymbolicVectorSystem final : public LeafSystem { void DoCalcTimeDerivatives(const Context& context, ContinuousState* derivatives) const final; - void DoCalcDiscreteVariableUpdates( + EventStatus CalcDiscreteUpdate( const drake::systems::Context& context, - const std::vector*>& events, - drake::systems::DiscreteValues* updates) const final; + drake::systems::DiscreteValues* updates) const; const std::optional time_var_{std::nullopt}; const VectorX state_vars_{}; diff --git a/systems/primitives/test/linear_system_test.cc b/systems/primitives/test/linear_system_test.cc index 4d0d07d31d0d..39f050ad16cc 100644 --- a/systems/primitives/test/linear_system_test.cc +++ b/systems/primitives/test/linear_system_test.cc @@ -355,15 +355,10 @@ TEST_F(TestLinearizeFromAffine, DiscreteAtNonEquilibrium) { class TestNonPeriodicSystem : public LeafSystem { public: TestNonPeriodicSystem() { - this->DeclareDiscreteState(1); this->DeclarePerStepEvent(PublishEvent()); - } - void DoCalcDiscreteVariableUpdates( - const Context& context, - const std::vector*>&, - DiscreteValues* discrete_state) const override { - (*discrete_state)[0] = context.get_discrete_state(0).GetAtIndex(0) + 1; + // State with no update event is sufficient to be non-periodic. + this->DeclareDiscreteState(1); } }; @@ -803,7 +798,8 @@ class MimoSystem final : public LeafSystem { if (is_discrete) { this->DeclareDiscreteState(2); - this->DeclarePeriodicDiscreteUpdateNoHandler(0.1, 0.0); + this->DeclarePeriodicDiscreteUpdateEvent(0.1, 0.0, + &MimoSystem::CalcDiscreteUpdate); } else { this->DeclareContinuousState(2); } @@ -841,10 +837,8 @@ class MimoSystem final : public LeafSystem { derivatives->SetFromVector(A_ * x + B0_ * u0 + B1_ * u1); } - void DoCalcDiscreteVariableUpdates( - const Context& context, - const std::vector*>&, - DiscreteValues* discrete_state) const final { + void CalcDiscreteUpdate(const Context& context, + DiscreteValues* discrete_state) const { Vector1 u0 = this->get_input_port(0).Eval(context); Vector3 u1 = this->get_input_port(1).Eval(context); Vector2 x = get_state_vector(context);