From 30df81d73cb974d88589a4b8fa0faff1a6197a4e Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Sat, 3 Jun 2023 08:42:24 -0400 Subject: [PATCH] Implements System::ExecuteInitializationEvents() and uses it in MeshcatPoseSliders() and JointSliders() --- .../pydrake/systems/framework_py_systems.cc | 3 + bindings/pydrake/systems/test/custom_test.py | 21 ++++++ multibody/meshcat/BUILD.bazel | 1 + multibody/meshcat/joint_sliders.cc | 2 + multibody/meshcat/joint_sliders.h | 5 +- multibody/meshcat/test/joint_sliders_test.cc | 9 +++ systems/framework/BUILD.bazel | 39 ++++++----- systems/framework/system.cc | 31 +++++++++ systems/framework/system.h | 10 +++ systems/framework/test/diagram_test.cc | 66 +----------------- systems/framework/test/leaf_system_test.cc | 69 +++++-------------- systems/framework/test_utilities/BUILD.bazel | 11 +++ .../initialization_test_system.h | 69 +++++++++++++++++++ visualization/BUILD.bazel | 1 + visualization/meshcat_pose_sliders.cc | 17 +---- .../test/meshcat_pose_sliders_test.cc | 9 +++ 16 files changed, 216 insertions(+), 147 deletions(-) create mode 100644 systems/framework/test_utilities/initialization_test_system.h diff --git a/bindings/pydrake/systems/framework_py_systems.cc b/bindings/pydrake/systems/framework_py_systems.cc index 5c77cc5640fc..51b6a86c342e 100644 --- a/bindings/pydrake/systems/framework_py_systems.cc +++ b/bindings/pydrake/systems/framework_py_systems.cc @@ -410,6 +410,9 @@ struct Impl { .def("CalcForcedUnrestrictedUpdate", &System::CalcForcedUnrestrictedUpdate, py::arg("context"), py::arg("state"), doc.System.CalcForcedUnrestrictedUpdate.doc) + .def("ExecuteInitializationEvents", + &System::ExecuteInitializationEvents, py::arg("context"), + doc.System.ExecuteInitializationEvents.doc) .def("GetUniquePeriodicDiscreteUpdateAttribute", &System::GetUniquePeriodicDiscreteUpdateAttribute, doc.System.GetUniquePeriodicDiscreteUpdateAttribute.doc) diff --git a/bindings/pydrake/systems/test/custom_test.py b/bindings/pydrake/systems/test/custom_test.py index 136c8c3d09dd..52eb72fb4630 100644 --- a/bindings/pydrake/systems/test/custom_test.py +++ b/bindings/pydrake/systems/test/custom_test.py @@ -638,6 +638,27 @@ def _system_reset(self, system, context, event, state): self.assertTrue(system.called_reset) self.assertTrue(system.called_system_reset) + # Test ExecuteInitializationEvents. + system = TrivialSystem() + context = system.CreateDefaultContext() + system.ExecuteInitializationEvents(context=context) + self.assertFalse(system.called_per_step) + self.assertFalse(system.called_periodic) + self.assertTrue(system.called_initialize_publish) + self.assertTrue(system.called_initialize_discrete) + self.assertTrue(system.called_initialize_unrestricted) + self.assertFalse(system.called_periodic_publish) + self.assertFalse(system.called_periodic_discrete) + self.assertFalse(system.called_periodic_unrestricted) + self.assertFalse(system.called_per_step_publish) + self.assertFalse(system.called_per_step_discrete) + self.assertFalse(system.called_per_step_unrestricted) + self.assertFalse(system.called_getwitness) + self.assertFalse(system.called_witness) + self.assertFalse(system.called_guard) + self.assertFalse(system.called_reset) + self.assertFalse(system.called_system_reset) + def test_event_handler_returns_none(self): """Checks that a Python event handler callback function is allowed to (implicitly) return None, instead of an EventStatus. Because of all the diff --git a/multibody/meshcat/BUILD.bazel b/multibody/meshcat/BUILD.bazel index 2fa8d62e8d09..9876f7863d4d 100644 --- a/multibody/meshcat/BUILD.bazel +++ b/multibody/meshcat/BUILD.bazel @@ -104,6 +104,7 @@ drake_cc_googletest( "//geometry:meshcat_visualizer", "//geometry/test_utilities:meshcat_environment", "//multibody/parsing", + "//systems/framework/test_utilities", ], ) diff --git a/multibody/meshcat/joint_sliders.cc b/multibody/meshcat/joint_sliders.cc index d988c16b8460..ca79f9f21c4c 100644 --- a/multibody/meshcat/joint_sliders.cc +++ b/multibody/meshcat/joint_sliders.cc @@ -279,6 +279,8 @@ Eigen::VectorXd JointSliders::Run(const Diagram& diagram, using Duration = std::chrono::duration; const auto start_time = Clock::now(); + diagram.ExecuteInitializationEvents(root_context.get()); + // Set the context to the initial slider values. plant_->SetPositions(&plant_context, this->get_output_port().Eval(sliders_context)); diff --git a/multibody/meshcat/joint_sliders.h b/multibody/meshcat/joint_sliders.h index 34eba3867ece..50fc50eda650 100644 --- a/multibody/meshcat/joint_sliders.h +++ b/multibody/meshcat/joint_sliders.h @@ -120,9 +120,12 @@ class JointSliders final : public systems::LeafSystem { value of the plant Context. @pre `diagram` must be a top-level (i.e., "root") diagram. - @pre `diagram` must contain this JointSliders system. @pre `diagram` must contain the `plant` that was passed into this JointSliders system's constructor. + @pre `diagram` must contain this JointSliders system, however the output of + these sliders need not be connected (even indirectly) to any `plant` input + port. The positions of the `plant` will be updated directly using a call to + `plant.SetPositions(...)` when the slider values change. */ Eigen::VectorXd Run(const systems::Diagram& diagram, std::optional timeout = std::nullopt, diff --git a/multibody/meshcat/test/joint_sliders_test.cc b/multibody/meshcat/test/joint_sliders_test.cc index 5ef51a9beb8f..5c83298c78d7 100644 --- a/multibody/meshcat/test/joint_sliders_test.cc +++ b/multibody/meshcat/test/joint_sliders_test.cc @@ -10,6 +10,7 @@ #include "drake/multibody/parsing/parser.h" #include "drake/multibody/plant/multibody_plant.h" #include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/test_utilities/initialization_test_system.h" namespace drake { namespace multibody { @@ -282,6 +283,9 @@ TEST_F(JointSlidersTest, Run) { MeshcatVisualizer::AddToBuilder(&builder_, scene_graph_, meshcat_); auto* dut = builder_.AddSystem>(meshcat_, &plant_, initial_value); + + auto init_system = builder_.AddSystem(); + auto diagram = builder_.Build(); // Run for a while. @@ -289,6 +293,11 @@ TEST_F(JointSlidersTest, Run) { Eigen::VectorXd q = dut->Run(*diagram, timeout); EXPECT_TRUE(CompareMatrices(q, initial_value)); + // Confirm that initialization events were triggered. + EXPECT_TRUE(init_system->get_pub_init()); + EXPECT_TRUE(init_system->get_dis_update_init()); + EXPECT_TRUE(init_system->get_unres_update_init()); + // Note: the stop button is deleted on timeout, so we cannot easily check // that it was created correctly here. diff --git a/systems/framework/BUILD.bazel b/systems/framework/BUILD.bazel index e649e2e5168a..74c86f987e82 100644 --- a/systems/framework/BUILD.bazel +++ b/systems/framework/BUILD.bazel @@ -734,7 +734,8 @@ drake_cc_googletest( "//common:essential", "//common/test_utilities:expect_no_throw", "//common/test_utilities:expect_throws_message", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:my_vector", + "//systems/framework/test_utilities:pack_value", ], ) @@ -746,7 +747,8 @@ drake_cc_googletest( "//common:essential", "//common/test_utilities:expect_no_throw", "//common/test_utilities:expect_throws_message", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:my_vector", + "//systems/framework/test_utilities:pack_value", ], ) @@ -803,7 +805,9 @@ drake_cc_googletest( "//common/test_utilities:is_dynamic_castable", "//examples/pendulum:pendulum_plant", "//systems/analysis/test_utilities:stateless_system", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:initialization_test_system", + "//systems/framework/test_utilities:pack_value", + "//systems/framework/test_utilities:scalar_conversion", "//systems/primitives:adder", "//systems/primitives:constant_value_source", "//systems/primitives:constant_vector_source", @@ -822,7 +826,7 @@ drake_cc_googletest( "//common/test_utilities:eigen_matrix_compare", "//common/test_utilities:expect_throws_message", "//common/test_utilities:is_dynamic_castable", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:my_vector", ], ) @@ -863,7 +867,7 @@ drake_cc_googletest( ":leaf_context", "//common:essential", "//common/test_utilities", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:pack_value", ], ) @@ -874,7 +878,9 @@ drake_cc_googletest( "//common:essential", "//common/test_utilities", "//common/test_utilities:limit_malloc", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:initialization_test_system", + "//systems/framework/test_utilities:my_vector", + "//systems/framework/test_utilities:pack_value", ], ) @@ -889,7 +895,7 @@ drake_cc_googletest( name = "model_values_test", deps = [ ":model_values", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:my_vector", ], ) @@ -898,7 +904,7 @@ drake_cc_googletest( deps = [ ":abstract_values", "//common:essential", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:pack_value", ], ) @@ -907,7 +913,7 @@ drake_cc_googletest( deps = [ ":parameters", "//common:essential", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:pack_value", ], ) @@ -941,7 +947,8 @@ drake_cc_googletest( deps = [ ":system_output", "//common:essential", - "//systems/framework/test_utilities", + "//common/test_utilities:is_dynamic_castable", + "//systems/framework/test_utilities:my_vector", ], ) @@ -972,7 +979,7 @@ drake_cc_googletest( ":input_port", ":leaf_system", "//common/test_utilities:expect_throws_message", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:my_vector", ], ) @@ -984,7 +991,7 @@ drake_cc_googletest( "//common:essential", "//common/test_utilities:expect_no_throw", "//common/test_utilities:expect_throws_message", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:my_vector", "//systems/primitives:constant_vector_source", ], ) @@ -1011,7 +1018,7 @@ drake_cc_googletest( "//common:unused", "//common/test_utilities:expect_no_throw", "//common/test_utilities:expect_throws_message", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:my_vector", ], ) @@ -1030,7 +1037,7 @@ drake_cc_googletest( "//common:essential", "//common:unused", "//common/test_utilities:expect_throws_message", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:my_vector", ], ) @@ -1048,7 +1055,7 @@ drake_cc_googletest( name = "single_output_vector_source_test", deps = [ ":single_output_vector_source", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:scalar_conversion", ], ) @@ -1058,7 +1065,7 @@ drake_cc_googletest( ":vector_system", "//common/test_utilities:expect_no_throw", "//common/test_utilities:expect_throws_message", - "//systems/framework/test_utilities", + "//systems/framework/test_utilities:scalar_conversion", "//systems/primitives:integrator", ], ) diff --git a/systems/framework/system.cc b/systems/framework/system.cc index 2fb17abfef1f..4769d7cc69d6 100644 --- a/systems/framework/system.cc +++ b/systems/framework/system.cc @@ -478,6 +478,37 @@ void System::GetInitializationEvents( DoGetInitializationEvents(context, events); } +template +void System::ExecuteInitializationEvents(Context* context) const { + auto discrete_updates = AllocateDiscreteVariables(); + auto state = context->CloneState(); + auto init_events = AllocateCompositeEventCollection(); + + // NOTE: The execution order here must match the code in + // Simulator::Initialize(). + GetInitializationEvents(*context, init_events.get()); + // Do unrestricted updates first. + if (init_events->get_unrestricted_update_events().HasEvents()) { + CalcUnrestrictedUpdate(*context, + init_events->get_unrestricted_update_events(), + state.get()); + ApplyUnrestrictedUpdate(init_events->get_unrestricted_update_events(), + state.get(), context); + } + // Do restricted (discrete variable) updates next. + if (init_events->get_discrete_update_events().HasEvents()) { + CalcDiscreteVariableUpdate(*context, + init_events->get_discrete_update_events(), + discrete_updates.get()); + ApplyDiscreteVariableUpdate(init_events->get_discrete_update_events(), + discrete_updates.get(), context); + } + // Do any publishes last. + if (init_events->get_publish_events().HasEvents()) { + Publish(*context, init_events->get_publish_events()); + } +} + template std::optional System::GetUniquePeriodicDiscreteUpdateAttribute() const { diff --git a/systems/framework/system.h b/systems/framework/system.h index 53a0dcafd801..e30c8b4d24ad 100644 --- a/systems/framework/system.h +++ b/systems/framework/system.h @@ -727,6 +727,16 @@ class System : public SystemBase { void GetInitializationEvents(const Context& context, CompositeEventCollection* events) const; + /** This method triggers all of the initialization events returned by + GetInitializationEvents(). The method allocates temporary storage to perform + the updates, and is intended only as a convenience method for callers who do + not want to use the full Simulator workflow. + + Note that this is not fully equivalent to Simulator::Initialize() because + _only_ initialization events are handled here, while Simulator::Initialize() + also processes other events associated with time zero. */ + void ExecuteInitializationEvents(Context* context) const; + /** Determines whether there exists a unique periodic timing (offset and period) that triggers one or more discrete update events (and, if so, returns that unique periodic timing). Thus, this method can be used (1) as a test to diff --git a/systems/framework/test/diagram_test.cc b/systems/framework/test/diagram_test.cc index 510e6ae231be..878864d3b77e 100644 --- a/systems/framework/test/diagram_test.cc +++ b/systems/framework/test/diagram_test.cc @@ -15,6 +15,7 @@ #include "drake/systems/framework/fixed_input_port_value.h" #include "drake/systems/framework/leaf_system.h" #include "drake/systems/framework/output_port.h" +#include "drake/systems/framework/test_utilities/initialization_test_system.h" #include "drake/systems/framework/test_utilities/pack_value.h" #include "drake/systems/framework/test_utilities/scalar_conversion.h" #include "drake/systems/primitives/adder.h" @@ -3691,59 +3692,6 @@ GTEST_TEST(RandomContextTest, SetRandomTest) { // Tests initialization works properly for all subsystems. GTEST_TEST(InitializationTest, InitializationTest) { - // Note: this class is duplicated in leaf_system_test. - class InitializationTestSystem : public LeafSystem { - public: - InitializationTestSystem() { - PublishEvent pub_event( - TriggerType::kInitialization, - std::bind(&InitializationTestSystem::InitPublish, this, - std::placeholders::_1, std::placeholders::_2)); - DeclareInitializationEvent(pub_event); - - DeclareInitializationEvent(DiscreteUpdateEvent( - TriggerType::kInitialization)); - DeclareInitializationEvent(UnrestrictedUpdateEvent( - TriggerType::kInitialization)); - } - - 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 Context&, - const PublishEvent& event) const { - EXPECT_EQ(event.get_trigger_type(), - TriggerType::kInitialization); - pub_init_ = true; - } - - void DoCalcDiscreteVariableUpdates( - const Context&, - const std::vector*>& events, - DiscreteValues*) const final { - EXPECT_EQ(events.size(), 1); - EXPECT_EQ(events.front()->get_trigger_type(), - TriggerType::kInitialization); - dis_update_init_ = true; - } - - void DoCalcUnrestrictedUpdate( - const Context&, - const std::vector*>& events, - State*) const final { - EXPECT_EQ(events.size(), 1); - EXPECT_EQ(events.front()->get_trigger_type(), - TriggerType::kInitialization); - unres_update_init_ = true; - } - - mutable bool pub_init_{false}; - mutable bool dis_update_init_{false}; - mutable bool unres_update_init_{false}; - }; - DiagramBuilder builder; auto sys0 = builder.AddSystem(); @@ -3752,17 +3700,7 @@ GTEST_TEST(InitializationTest, InitializationTest) { auto dut = builder.Build(); auto context = dut->CreateDefaultContext(); - auto discrete_updates = dut->AllocateDiscreteVariables(); - auto state = context->CloneState(); - auto init_events = dut->AllocateCompositeEventCollection(); - dut->GetInitializationEvents(*context, init_events.get()); - - dut->Publish(*context, init_events->get_publish_events()); - dut->CalcDiscreteVariableUpdate(*context, - init_events->get_discrete_update_events(), - discrete_updates.get()); - dut->CalcUnrestrictedUpdate( - *context, init_events->get_unrestricted_update_events(), state.get()); + dut->ExecuteInitializationEvents(context.get()); EXPECT_TRUE(sys0->get_pub_init()); EXPECT_TRUE(sys0->get_dis_update_init()); diff --git a/systems/framework/test/leaf_system_test.cc b/systems/framework/test/leaf_system_test.cc index 65baa19f3ee6..d5e7ff6de996 100644 --- a/systems/framework/test/leaf_system_test.cc +++ b/systems/framework/test/leaf_system_test.cc @@ -22,6 +22,7 @@ #include "drake/systems/framework/leaf_context.h" #include "drake/systems/framework/system.h" #include "drake/systems/framework/system_output.h" +#include "drake/systems/framework/test_utilities/initialization_test_system.h" #include "drake/systems/framework/test_utilities/my_vector.h" #include "drake/systems/framework/test_utilities/pack_value.h" @@ -2812,56 +2813,9 @@ GTEST_TEST(RandomContextTest, SetRandomTest) { .all()); } -// Tests initialization works properly for a leaf system. -GTEST_TEST(InitializationTest, InitializationTest) { - class InitializationTestSystem : public LeafSystem { - public: - InitializationTestSystem() { - PublishEvent pub_event( - std::bind(&InitializationTestSystem::InitPublish, this, - std::placeholders::_1, std::placeholders::_2)); - DeclareInitializationEvent(pub_event); - - DeclareInitializationEvent(DiscreteUpdateEvent()); - DeclareInitializationEvent(UnrestrictedUpdateEvent()); - } - - 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 Context&, - const PublishEvent& event) const { - EXPECT_EQ(event.get_trigger_type(), TriggerType::kInitialization); - pub_init_ = true; - } - - void DoCalcDiscreteVariableUpdates( - const Context&, - const std::vector*>& events, - DiscreteValues*) const final { - EXPECT_EQ(events.size(), 1); - EXPECT_EQ(events.front()->get_trigger_type(), - TriggerType::kInitialization); - dis_update_init_ = true; - } - - void DoCalcUnrestrictedUpdate( - const Context&, - const std::vector*>& events, - State*) const final { - EXPECT_EQ(events.size(), 1); - EXPECT_EQ(events.front()->get_trigger_type(), - TriggerType::kInitialization); - unres_update_init_ = true; - } - - mutable bool pub_init_{false}; - mutable bool dis_update_init_{false}; - mutable bool unres_update_init_{false}; - }; - +// Tests initialization works properly for a leaf system. This flavor checks +// the event functions individually. +GTEST_TEST(InitializationTest, ManualEventProcessing) { InitializationTestSystem dut; auto context = dut.CreateDefaultContext(); auto discrete_updates = dut.AllocateDiscreteVariables(); @@ -2882,6 +2836,21 @@ GTEST_TEST(InitializationTest, InitializationTest) { EXPECT_TRUE(dut.get_unres_update_init()); } +// Tests initialization works properly for a leaf system. This flavor checks +// the built-in System function for initialization. +GTEST_TEST(InitializationTest, DefaultEventProcessing) { + InitializationTestSystem dut; + auto context = dut.CreateDefaultContext(); + + dut.ExecuteInitializationEvents(context.get()); + + EXPECT_TRUE(dut.get_pub_init()); + EXPECT_TRUE(dut.get_dis_update_init()); + EXPECT_TRUE(dut.get_unres_update_init()); + EXPECT_EQ(context->get_discrete_state_vector()[0], 1.23); + EXPECT_TRUE(context->get_abstract_state(0)); +} + // Although many of the tests above validate behavior of events when the // event dispatchers DoPublish() etc are overridden, the preferred method // for users is to provide individual handler functions for each event. There diff --git a/systems/framework/test_utilities/BUILD.bazel b/systems/framework/test_utilities/BUILD.bazel index 34b37891cc40..960fb4fe92ed 100644 --- a/systems/framework/test_utilities/BUILD.bazel +++ b/systems/framework/test_utilities/BUILD.bazel @@ -13,12 +13,23 @@ drake_cc_package_library( testonly = 1, visibility = ["//visibility:public"], deps = [ + ":initialization_test_system", ":my_vector", ":pack_value", ":scalar_conversion", ], ) +drake_cc_library( + name = "initialization_test_system", + testonly = 1, + srcs = [], + hdrs = ["initialization_test_system.h"], + deps = [ + "//systems/framework", + ], +) + drake_cc_library( name = "pack_value", testonly = 1, diff --git a/systems/framework/test_utilities/initialization_test_system.h b/systems/framework/test_utilities/initialization_test_system.h new file mode 100644 index 000000000000..ccac84b2866a --- /dev/null +++ b/systems/framework/test_utilities/initialization_test_system.h @@ -0,0 +1,69 @@ +#pragma once + +#include + +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace systems { + +/** A LeafSystem which declares all possible initialization events, and records + information (in mutable variables and in the context) about whether the events + have been processed. This can be used to confirm that initialization events + are properly handled. */ +class InitializationTestSystem : public LeafSystem { + public: + InitializationTestSystem() { + PublishEvent pub_event( + TriggerType::kInitialization, + std::bind(&InitializationTestSystem::InitPublish, this, + std::placeholders::_1, std::placeholders::_2)); + DeclareInitializationEvent(pub_event); + + DeclareDiscreteState(1); + DeclareAbstractState(Value(false)); + + DeclareInitializationEvent( + DiscreteUpdateEvent(TriggerType::kInitialization)); + DeclareInitializationEvent( + UnrestrictedUpdateEvent(TriggerType::kInitialization)); + } + + 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 Context&, + const PublishEvent& event) const { + EXPECT_EQ(event.get_trigger_type(), TriggerType::kInitialization); + 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); + 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); + unres_update_init_ = true; + state->get_mutable_abstract_state(0) = true; + } + + mutable bool pub_init_{false}; + mutable bool dis_update_init_{false}; + mutable bool unres_update_init_{false}; +}; + +} // namespace systems +} // namespace drake diff --git a/visualization/BUILD.bazel b/visualization/BUILD.bazel index b294fd90ff91..d7654d1ce0c0 100644 --- a/visualization/BUILD.bazel +++ b/visualization/BUILD.bazel @@ -87,6 +87,7 @@ drake_cc_googletest( "//geometry:meshcat_visualizer", "//geometry/test_utilities:meshcat_environment", "//systems/analysis:simulator", + "//systems/framework/test_utilities", ], ) diff --git a/visualization/meshcat_pose_sliders.cc b/visualization/meshcat_pose_sliders.cc index af239e5c272e..efadce7a9976 100644 --- a/visualization/meshcat_pose_sliders.cc +++ b/visualization/meshcat_pose_sliders.cc @@ -210,22 +210,7 @@ RigidTransformd MeshcatPoseSliders::Run( using Duration = std::chrono::duration; const auto start_time = Clock::now(); - // Handle initialization events. - auto init_events = system.AllocateCompositeEventCollection(); - system.GetInitializationEvents(system_context, init_events.get()); - if (init_events->get_publish_events().HasEvents()) { - system.Publish(system_context, init_events->get_publish_events()); - } - if (init_events->get_discrete_update_events().HasEvents()) { - system.CalcDiscreteVariableUpdate( - system_context, init_events->get_discrete_update_events(), - &root_context->get_mutable_discrete_state()); - } - if (init_events->get_unrestricted_update_events().HasEvents()) { - system.CalcUnrestrictedUpdate(system_context, - init_events->get_unrestricted_update_events(), - &root_context->get_mutable_state()); - } + system.ExecuteInitializationEvents(root_context.get()); RigidTransformd pose = nominal_pose_; diff --git a/visualization/test/meshcat_pose_sliders_test.cc b/visualization/test/meshcat_pose_sliders_test.cc index 2b6ab491a1ed..090e56628368 100644 --- a/visualization/test/meshcat_pose_sliders_test.cc +++ b/visualization/test/meshcat_pose_sliders_test.cc @@ -7,6 +7,7 @@ #include "drake/geometry/test_utilities/meshcat_environment.h" #include "drake/systems/analysis/simulator.h" #include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/test_utilities/initialization_test_system.h" namespace drake { namespace visualization { @@ -180,6 +181,9 @@ GTEST_TEST(MeshcatPoseSlidersTest, Run) { auto* dut = builder.AddSystem(meshcat, initial_pose); auto* recorder = builder.AddSystem(); builder.Connect(dut->get_output_port(), recorder->get_input_port()); + + auto init_system = builder.AddSystem(); + auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); @@ -190,6 +194,11 @@ GTEST_TEST(MeshcatPoseSlidersTest, Run) { // Check that the recorder's Publish method was called. EXPECT_TRUE(recorder->pose().IsExactlyEqualTo(initial_pose)); + // Confirm that initialization events were triggered. + EXPECT_TRUE(init_system->get_pub_init()); + EXPECT_TRUE(init_system->get_dis_update_init()); + EXPECT_TRUE(init_system->get_unres_update_init()); + // Note: the stop button is deleted on timeout, so we cannot easily check // that it was created correctly here.