From c09900e4eb14dd0494d12f1108f660223b6b02ff Mon Sep 17 00:00:00 2001 From: Rick Poyner Date: Mon, 12 Jul 2021 19:04:20 -0400 Subject: [PATCH] SignalLogger: Remove context-per-thread hazard (breaking) Relevant to: #10228 Move the internal SignalLog member to become a cache entry. Abandon the data access methods on SignalLogger, and instead provide ergonomic lookup methods to get the SignalLog itself. Make SignalLog copyable and movable for compatibility with the cache. Provide complete python bindings for SignalLogger and SignalLog. Update numerous tests and examples. This patch is breaking; it removes existing (if problematic) APIs without deprecation. --- bindings/pydrake/systems/primitives_py.cc | 67 +++++++++++++++---- bindings/pydrake/systems/pyplot_visualizer.py | 6 +- .../pydrake/systems/test/primitives_test.py | 16 +++-- .../systems/test/pyplot_visualizer_test.py | 3 +- examples/acrobot/run_lqr_w_estimator.cc | 15 +++-- examples/acrobot/spong_sim.cc | 2 +- examples/acrobot/spong_sim.py | 3 +- examples/fibonacci/run_fibonacci.cc | 7 +- .../fibonacci_difference_equation_test.cc | 3 +- .../end_effector_teleop_sliders.py | 6 +- examples/manipulation_station/joint_teleop.py | 6 +- examples/van_der_pol/van_der_pol.cc | 2 +- systems/analysis/test/simulator_test.cc | 19 +++--- systems/primitives/signal_log.h | 13 ++-- systems/primitives/signal_logger.cc | 52 +++++++++++++- systems/primitives/signal_logger.h | 49 ++++++-------- .../test/discrete_derivative_test.cc | 19 +++--- systems/primitives/test/random_source_test.cc | 14 ++-- systems/primitives/test/signal_logger_test.cc | 33 +++++---- systems/sensors/test/beam_model_test.cc | 2 +- tutorials/dynamical_systems.ipynb | 18 +++-- 21 files changed, 235 insertions(+), 120 deletions(-) diff --git a/bindings/pydrake/systems/primitives_py.cc b/bindings/pydrake/systems/primitives_py.cc index 0c7692492aa0..c95e33261690 100644 --- a/bindings/pydrake/systems/primitives_py.cc +++ b/bindings/pydrake/systems/primitives_py.cc @@ -229,31 +229,72 @@ PYBIND11_MODULE(primitives, m) { py::arg("min_value"), py::arg("max_value"), doc.Saturation.ctor.doc_2args); - DefineTemplateClassWithDefault, LeafSystem>( - m, "SignalLogger", GetPyParam(), doc.SignalLogger.doc) + DefineTemplateClassWithDefault>( + m, "SignalLog", GetPyParam(), doc.SignalLog.doc) .def(py::init(), py::arg("input_size"), - py::arg("batch_allocation_size") = 1000, doc.SignalLogger.ctor.doc) - .def("set_publish_period", &SignalLogger::set_publish_period, - py::arg("period"), doc.SignalLogger.set_publish_period.doc) - .def("set_forced_publish_only", - &SignalLogger::set_forced_publish_only, - doc.SignalLogger.set_forced_publish_only.doc) + py::arg("batch_allocation_size") = 1000, doc.SignalLog.ctor.doc) + .def("num_samples", &SignalLog::num_samples, + doc.SignalLog.num_samples.doc) .def( "sample_times", - [](const SignalLogger* self) { + [](const SignalLog* self) { // Reference return CopyIfNotPodType(self->sample_times()); }, return_value_policy_for_scalar_type(), - doc.SignalLogger.sample_times.doc) + doc.SignalLog.sample_times.doc) .def( "data", - [](const SignalLogger* self) { + [](const SignalLog* self) { // Reference. return CopyIfNotPodType(self->data()); }, - return_value_policy_for_scalar_type(), doc.SignalLogger.data.doc) - .def("reset", &SignalLogger::reset, doc.SignalLogger.reset.doc); + return_value_policy_for_scalar_type(), doc.SignalLog.data.doc) + .def("reset", &SignalLog::reset, doc.SignalLog.reset.doc) + .def("AddData", &SignalLog::AddData, py::arg("time"), + py::arg("sample"), doc.SignalLog.AddData.doc) + .def("get_input_size", &SignalLog::get_input_size, + doc.SignalLog.get_input_size.doc); + + auto cls = + DefineTemplateClassWithDefault, LeafSystem>( + m, "SignalLogger", GetPyParam(), doc.SignalLogger.doc) + .def(py::init(), py::arg("input_size"), + py::arg("batch_allocation_size") = 1000, + doc.SignalLogger.ctor.doc) + .def("set_publish_period", &SignalLogger::set_publish_period, + py::arg("period"), doc.SignalLogger.set_publish_period.doc) + .def("set_forced_publish_only", + &SignalLogger::set_forced_publish_only, + doc.SignalLogger.set_forced_publish_only.doc) + .def( + "GetLog", + [](const SignalLogger* self, const Context& context) + -> const SignalLog& { return self->GetLog(context); }, + py::arg("context"), py_rvp::reference, + doc.SignalLogger.GetLog.doc_1args) + .def( + "GetLog", + [](const SignalLogger* self, const System& outer_system, + const Context& outer_context) -> const SignalLog& { + return self->GetLog(outer_system, outer_context); + }, + py::arg("outer_system"), py::arg("outer_context"), + py_rvp::reference, doc.SignalLogger.GetLog.doc_2args) + .def( + "GetMutableLog", + [](const SignalLogger* self, const Context& context) + -> SignalLog& { return self->GetMutableLog(context); }, + py::arg("context"), py_rvp::reference, + doc.SignalLogger.GetMutableLog.doc_1args) + .def( + "GetMutableLog", + [](const SignalLogger* self, const System& outer_system, + const Context& outer_context) -> SignalLog& { + return self->GetMutableLog(outer_system, outer_context); + }, + py::arg("outer_system"), py::arg("outer_context"), + py_rvp::reference, doc.SignalLogger.GetMutableLog.doc_2args); AddTemplateFunction(m, "LogOutput", &LogOutput, GetPyParam(), py::arg("src"), py::arg("builder"), diff --git a/bindings/pydrake/systems/pyplot_visualizer.py b/bindings/pydrake/systems/pyplot_visualizer.py index 349d81413c22..f2e4f0dfacef 100644 --- a/bindings/pydrake/systems/pyplot_visualizer.py +++ b/bindings/pydrake/systems/pyplot_visualizer.py @@ -5,7 +5,7 @@ from warnings import warn from pydrake.systems.framework import LeafSystem, PublishEvent, TriggerType -from pydrake.systems.primitives import SignalLogger +from pydrake.systems.primitives import SignalLog from pydrake.trajectories import Trajectory @@ -108,7 +108,7 @@ def get_recording_as_animation(self, **kwargs): def animate(self, log, resample=True, **kwargs): """ Args: - log: A reference to a pydrake.systems.primitives.SignalLogger + log: A reference to a pydrake.systems.primitives.SignalLog or a pydrake.trajectories.Trajectory that contains the plant state after running a simulation. resample: Whether we should do a resampling operation to make the @@ -117,7 +117,7 @@ def animate(self, log, resample=True, **kwargs): matches the sample timestep of the log. Additional kwargs are passed through to FuncAnimation. """ - if isinstance(log, SignalLogger): + if isinstance(log, SignalLog): t = log.sample_times() x = log.data() diff --git a/bindings/pydrake/systems/test/primitives_test.py b/bindings/pydrake/systems/test/primitives_test.py index 34ed3011a6d1..a798dc764634 100644 --- a/bindings/pydrake/systems/test/primitives_test.py +++ b/bindings/pydrake/systems/test/primitives_test.py @@ -141,27 +141,31 @@ def test_signal_logger(self, T): diagram = builder.Build() simulator = Simulator_[T](diagram) + context = simulator.get_context() kTime = 1. simulator.AdvanceTo(kTime) # Verify outputs of the every-step logger - t = logger_per_step.sample_times() - x = logger_per_step.data() + log_per_step = logger_per_step.GetMutableLog(diagram, context) + t = log_per_step.sample_times() + x = log_per_step.data() self.assertTrue(t.shape[0] > 2) self.assertTrue(t.shape[0] == x.shape[1]) numpy_compare.assert_allclose( t[-1]*kValue, x[0, -1], atol=1e-15, rtol=0 ) - numpy_compare.assert_equal(x, logger_per_step_2.data()) + log_per_step_2 = logger_per_step_2.GetLog(diagram, context) + numpy_compare.assert_equal(x, log_per_step_2.data()) # Verify outputs of the periodic logger - t = logger_periodic.sample_times() - x = logger_periodic.data() + log_periodic = logger_periodic.GetLog(diagram, context) + t = log_periodic.sample_times() + x = log_periodic.data() # Should log exactly once every kPeriod, up to and including kTime. self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.) - logger_per_step.reset() + log_per_step.reset() # Verify that t and x retain their values after systems are deleted. t_copy = t.copy() diff --git a/bindings/pydrake/systems/test/pyplot_visualizer_test.py b/bindings/pydrake/systems/test/pyplot_visualizer_test.py index 3134c6c4d553..5cc0d5f5d7a3 100644 --- a/bindings/pydrake/systems/test/pyplot_visualizer_test.py +++ b/bindings/pydrake/systems/test/pyplot_visualizer_test.py @@ -84,9 +84,10 @@ def test_simple_visualizer(self): context.SetContinuousState([0.9]) simulator = Simulator(diagram, context) + log = logger.GetLog(diagram, context) simulator.AdvanceTo(.1) - ani = visualizer.animate(logger, repeat=True) + ani = visualizer.animate(log, repeat=True) self.assertIsInstance(ani, animation.FuncAnimation) def test_trajectory(self): diff --git a/examples/acrobot/run_lqr_w_estimator.cc b/examples/acrobot/run_lqr_w_estimator.cc index 9508a0578561..9408956f2d07 100644 --- a/examples/acrobot/run_lqr_w_estimator.cc +++ b/examples/acrobot/run_lqr_w_estimator.cc @@ -103,6 +103,9 @@ int do_main() { // Build the system/simulator. auto diagram = builder.Build(); systems::Simulator simulator(*diagram); + const auto& context = simulator.get_context(); + const auto& x_log = x_logger->GetLog(*diagram, context); + const auto& xhat_log = xhat_logger->GetLog(*diagram, context); // Set an initial condition near the upright fixed point. AcrobotState& x0 = acrobot_w_encoder->get_mutable_acrobot_state( @@ -135,18 +138,18 @@ int do_main() { using common::ToPythonTuple; CallPython("figure", 1); CallPython("clf"); - CallPython("plot", x_logger->sample_times(), - (x_logger->data().row(0).array() - M_PI) + CallPython("plot", x_log.sample_times(), + (x_log.data().row(0).array() - M_PI) .matrix().transpose()); - CallPython("plot", x_logger->sample_times(), - x_logger->data().row(1).transpose()); + CallPython("plot", x_log.sample_times(), + x_log.data().row(1).transpose()); CallPython("legend", ToPythonTuple("theta1 - PI", "theta2")); CallPython("axis", "tight"); CallPython("figure", 2); CallPython("clf"); - CallPython("plot", x_logger->sample_times(), - (x_logger->data().array() - xhat_logger->data().array()) + CallPython("plot", x_log.sample_times(), + (x_log.data().array() - xhat_log.data().array()) .matrix().transpose()); CallPython("ylabel", "error"); CallPython("legend", ToPythonTuple("theta1", "theta2", "theta1dot", diff --git a/examples/acrobot/spong_sim.cc b/examples/acrobot/spong_sim.cc index 51573699a08e..551812dcbd0b 100644 --- a/examples/acrobot/spong_sim.cc +++ b/examples/acrobot/spong_sim.cc @@ -120,7 +120,7 @@ std::string Simulate(const YAML::Node& scenario_node) { simulator.AdvanceTo(scenario.t_final); Output output; - output.x_tape = state_logger->data(); + output.x_tape = state_logger->GetLog(*diagram, context).data(); drake::yaml::YamlWriteArchive writer; writer.Accept(output); // The EmitString call below saves a document like so: diff --git a/examples/acrobot/spong_sim.py b/examples/acrobot/spong_sim.py index ad99a92ff8e2..5cf3ece6844a 100644 --- a/examples/acrobot/spong_sim.py +++ b/examples/acrobot/spong_sim.py @@ -37,6 +37,7 @@ def simulate(*, initial_state, controller_params, t_final, tape_period): plant_context = diagram.GetMutableSubsystemContext(plant, context) controller_context = diagram.GetMutableSubsystemContext( controller, context) + log = state_logger.GetLog(diagram, context) plant_context.SetContinuousState(initial_state) controller_context.get_mutable_numeric_parameter(0).SetFromVector( @@ -44,7 +45,7 @@ def simulate(*, initial_state, controller_params, t_final, tape_period): simulator.AdvanceTo(t_final) - x_tape = state_logger.data() + x_tape = log.data() return x_tape diff --git a/examples/fibonacci/run_fibonacci.cc b/examples/fibonacci/run_fibonacci.cc index da32d522a4e3..e031f1bb3283 100644 --- a/examples/fibonacci/run_fibonacci.cc +++ b/examples/fibonacci/run_fibonacci.cc @@ -29,12 +29,13 @@ int main(int argc, char* argv[]) { // Create a Simulator and use it to advance time until t=steps*h. systems::Simulator simulator(*diagram); + const auto& log = logger->GetLog(*diagram, simulator.get_context()); simulator.AdvanceTo(FLAGS_steps * FibonacciDifferenceEquation::kPeriod); // Print out the contents of the log. - for (int n = 0; n < logger->sample_times().size(); ++n) { - const double t = logger->sample_times()[n]; - std::cout << n << ": " << logger->data()(0, n) + for (int n = 0; n < log.sample_times().size(); ++n) { + const double t = log.sample_times()[n]; + std::cout << n << ": " << log.data()(0, n) << " (t=" << t << ")\n"; } diff --git a/examples/fibonacci/test/fibonacci_difference_equation_test.cc b/examples/fibonacci/test/fibonacci_difference_equation_test.cc index 55ad6eb60ec2..abeac4fa99aa 100644 --- a/examples/fibonacci/test/fibonacci_difference_equation_test.cc +++ b/examples/fibonacci/test/fibonacci_difference_equation_test.cc @@ -22,6 +22,7 @@ GTEST_TEST(Fibonacci, CheckSequence) { auto diagram = builder.Build(); systems::Simulator simulator(*diagram); + const auto& log = logger->GetLog(*diagram, simulator.get_context()); // Simulate forward to fibonacci(6): 0 1 1 2 3 5 8 simulator.AdvanceTo(6 * FibonacciDifferenceEquation::kPeriod); @@ -29,7 +30,7 @@ GTEST_TEST(Fibonacci, CheckSequence) { Eigen::VectorXd expected(7); expected << 0, 1, 1, 2, 3, 5, 8; - EXPECT_EQ(logger->data().transpose(), expected); + EXPECT_EQ(log.data().transpose(), expected); } } // namespace diff --git a/examples/manipulation_station/end_effector_teleop_sliders.py b/examples/manipulation_station/end_effector_teleop_sliders.py index 117633adddd7..3413e80390cf 100644 --- a/examples/manipulation_station/end_effector_teleop_sliders.py +++ b/examples/manipulation_station/end_effector_teleop_sliders.py @@ -380,13 +380,15 @@ def main(): differential_ik, simulator.get_mutable_context()), q0) simulator.set_target_realtime_rate(args.target_realtime_rate) + iiwa_velocities_log = iiwa_velocities.GetLog( + diagram, simulator.get_context()) simulator.AdvanceTo(args.duration) # Ensure that our initialization logic was correct, by inspecting our # logged joint velocities. if args.test: - for time, qdot in zip(iiwa_velocities.sample_times(), - iiwa_velocities.data().transpose()): + for time, qdot in zip(iiwa_velocities_log.sample_times(), + iiwa_velocities_log.data().transpose()): # TODO(jwnimmer-tri) We should be able to do better than a 40 # rad/sec limit, but that's the best we can enforce for now. if qdot.max() > 0.1: diff --git a/examples/manipulation_station/joint_teleop.py b/examples/manipulation_station/joint_teleop.py index f32499e4005a..e4c609732fec 100644 --- a/examples/manipulation_station/joint_teleop.py +++ b/examples/manipulation_station/joint_teleop.py @@ -152,8 +152,10 @@ def main(): # Ensure that our initialization logic was correct, by inspecting our # logged joint velocities. if args.test: - for time, qdot in zip(iiwa_velocities.sample_times(), - iiwa_velocities.data().transpose()): + iiwa_velocities_log = iiwa_velocities.GetLog( + diagram, simulator.get_context()) + for time, qdot in zip(iiwa_velocities_log.sample_times(), + iiwa_velocities_log.data().transpose()): # TODO(jwnimmer-tri) We should be able to do better than a 40 # rad/sec limit, but that's the best we can enforce for now. if qdot.max() > 0.1: diff --git a/examples/van_der_pol/van_der_pol.cc b/examples/van_der_pol/van_der_pol.cc index 7f04ec213902..b9737e53ddef 100644 --- a/examples/van_der_pol/van_der_pol.cc +++ b/examples/van_der_pol/van_der_pol.cc @@ -57,7 +57,7 @@ Eigen::Matrix2Xd VanDerPolOscillator::CalcLimitCycle() { // simulation results) of the cycle for μ=1. simulator.AdvanceTo(6.667); - return logger->data(); + return logger->GetLog(*diagram, simulator.get_context()).data(); } // q̈ + μ(q² - 1)q̇ + q = 0 diff --git a/systems/analysis/test/simulator_test.cc b/systems/analysis/test/simulator_test.cc index c5156f0fff40..f7c5c7b5a34f 100644 --- a/systems/analysis/test/simulator_test.cc +++ b/systems/analysis/test/simulator_test.cc @@ -1086,14 +1086,16 @@ GTEST_TEST(SimulatorTest, ExampleDiscreteSystem) { // Create a Simulator and use it to advance time until t=3*h. Simulator simulator(*diagram); + const auto& log = logger->GetLog(*diagram, simulator.get_context()); + simulator.AdvanceTo(3 * ExampleDiscreteSystem::kPeriod); testing::internal::CaptureStdout(); // Not in example. // Print out the contents of the log. - for (int n = 0; n < logger->sample_times().size(); ++n) { - const double t = logger->sample_times()[n]; - std::cout << n << ": " << logger->data()(0, n) + for (int n = 0; n < log.sample_times().size(); ++n) { + const double t = log.sample_times()[n]; + std::cout << n << ": " << log.data()(0, n) << " (" << t << ")\n"; } @@ -1162,9 +1164,10 @@ GTEST_TEST(SimulatorTest, SinusoidalHybridSystem) { const double t_final = 10.0; const double initial_value = std::sin(-f * h); // = sin(f*-1*h) Simulator simulator(*diagram); + auto& context = simulator.get_mutable_context(); + const auto& log = logger->GetLog(*diagram, context); - simulator.get_mutable_context().get_mutable_discrete_state()[0] = - initial_value; + context.get_mutable_discrete_state()[0] = initial_value; simulator.AdvanceTo(t_final); // Set a very tight tolerance value. @@ -1180,8 +1183,8 @@ GTEST_TEST(SimulatorTest, SinusoidalHybridSystem) { // y₂ 2 t = 2 h sin(f h) // y₃ 3 t = 3 h sin(f 2 h) // ... - const VectorX times = logger->sample_times(); - const MatrixX data = logger->data(); + const VectorX times = log.sample_times(); + const MatrixX data = log.data(); ASSERT_EQ(times.size(), std::round(t_final/h) + 1); ASSERT_EQ(data.rows(), 1); @@ -1819,7 +1822,7 @@ GTEST_TEST(SimulatorTest, Issue10443) { // Should log exactly once every kPeriod, up to and including // kTime. Eigen::VectorBlock> t_periodic = - periodic_logger.sample_times(); + periodic_logger.GetLog(*diagram, simulator.get_context()).sample_times(); EXPECT_EQ(t_periodic.size(), kTime * kFrequency + 1); } diff --git a/systems/primitives/signal_log.h b/systems/primitives/signal_log.h index bfb21aff3c25..ce755f99d0dc 100644 --- a/systems/primitives/signal_log.h +++ b/systems/primitives/signal_log.h @@ -17,7 +17,7 @@ namespace systems { template class SignalLog { public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SignalLog) + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SignalLog) /** Constructs the signal log. @param input_size Dimension of the per-time step data set. @@ -58,12 +58,13 @@ class SignalLog { int64_t get_input_size() const { return data_.rows(); } private: - const int batch_allocation_size_{1000}; + // This is logically constant over an instance, but is not declared const so + // that default copy and assignment can work. + int batch_allocation_size_{1000}; - // Use mutable variables to hold the logged data. - mutable int64_t num_samples_{0}; - mutable VectorX sample_times_; - mutable MatrixX data_; + int64_t num_samples_{0}; + VectorX sample_times_; + MatrixX data_; }; } // namespace systems } // namespace drake diff --git a/systems/primitives/signal_logger.cc b/systems/primitives/signal_logger.cc index e35c5b702a1e..ce658ba9791c 100644 --- a/systems/primitives/signal_logger.cc +++ b/systems/primitives/signal_logger.cc @@ -7,8 +7,19 @@ namespace systems { template SignalLogger::SignalLogger(int input_size, int batch_allocation_size) - : LeafSystem(SystemTypeTag{}), - log_(input_size, batch_allocation_size) { + : LeafSystem(SystemTypeTag{}) { + // This cache entry just maintains log storage. It is only ever updated by + // WriteToLog(). This declaration of the cache entry invokes no invalidation + // support from the cache system. + log_cache_index_ = + this->DeclareCacheEntry( + "log", ValueProducer( + // TODO(jwnimmer-tri) Improve ValueProducer constructor sugar. + internal::AbstractValueCloner( + SignalLog(input_size, batch_allocation_size)), + &ValueProducer::NoopCalc), + {this->nothing_ticket()}).cache_index(); + this->DeclareInputPort("data", kVectorValued, input_size); // Use a per-step event by default; disabled by set_publish_period() or @@ -81,9 +92,44 @@ void SignalLogger::set_forced_publish_only() { } } +template +const SignalLog& +SignalLogger::GetLog(const Context& context) const { + // Relying on the mutable implementation here avoids pointless out-of-date + // checks. + return GetMutableLog(context); +} + +template +const SignalLog& +SignalLogger::GetLog(const System& outer_system, + const Context& outer_context) const { + // Relying on the mutable implementation here avoids pointless out-of-date + // checks. + return GetMutableLog(outer_system, outer_context); +} + +template +SignalLog& +SignalLogger::GetMutableLog(const Context& context) const { + CacheEntryValue& value = + this->get_cache_entry(log_cache_index_) + .get_mutable_cache_entry_value(context); + return value.GetMutableValueOrThrow>(); +} + +template +SignalLog& +SignalLogger::GetMutableLog(const System& outer_system, + const Context& outer_context) const { + return GetMutableLog( + outer_system.GetSubsystemContext(*this, outer_context)); +} + template EventStatus SignalLogger::WriteToLog(const Context& context) const { - log_.AddData(context.get_time(), this->get_input_port().Eval(context)); + GetMutableLog(context).AddData( + context.get_time(), this->get_input_port().Eval(context)); return EventStatus::Succeeded(); } diff --git a/systems/primitives/signal_logger.h b/systems/primitives/signal_logger.h index 8a5904defa4e..deac5fc32d5d 100644 --- a/systems/primitives/signal_logger.h +++ b/systems/primitives/signal_logger.h @@ -15,13 +15,12 @@ namespace drake { namespace systems { -// TODO(sherm1) This System has problems as discussed in issue #10228 that -// should be fixed. -/// A discrete sink block which logs its input to memory (not thread safe). This -/// data is then retrievable (e.g. after a simulation) via a handful of accessor -/// methods. This system holds a large, mutable Eigen matrix for data storage, -/// where each column corresponds to a data point. It saves a data point and -/// the context time whenever it samples its input. +/// A discrete sink block which logs its input to a SignalLog object stored in +/// the context. This data is then retrievable (e.g. after a simulation) via +/// methods of the SignalLog. The SignalLog object holds a large, mutable Eigen +/// matrix for data storage, where each column corresponds to a data +/// point. This system saves a data point and the context time whenever it +/// samples its input. /// /// By default, sampling is performed every time the Simulator completes a /// trajectory-advancing substep (that is, via a per-step Publish event), with @@ -35,11 +34,6 @@ namespace systems { /// publish events, so should be disabled if you want to control logging /// yourself. /// -/// @warning %SignalLogger is _not_ thread-safe because it writes to a mutable -/// buffer internally. If you have a Diagram that contains a SignalLogger, -/// even with each thread having its own Context, the threads will conflict. -/// You would have to have separate Diagrams in each thread to avoid trouble. -/// /// @see LogOutput() for a convenient way to add %logging to a Diagram. /// @see Simulator::set_publish_every_time_step() /// @see Simulator::set_publish_at_initialization() @@ -90,23 +84,22 @@ class SignalLogger final : public LeafSystem { /// @throws std::exception if set_publish_period() has been called. void set_forced_publish_only(); - /// Returns the number of samples taken since construction or last reset(). - int num_samples() const { return log_.num_samples(); } + /// Access the signal log within this component's context. + const SignalLog& GetLog(const Context& context) const; - /// Provides access to the sample times of the logged data. Time is taken - /// from the Context when the log entry is added. - Eigen::VectorBlock> sample_times() const { - return log_.sample_times(); - } + /// Access the signal log within this component's context, given a containing + /// system and its matching context. + const SignalLog& GetLog(const System& outer_system, + const Context& outer_context) const; - /// Provides access to the logged data. - Eigen::Block, Eigen::Dynamic, Eigen::Dynamic, true> - data() const { - return log_.data(); - } + /// Access the signal log as a mutable object within this component's + /// context. + SignalLog& GetMutableLog(const Context& context) const; - /// Clears the logged data. - void reset() { log_.reset(); } + /// Access the signal log as a mutable object within this component's + /// context, given a containing system and its matching context. + SignalLog& GetMutableLog(const System& outer_system, + const Context& outer_context) const; private: template friend class SignalLogger; @@ -125,7 +118,9 @@ class SignalLogger final : public LeafSystem { LoggingMode logging_mode_{kPerStep}; - mutable SignalLog log_; // TODO(sherm1) Not thread safe :( + // The index of a cache entry that stores the log data. It is stored as a + // cache entry to maintain thread safety in context-per-thread usage. + CacheIndex log_cache_index_{}; }; /// Provides a convenience function for adding a SignalLogger, initialized to diff --git a/systems/primitives/test/discrete_derivative_test.cc b/systems/primitives/test/discrete_derivative_test.cc index 29b7778cdcee..b078fda80f16 100644 --- a/systems/primitives/test/discrete_derivative_test.cc +++ b/systems/primitives/test/discrete_derivative_test.cc @@ -49,35 +49,36 @@ void RunFirstOrderHold(const bool suppress_initial_transient) { suppress_initial_transient); builder.Connect(ppsource->get_output_port(), deriv->get_input_port()); - auto log = LogOutput(deriv->get_output_port(), &builder); + auto logger = LogOutput(deriv->get_output_port(), &builder); // Evaluate the outputs at a distinct sampling interval. - log->set_publish_period(time_step / 3.267); + logger->set_publish_period(time_step / 3.267); auto diagram = builder.Build(); Simulator simulator(*diagram); + const auto& log = logger->GetLog(*diagram, simulator.get_context()); simulator.set_publish_at_initialization(false); simulator.set_publish_every_time_step(false); simulator.AdvanceTo(kDuration); - for (int i = 0; i < log->sample_times().size(); i++) { - if (log->sample_times()(i) == 0.0) { + for (int i = 0; i < log.sample_times().size(); i++) { + if (log.sample_times()(i) == 0.0) { // The initial outputs should be zero (due to the zero initial // conditions). - EXPECT_TRUE(CompareMatrices(log->data().col(i), Vector2d(0., 0.))); - } else if (log->sample_times()(i) <= time_step) { + EXPECT_TRUE(CompareMatrices(log.data().col(i), Vector2d(0., 0.))); + } else if (log.sample_times()(i) <= time_step) { if (!suppress_initial_transient) { // The outputs should jump for one timestep because u(0) is non-zero. EXPECT_TRUE(CompareMatrices( - log->data().col(i), Vector2d(4. / time_step, 5. / time_step), + log.data().col(i), Vector2d(4. / time_step, 5. / time_step), 1e-12)); } else { // The outputs should remain zero until there two input samples. - EXPECT_TRUE(CompareMatrices(log->data().col(i), Vector2d(0., 0.))); + EXPECT_TRUE(CompareMatrices(log.data().col(i), Vector2d(0., 0.))); } } else { // Once time has advanced, outputs should have the steady-state // derivatives. - EXPECT_TRUE(CompareMatrices(log->data().col(i), Vector2d(2, 3), 1e-12)); + EXPECT_TRUE(CompareMatrices(log.data().col(i), Vector2d(2, 3), 1e-12)); } } } diff --git a/systems/primitives/test/random_source_test.cc b/systems/primitives/test/random_source_test.cc index 460691fa8648..dedfb2eef837 100644 --- a/systems/primitives/test/random_source_test.cc +++ b/systems/primitives/test/random_source_test.cc @@ -44,7 +44,8 @@ void CheckStatistics( simulator.Initialize(); simulator.AdvanceTo(20); - const auto& x = logger->data(); + const auto& log = logger->GetLog(*diagram, simulator.get_context()); + const auto& x = log.data(); const int N = x.size(); @@ -257,20 +258,23 @@ GTEST_TEST(RandomSourceTest, CorrelationTest) { const double kSampleTime = 0.0025; const auto* random1 = builder.AddSystem( RandomDistribution::kGaussian, kSize, kSampleTime); - const auto* log1 = LogOutput(random1->get_output_port(0), &builder); + const auto* logger1 = LogOutput(random1->get_output_port(0), &builder); const auto* random2 = builder.AddSystem( RandomDistribution::kGaussian, kSize, kSampleTime); - const auto* log2 = LogOutput(random2->get_output_port(0), &builder); + const auto* logger2 = LogOutput(random2->get_output_port(0), &builder); const auto diagram = builder.Build(); systems::Simulator simulator(*diagram); + const auto& context = simulator.get_context(); + const auto& log1(logger1->GetLog(*diagram, context)); + const auto& log2(logger2->GetLog(*diagram, context)); simulator.Initialize(); simulator.AdvanceTo(20); - const auto& x1 = log1->data(); - const auto& x2 = log2->data(); + const auto& x1 = log1.data(); + const auto& x2 = log2.data(); EXPECT_EQ(x1.size(), x2.size()); const int N = static_cast(x1.size()) / 2; diff --git a/systems/primitives/test/signal_logger_test.cc b/systems/primitives/test/signal_logger_test.cc index 4edef0652ad9..bc28b77d8b13 100644 --- a/systems/primitives/test/signal_logger_test.cc +++ b/systems/primitives/test/signal_logger_test.cc @@ -44,6 +44,8 @@ GTEST_TEST(TestSignalLogger, LinearSystemTest) { // Simulate the simple system from x(0) = 1.0. Simulator simulator(*diagram); Context& context = simulator.get_mutable_context(); + const auto& log = logger->GetLog(*diagram, context); + const auto& log2 = logger2->GetLog(*diagram, context); context.get_mutable_continuous_state_vector().SetAtIndex(0, 1.0); // Make the integrator tolerance sufficiently tight for the test to pass. @@ -55,14 +57,14 @@ GTEST_TEST(TestSignalLogger, LinearSystemTest) { simulator.AdvanceTo(3); // Gets the time stamps when each data point is saved. - const auto& t = logger->sample_times(); + const auto& t = log.sample_times(); // Gets the logged data. - const auto& x = logger->data(); + const auto& x = log.data(); EXPECT_EQ(x.cols(), t.size()); // Check that num_samples() makes sense. - EXPECT_EQ(logger->num_samples(), t.size()); + EXPECT_EQ(log.num_samples(), t.size()); // Now check the data (against the known solution to the diff eq). const Eigen::MatrixXd expected_x = exp(-t.transpose().array()); @@ -72,17 +74,17 @@ GTEST_TEST(TestSignalLogger, LinearSystemTest) { EXPECT_TRUE(CompareMatrices(expected_x, x, tol)); // Confirm that both loggers acquired the same data. - EXPECT_TRUE(CompareMatrices(logger->sample_times(), logger2->sample_times())); - EXPECT_TRUE(CompareMatrices(logger->data(), logger2->data())); + EXPECT_TRUE(CompareMatrices(log.sample_times(), log2.sample_times())); + EXPECT_TRUE(CompareMatrices(log.data(), log2.data())); // Test that reset makes everything empty. - logger->reset(); - EXPECT_EQ(logger->num_samples(), 0); - EXPECT_EQ(logger->sample_times().size(), 0); - EXPECT_EQ(logger->data().cols(), 0); + logger->GetMutableLog(*diagram, context).reset(); + EXPECT_EQ(log.num_samples(), 0); + EXPECT_EQ(log.sample_times().size(), 0); + EXPECT_EQ(log.data().cols(), 0); simulator.AdvanceTo(4.); - EXPECT_EQ(logger->data().cols(), logger->num_samples()); + EXPECT_EQ(log.data().cols(), log.num_samples()); } // Test that set_publish_period() causes correct triggering even for logging @@ -101,7 +103,8 @@ GTEST_TEST(TestSignalLogger, SetPublishPeriod) { // Run simulation simulator.AdvanceTo(1); - EXPECT_EQ(logger->num_samples(), 11); + const auto& log = logger->GetLog(*diagram, simulator.get_context()); + EXPECT_EQ(log.num_samples(), 11); // Check that we can only call set_publish_period() once. DRAKE_EXPECT_THROWS_MESSAGE(logger->set_publish_period(0.2), std::logic_error, @@ -123,15 +126,17 @@ GTEST_TEST(TestSignalLogger, SetForcedPublishOnly) { auto diagram = builder.Build(); auto context = diagram->CreateDefaultContext(); - EXPECT_EQ(logger->num_samples(), 0); + const SignalLog& log(logger->GetLog(*diagram, *context)); + + EXPECT_EQ(log.num_samples(), 0); diagram->Publish(*context); - EXPECT_EQ(logger->num_samples(), 1); + EXPECT_EQ(log.num_samples(), 1); // Harmless to call this again. DRAKE_EXPECT_NO_THROW(logger->set_forced_publish_only()); diagram->Publish(*context); - EXPECT_EQ(logger->num_samples(), 2); + EXPECT_EQ(log.num_samples(), 2); // Check that set_publish_period() can't be called after forced-publish. DRAKE_EXPECT_THROWS_MESSAGE( diff --git a/systems/sensors/test/beam_model_test.cc b/systems/sensors/test/beam_model_test.cc index 7b17c2870dcc..acf2eea3e28f 100644 --- a/systems/sensors/test/beam_model_test.cc +++ b/systems/sensors/test/beam_model_test.cc @@ -129,7 +129,7 @@ GTEST_TEST(BeamModelTest, TestProbabilityDensity) { simulator.Initialize(); simulator.AdvanceTo(50); - const auto& x = logger->data(); + const auto& x = logger->GetLog(*diagram, simulator.get_context()).data(); const int N = x.size(); diff --git a/tutorials/dynamical_systems.ipynb b/tutorials/dynamical_systems.ipynb index 0f12adaf7744..c2087345ca18 100644 --- a/tutorials/dynamical_systems.ipynb +++ b/tutorials/dynamical_systems.ipynb @@ -211,11 +211,12 @@ "\n", "# Create the simulator, and simulate for 10 seconds.\n", "simulator = Simulator(diagram, context)\n", + "log = logger.GetLog(diagram, context)\n", "simulator.AdvanceTo(10)\n", "\n", "# Plot the results.\n", "plt.figure()\n", - "plt.plot(logger.sample_times(), logger.data().transpose())\n", + "plt.plot(log.sample_times(), log.data().transpose())\n", "plt.xlabel('t')\n", "plt.ylabel('y(t)');" ] @@ -234,6 +235,8 @@ "\n", "# Create the simulator.\n", "simulator = Simulator(diagram)\n", + "context = simulator.get_context()\n", + "log = logger.GetLog(diagram, context)\n", "\n", "# Set the initial conditions, x(0).\n", "state = simulator.get_mutable_context().get_mutable_discrete_state_vector()\n", @@ -244,7 +247,7 @@ "\n", "# Plot the results.\n", "plt.figure()\n", - "plt.stem(logger.sample_times(), logger.data().transpose(), use_line_collection=True)\n", + "plt.stem(log.sample_times(), log.data().transpose(), use_line_collection=True)\n", "plt.xlabel('n')\n", "plt.ylabel('y[n]');" ] @@ -335,6 +338,7 @@ "# Set up a simulator to run this diagram.\n", "simulator = Simulator(diagram)\n", "context = simulator.get_mutable_context()\n", + "log = logger.GetMutableLog(diagram, context)\n", "\n", "# We'll try to regulate the pendulum to a particular angle.\n", "desired_angle = np.pi/2.\n", @@ -347,19 +351,19 @@ "# The diagram has a single input port (port index 0), which is the desired_state.\n", "diagram.get_input_port(0).FixValue(context, [desired_angle, 0.])\n", "\n", - "# Reset the logger only because we've written this notebook with the opportunity to \n", - "# simulate multiple times (in this cell) using the same logger object. This is \n", + "# Reset the log only because we've written this notebook with the opportunity to \n", + "# simulate multiple times (in this cell) using the same log object. This is \n", "# often not needed.\n", - "logger.reset()\n", + "log.reset()\n", "\n", "# Simulate for 10 seconds.\n", "simulator.AdvanceTo(20);\n", "\n", "# Plot the results.\n", - "t = logger.sample_times()\n", + "t = log.sample_times()\n", "plt.figure()\n", "# Plot theta.\n", - "plt.plot(t, logger.data()[0,:],'.-')\n", + "plt.plot(t, log.data()[0,:],'.-')\n", "# Draw a line for the desired angle.\n", "plt.plot([t[0], t[-1]], [desired_angle, desired_angle], 'g' )\n", "plt.xlabel('time (seconds)')\n",