diff --git a/systems/primitives/BUILD.bazel b/systems/primitives/BUILD.bazel index 4d40ca52b724..59e08020d664 100644 --- a/systems/primitives/BUILD.bazel +++ b/systems/primitives/BUILD.bazel @@ -41,6 +41,7 @@ drake_cc_package_library( ":trajectory_linear_system", ":trajectory_source", ":vector_log", + ":vector_log_sink", ":wrap_to_system", ":zero_order_hold", ], @@ -323,6 +324,16 @@ drake_cc_library( ], ) +drake_cc_library( + name = "vector_log_sink", + srcs = ["vector_log_sink.cc"], + hdrs = ["vector_log_sink.h"], + deps = [ + ":vector_log", + "//systems/framework", + ], +) + drake_cc_library( name = "wrap_to_system", srcs = ["wrap_to_system.cc"], @@ -671,6 +682,22 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "vector_log_sink_test", + deps = [ + ":affine_system", + ":constant_vector_source", + ":linear_system", + ":vector_log_sink", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_no_throw", + "//common/test_utilities:expect_throws_message", + "//systems/analysis:simulator", + "//systems/framework", + "//systems/framework/test_utilities:scalar_conversion", + ], +) + drake_cc_googletest( name = "wrap_to_system_test", deps = [ diff --git a/systems/primitives/test/vector_log_sink_test.cc b/systems/primitives/test/vector_log_sink_test.cc new file mode 100644 index 000000000000..a3d9d31dfd2d --- /dev/null +++ b/systems/primitives/test/vector_log_sink_test.cc @@ -0,0 +1,216 @@ +#include "drake/systems/primitives/vector_log_sink.h" + +#include +#include + +#include + +#include "drake/common/eigen_types.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_no_throw.h" +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/systems/analysis/simulator.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/test_utilities/scalar_conversion.h" +#include "drake/systems/primitives/constant_vector_source.h" +#include "drake/systems/primitives/linear_system.h" + +namespace drake { +namespace systems { +namespace { + +GTEST_TEST(TestVectorLogSink, LogGetters) { + VectorLogSink dut(11); + auto context = dut.CreateDefaultContext(); + EXPECT_EQ(&(dut.GetLog(*context)), &(dut.GetMutableLog(*context))); +} + +GTEST_TEST(TestVectorLogSink, LogFinders) { + DiagramBuilder builder; + builder.AddSystem>(22.0); + auto dut = builder.AddSystem>(1); + auto diagram = builder.Build(); + auto diagram_context = diagram->CreateDefaultContext(); + auto& logger_context = dut->GetMyContextFromRoot(*diagram_context); + + EXPECT_EQ(&(dut->GetLog(logger_context)), + &(dut->FindLog(*diagram_context))); + EXPECT_EQ(&(dut->FindLog(*diagram_context)), + &(dut->FindMutableLog(*diagram_context))); + + DRAKE_EXPECT_THROWS_MESSAGE(dut->GetLog(*diagram_context), + ".*Context was not created for.*"); + DRAKE_EXPECT_THROWS_MESSAGE(dut->FindLog(logger_context), + ".*given context must be a root context.*"); +} + +GTEST_TEST(TestVectorLogSink, LogFindersWrongContext) { + // Log sink is in *a* diagram, so something working is plausible. + DiagramBuilder builder; + auto dut = builder.AddSystem>(1); + auto diagram = builder.Build(); + + // Some unrelated wrong diagram exists to provide bad input for the test. + DiagramBuilder wrong_builder; + wrong_builder.AddSystem>(22.0); + auto wrong_diagram = wrong_builder.Build(); + auto wrong_diagram_context = wrong_diagram->CreateDefaultContext(); + + // Both access methods fail context validation. + DRAKE_EXPECT_THROWS_MESSAGE(dut->GetLog(*wrong_diagram_context), + ".*Context was not created for.*"); + DRAKE_EXPECT_THROWS_MESSAGE(dut->FindLog(*wrong_diagram_context), + ".*Context was not created for.*"); +} + +// Log the output of a simple linear system (with a known solution). +GTEST_TEST(TestVectorLogSink, LinearSystemTest) { + using std::exp; + DiagramBuilder builder; + + // xdot = -x. y = x. (No inputs). + auto plant = builder.AddSystem>( + Vector1d::Constant(-1.0), // A. + Eigen::MatrixXd::Zero(1, 0), // B. + Vector1d::Constant(1.0), // C. + Eigen::MatrixXd::Zero(1, 0)); // D. + plant->set_name("plant"); + + auto logger = builder.AddSystem>(1); + logger->set_name("logger"); + builder.Cascade(*plant, *logger); + + // Test the AddVectorLogSink helper method, too. + auto logger2 = LogVectorOutput(plant->get_output_port(), &builder); + + auto diagram = builder.Build(); + + // Simulate the simple system from x(0) = 1.0. + Simulator simulator(*diagram); + Context& context = simulator.get_mutable_context(); + const auto& log = logger->FindLog(context); + const auto& log2 = logger2->FindLog(context); + context.get_mutable_continuous_state_vector().SetAtIndex(0, 1.0); + + // Make the integrator tolerance sufficiently tight for the test to pass. + simulator.get_mutable_integrator().set_target_accuracy(1e-4); + + simulator.Initialize(); + // The Simulator schedules VectorLogSink's default per-step event, which + // performs data logging. + simulator.AdvanceTo(3); + + // Gets the time stamps when each data point is saved. + const auto& t = log.sample_times(); + + // Gets the logged data. + const auto& x = log.data(); + EXPECT_EQ(x.cols(), t.size()); + + // Check that num_samples() makes sense. + 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()); + + // Tolerance allows the default RK3 integrator to just squeak by. + double tol = 1e-5; + EXPECT_TRUE(CompareMatrices(expected_x, x, tol)); + + // Confirm that both loggers acquired the same data. + EXPECT_TRUE(CompareMatrices(log.sample_times(), log2.sample_times())); + EXPECT_TRUE(CompareMatrices(log.data(), log2.data())); + + // Test that Clear makes everything empty. + logger->FindMutableLog(context).Clear(); + EXPECT_EQ(log.num_samples(), 0); + EXPECT_EQ(log.sample_times().size(), 0); + EXPECT_EQ(log.data().cols(), 0); + + simulator.AdvanceTo(4.); + EXPECT_EQ(log.data().cols(), log.num_samples()); +} + +// Test that periodic publishing causes correct triggering even for logging +// a constant signal. +GTEST_TEST(TestVectorLogSink, PeriodicPublish) { + // Add System and Connect + DiagramBuilder builder; + auto system = builder.AddSystem>(2.0); + auto logger = LogVectorOutput(system->get_output_port(), &builder, 0.125); + auto diagram = builder.Build(); + + // Construct Simulator + Simulator simulator(*diagram); + + // Run simulation + simulator.AdvanceTo(1); + + const auto& log = logger->FindLog(simulator.get_context()); + EXPECT_EQ(log.num_samples(), 9); +} + +// Test that setting forced-publish-only triggers on an explicit Publish(). +GTEST_TEST(TestVectorLogSink, ForcedPublishOnly) { + // Add System and Connect + DiagramBuilder builder; + auto system = builder.AddSystem>(2.0); + auto logger = LogVectorOutput(system->get_output_port(), &builder, + {TriggerType::kForced}); + auto diagram = builder.Build(); + + auto context = diagram->CreateDefaultContext(); + const auto& log = logger->FindLog(*context); + + EXPECT_EQ(log.num_samples(), 0); + + diagram->Publish(*context); + EXPECT_EQ(log.num_samples(), 1); +} + +// Test that setting no forced publish triggers does not log on Publish(). +GTEST_TEST(TestVectorLogSink, NoForcedPublish) { + // Add System and Connect + DiagramBuilder builder; + auto system = builder.AddSystem>(2.0); + auto logger = LogVectorOutput(system->get_output_port(), &builder, + {TriggerType::kPeriodic}, 0.125); + auto diagram = builder.Build(); + + auto context = diagram->CreateDefaultContext(); + const auto& log = logger->FindLog(*context); + + EXPECT_EQ(log.num_samples(), 0); + + diagram->Publish(*context); + EXPECT_EQ(log.num_samples(), 0); +} + +GTEST_TEST(TestVectorLogSink, ScalarConversion) { + VectorLogSink dut_per_step_publish(2); + VectorLogSink dut_forced_publish(2, {TriggerType::kForced}); + VectorLogSink dut_periodic_publish(2, 0.25); + for (const auto* dut : { + &dut_per_step_publish, &dut_forced_publish, &dut_periodic_publish}) { + EXPECT_TRUE(is_autodiffxd_convertible(*dut, [&](const auto& converted) { + ASSERT_EQ(converted.num_input_ports(), 1); + EXPECT_EQ(converted.get_input_port().size(), 2); + })); + EXPECT_TRUE(is_symbolic_convertible(*dut, [&](const auto& converted) { + ASSERT_EQ(converted.num_input_ports(), 1); + EXPECT_EQ(converted.get_input_port().size(), 2); + })); + } +} + +GTEST_TEST(TestVectorLogSink, DiagramToAutoDiff) { + DiagramBuilder builder; + auto system = builder.AddSystem>(2.0); + LogVectorOutput(system->get_output_port(), &builder); + auto diagram = builder.Build(); + EXPECT_TRUE(is_autodiffxd_convertible(*diagram)); +} + +} // namespace +} // namespace systems +} // namespace drake diff --git a/systems/primitives/vector_log_sink.cc b/systems/primitives/vector_log_sink.cc new file mode 100644 index 000000000000..bcfd8a997ec7 --- /dev/null +++ b/systems/primitives/vector_log_sink.cc @@ -0,0 +1,121 @@ +#include "drake/systems/primitives/vector_log_sink.h" + +#include "drake/common/default_scalars.h" + +namespace drake { +namespace systems { + +template +VectorLogSink::VectorLogSink(int input_size, double publish_period) + : VectorLogSink( + input_size, + (publish_period > 0.0) ? + TriggerTypeSet({TriggerType::kForced, TriggerType::kPeriodic}) : + TriggerTypeSet({TriggerType::kForced, TriggerType::kPerStep}), + publish_period) {} + +template +VectorLogSink::VectorLogSink( + int input_size, + const TriggerTypeSet& publish_triggers, + double publish_period) + : LeafSystem(SystemTypeTag{}), + publish_triggers_(publish_triggers), + publish_period_(publish_period) { + DRAKE_DEMAND(publish_period >= 0.0); + DRAKE_DEMAND(!publish_triggers.empty()); + + // 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(VectorLog(input_size), &ValueProducer::NoopCalc), + {this->nothing_ticket()}).cache_index(); + + this->DeclareInputPort("data", kVectorValued, input_size); + + // Check that publish_triggers does not contain an unsupported trigger. + for (const auto& trigger : publish_triggers) { + DRAKE_THROW_UNLESS((trigger == TriggerType::kForced) || + (trigger == TriggerType::kPeriodic) || + (trigger == TriggerType::kPerStep)); + } + + // Declare a forced publish so that any time Publish(.) is called on this + // system (or a Diagram containing it), a message is emitted. + if (publish_triggers.find(TriggerType::kForced) != publish_triggers.end()) { + this->DeclareForcedPublishEvent(&VectorLogSink::WriteToLog); + } + + if (publish_triggers.find(TriggerType::kPeriodic) != publish_triggers.end()) { + DRAKE_THROW_UNLESS(publish_period > 0.0); + const double offset = 0.0; + this->DeclarePeriodicPublishEvent( + publish_period, offset, &VectorLogSink::WriteToLog); + } else { + // publish_period > 0 without TriggerType::kPeriodic has no meaning and is + // likely a mistake. + DRAKE_THROW_UNLESS(publish_period == 0.0); + } + + if (publish_triggers.find(TriggerType::kPerStep) != publish_triggers.end()) { + this->DeclarePerStepEvent( + systems::PublishEvent([this]( + const systems::Context& context, + const systems::PublishEvent&) { + this->WriteToLog(context); + })); + } +} + +template +template +VectorLogSink::VectorLogSink(const VectorLogSink& other) + : VectorLogSink(other.get_input_port().size(), + other.publish_triggers_, + other.publish_period_) {} + +template +const VectorLog& +VectorLogSink::GetLog(const Context& context) const { + // Relying on the mutable implementation here avoids pointless out-of-date + // checks. + return GetMutableLog(context); +} + +template +VectorLog& +VectorLogSink::GetMutableLog(const Context& context) const { + this->ValidateContext(context); + CacheEntryValue& value = + this->get_cache_entry(log_cache_index_) + .get_mutable_cache_entry_value(context); + return value.GetMutableValueOrThrow>(); +} + +template +const VectorLog& +VectorLogSink::FindLog(const Context& root_context) const { + return FindMutableLog(root_context); +} + +template +VectorLog& +VectorLogSink::FindMutableLog(const Context& root_context) const { + return GetMutableLog(this->GetMyContextFromRoot(root_context)); +} + +template +EventStatus VectorLogSink::WriteToLog(const Context& context) const { + GetMutableLog(context).AddData( + context.get_time(), this->get_input_port().Eval(context)); + return EventStatus::Succeeded(); +} + +} // namespace systems +} // namespace drake + +DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( + class ::drake::systems::VectorLogSink) diff --git a/systems/primitives/vector_log_sink.h b/systems/primitives/vector_log_sink.h new file mode 100644 index 000000000000..51306a685ba9 --- /dev/null +++ b/systems/primitives/vector_log_sink.h @@ -0,0 +1,191 @@ +#pragma once + +#include +#include +#include + +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/eigen_types.h" +#include "drake/systems/framework/context.h" +#include "drake/systems/framework/diagram_builder.h" +#include "drake/systems/framework/leaf_system.h" +#include "drake/systems/framework/output_port.h" +#include "drake/systems/primitives/vector_log.h" + +namespace drake { +namespace systems { + +// TODO(rpoyner-tri) All of the multi-trigger API and implementation here +// (TriggerTypeSet, logic for trigger types and publish periods) is duplicated +// from LcmPublisherSystem. It should all be factored to LeafSystem, especially +// if a third use of this pattern turns up. +using TriggerTypeSet = std::unordered_set; + +/// A discrete sink block which logs its vector-valued input to per-context +/// memory. This data is then retrievable outside of System operation, +/// e.g. after a simulation. See the warning below. +/// +/// The stored log (a VectorLog) holds a large, Eigen matrix for data storage, +/// where each column corresponds to a data point. The VectorLogSink saves a +/// data point and the context time whenever it samples its input. +/// +/// @warning The logged data MUST NOT be used to modify the behavior of a +/// simulation. In technical terms, the log is not stored as System State, so +/// should not be considered part of that state. This distinction allows the +/// implementation to use `Publish()` as the event handler, rather than one of +/// the state-modifying handlers. +/// +/// By default, sampling is performed every time the Simulator completes a +/// trajectory-advancing substep (that is, via a per-step Publish event), with +/// the first sample occurring during Simulator::Initialize(). That means the +/// samples will generally be unevenly spaced in time. If you prefer regular +/// sampling, you may optionally specify a "publish period" in which case +/// sampling occurs periodically, with the first sample occurring at time 0. +/// Alternatively (not common), you can specify that logging should only occur +/// at "forced publish" events, meaning at explicit calls to System::Publish(). +/// The Simulator's "publish every time step" option also results in forced +/// publish events, so should be disabled (the default setting) if you want to +/// control logging yourself. +/// +/// @see LogVectorOutput() for a convenient way to add %logging to a Diagram. +/// +/// @system +/// name: VectorLogSink +/// input_ports: +/// - data +/// @endsystem +/// +/// @tparam_default_scalar +/// @ingroup primitive_systems +template +class VectorLogSink final : public LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(VectorLogSink) + + /// Constructs the vector log sink. + /// + /// @anchor vector_log_sink_default_triggers + /// Sets the default set of publish triggers: + /// if publish_period = 0, publishes on forced events and per step, + /// if publish_period > 0, publishes on forced events and periodically. + /// + /// @param input_size Dimension of the (single) input port. This corresponds + /// to the number of rows of the data matrix. + /// @param publish_period Period that messages will be published (optional). + /// If the publish period is zero or not supplied, VectorLogSink will use + /// per-step publishing instead; see + /// LeafSystem::DeclarePerStepPublishEvent(). + /// @pre publish_period is non-negative. + /// @see LogVectorOutput() helper function for a convenient way to add + /// %logging. + explicit VectorLogSink(int input_size, double publish_period = 0.0); + + /// Constructs the vector log sink with a specified set of publish triggers. + /// + /// @param input_size Dimension of the (single) input port. This corresponds + /// to the number of rows of the data matrix. + /// @param publish_triggers Set of triggers that determine when messages will + /// be published. Supported TriggerTypes are {kForced, kPeriodic, kPerStep}. + /// Will throw an error if empty or if unsupported types are provided. + /// @param publish_period Period that messages will be published (optional). + /// publish_period should only be non-zero if one of the publish_triggers is + /// kPeriodic. + /// @pre publish_period is non-negative. + /// @pre publish_period > 0 if and only if publish_triggers contains + /// kPeriodic. + /// @see LogVectorOutput() helper function for a convenient way to add + /// %logging. + VectorLogSink(int input_size, + const TriggerTypeSet& publish_triggers, + double publish_period = 0.0); + + /// Scalar-converting copy constructor. See @ref system_scalar_conversion. + template + explicit VectorLogSink(const VectorLogSink&); + + /// Access the log within this component's context. + /// @throws std::exception if context was not created for this system. + const VectorLog& GetLog(const Context& context) const; + + /// Access the log as a mutable object within this component's context. + /// @throws std::exception if context was not created for this system. + VectorLog& GetMutableLog(const Context& context) const; + + /// Access the log within a containing root context. + /// @throws std::exception if supplied context is not a root context, or was + /// not created for the containing diagram. + const VectorLog& FindLog(const Context& root_context) const; + + /// Access the log as a mutable object within a containing root context. + /// @throws std::exception if supplied context is not a root context, or was + /// not created for the containing diagram. + VectorLog& FindMutableLog(const Context& root_context) const; + + private: + template friend class VectorLogSink; + + // Remember trigger details for use in scalar conversion. + TriggerTypeSet publish_triggers_; + double publish_period_{}; + + // Logging is done in this event handler. + EventStatus WriteToLog(const Context& context) const; + + // 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_{}; +}; + +/// LogVectorOutput provides a convenience function for adding a VectorLogSink, +/// initialized to the correct size, and connected to an output in a +/// DiagramBuilder. This overload supports the default set of publish triggers. +/// See @ref vector_log_sink_default_triggers "default triggers description". +/// +/// @param src the output port to attach logging to. +/// @param builder the diagram builder. +/// @param publish_period Period that messages will be published (optional). +/// If the publish period is zero, VectorLogSink will use per-step +/// publishing instead; see LeafSystem::DeclarePerStepPublishEvent(). +/// @pre publish_period is non-negative. +template +VectorLogSink* LogVectorOutput(const OutputPort& src, + DiagramBuilder* builder, + double publish_period = 0.0) { + VectorLogSink* sink = + builder->template AddSystem>(src.size(), publish_period); + builder->Connect(src, sink->get_input_port()); + return sink; +} + +/// LogVectorOutput provides a convenience function for adding a VectorLogSink, +/// initialized to the correct size, and connected to an output in a +/// DiagramBuilder. This overload supports the full variety of publish trigger +/// behavior. +/// +/// @param src the output port to attach logging to. +/// @param builder the diagram builder. +/// @param publish_triggers Set of triggers that determine when messages will +/// be published. Supported TriggerTypes are {kForced, kPeriodic, kPerStep}. +/// Will throw an error if empty or if unsupported types are provided. +/// @param publish_period Period that messages will be published (optional). +/// publish_period should only be non-zero if one of the publish_triggers is +/// kPeriodic. +/// @pre publish_period is non-negative. +/// @pre publish_period > 0 if and only if publish_triggers contains kPeriodic. +template +VectorLogSink* LogVectorOutput( + const OutputPort& src, + DiagramBuilder* builder, + const TriggerTypeSet& publish_triggers, + double publish_period = 0.0) { + VectorLogSink* sink = + builder->template AddSystem>( + src.size(), publish_triggers, publish_period); + builder->Connect(src, sink->get_input_port()); + return sink; +} + +} // namespace systems +} // namespace drake