From 2cb554a91d4a00ac5a4a7a47b81ca2a5bd977115 Mon Sep 17 00:00:00 2001 From: Michael Sherman Date: Fri, 18 Aug 2017 11:41:43 -0700 Subject: [PATCH 1/2] Basic caching infrastructure. Rearchitect fixed input ports to support invalidation. Simplified SystemOutput. Get rid of unnecessary data structures in diagram context. Moved FixInputPort to ContextBase. Convert spring-mass test to take advantage of cached derivatives. Modify depth sensor to use the cache. Add validate step in context allocation. Rename Freestanding -> Fixed. Merge changes from PRs: dependency tracker, cache, cache entry, cloneable DiagramContinuous/DiscreteState, connect base classes, output port allocator context parameter removal, PR #8623, input port PR #8760, diagram output PR #8857. Untangle output ports from system and move implementation to header. --- .../pydrake/systems/framework_py_semantics.cc | 1 - .../pydrake/systems/framework_py_systems.cc | 4 +- .../pydrake/systems/rgbd_camera_example.py | 2 +- bindings/pydrake/systems/test/custom_test.py | 2 +- bindings/pydrake/systems/test/general_test.py | 2 +- .../pydrake/systems/test/primitives_test.py | 12 +- .../pydrake/systems/test/rendering_test.py | 2 +- bindings/pydrake/systems/test/test_util_py.cc | 2 +- bindings/pydrake/test/automotive_test.py | 6 +- .../qp_output_translator_system.cc | 2 - .../qp_output_translator_system.h | 1 - systems/framework/BUILD.bazel | 37 ++- systems/framework/context.h | 281 +++++++++++++++--- systems/framework/context_base.cc | 43 ++- systems/framework/context_base.h | 194 +++++++++++- systems/framework/dependency_tracker.h | 11 + systems/framework/diagram.cc | 3 - systems/framework/diagram.h | 178 +++++------ systems/framework/diagram_context.h | 244 +++++++++------ systems/framework/diagram_output_port.h | 26 +- systems/framework/fixed_input_port_value.cc | 7 +- systems/framework/fixed_input_port_value.h | 30 +- systems/framework/leaf_context.h | 80 +++-- systems/framework/leaf_output_port.h | 241 ++++++++------- systems/framework/leaf_system.h | 50 ++-- systems/framework/output_port.h | 260 ++++++++-------- systems/framework/output_port_base.cc | 24 ++ systems/framework/output_port_base.h | 106 +++++++ systems/framework/output_port_value.cc | 18 -- systems/framework/output_port_value.h | 203 ++++--------- systems/framework/system.h | 230 ++++++++++---- systems/framework/system_base.cc | 102 ++++++- systems/framework/system_base.h | 239 ++++++++++++++- .../framework/system_symbolic_inspector.cc | 2 +- systems/framework/system_symbolic_inspector.h | 2 +- systems/framework/test/cache_entry_test.cc | 4 +- .../framework/test/diagram_context_test.cc | 9 +- systems/framework/test/diagram_test.cc | 19 ++ .../test/fixed_input_port_value_test.cc | 55 +++- systems/framework/test/leaf_context_test.cc | 40 +++ systems/framework/test/leaf_system_test.cc | 48 ++- systems/framework/test/output_port_test.cc | 38 ++- .../framework/test/output_port_value_test.cc | 139 +++------ systems/framework/test/system_base_test.cc | 3 +- systems/framework/test/system_test.cc | 55 ++-- .../spring_mass_system/spring_mass_system.cc | 8 +- .../spring_mass_system/spring_mass_system.h | 2 +- .../test/spring_mass_system_test.cc | 197 ++++++------ .../test/constant_value_source_test.cc | 6 +- systems/sensors/BUILD.bazel | 1 + systems/sensors/depth_sensor.cc | 43 ++- systems/sensors/depth_sensor.h | 9 + systems/sensors/test/beam_model_test.cc | 2 +- 53 files changed, 2209 insertions(+), 1116 deletions(-) create mode 100644 systems/framework/output_port_base.cc create mode 100644 systems/framework/output_port_base.h diff --git a/bindings/pydrake/systems/framework_py_semantics.cc b/bindings/pydrake/systems/framework_py_semantics.cc index 9e0bab6ea67d..da894c5d5999 100644 --- a/bindings/pydrake/systems/framework_py_semantics.cc +++ b/bindings/pydrake/systems/framework_py_semantics.cc @@ -178,7 +178,6 @@ void DefineFrameworkPySemantics(py::module m) { auto system_output = DefineTemplateClassWithDefault>( m, "SystemOutput", GetPyParam()); - DefClone(&system_output); system_output .def("get_num_ports", &SystemOutput::get_num_ports) .def("get_data", &SystemOutput::get_data, diff --git a/bindings/pydrake/systems/framework_py_systems.cc b/bindings/pydrake/systems/framework_py_systems.cc index b9aefa6f758b..1277a89817cc 100644 --- a/bindings/pydrake/systems/framework_py_systems.cc +++ b/bindings/pydrake/systems/framework_py_systems.cc @@ -273,7 +273,9 @@ struct Impl { py::arg("input_port"), py::arg("output_port")) // Context. .def("CreateDefaultContext", &System::CreateDefaultContext) - .def("AllocateOutput", &System::AllocateOutput) + .def("AllocateOutput", + overload_cast_explicit>>( + &System::AllocateOutput)) .def( "EvalVectorInput", [](const System* self, const Context& arg1, int arg2) { diff --git a/bindings/pydrake/systems/rgbd_camera_example.py b/bindings/pydrake/systems/rgbd_camera_example.py index 1129715b31de..45a60c0e6f0b 100644 --- a/bindings/pydrake/systems/rgbd_camera_example.py +++ b/bindings/pydrake/systems/rgbd_camera_example.py @@ -43,7 +43,7 @@ # Allocate context and render. context = camera.CreateDefaultContext() context.FixInputPort(0, BasicVector(x)) -output = camera.AllocateOutput(context) +output = camera.AllocateOutput() camera.CalcOutput(context, output) # Get images from computed output. diff --git a/bindings/pydrake/systems/test/custom_test.py b/bindings/pydrake/systems/test/custom_test.py index 952100edbb6c..a59bed4673db 100644 --- a/bindings/pydrake/systems/test/custom_test.py +++ b/bindings/pydrake/systems/test/custom_test.py @@ -103,7 +103,7 @@ def test_adder_execution(self): system = self._create_adder_system() context = system.CreateDefaultContext() self._fix_adder_inputs(context) - output = system.AllocateOutput(context) + output = system.AllocateOutput() self.assertEquals(output.get_num_ports(), 1) system.CalcOutput(context, output) value = output.get_vector_data(0).get_value() diff --git a/bindings/pydrake/systems/test/general_test.py b/bindings/pydrake/systems/test/general_test.py index 9aab426d2920..d3bce22adb8b 100644 --- a/bindings/pydrake/systems/test/general_test.py +++ b/bindings/pydrake/systems/test/general_test.py @@ -137,7 +137,7 @@ def test_simulator_ctor(self): def check_output(context): # Check number of output ports and value for a given context. - output = system.AllocateOutput(context) + output = system.AllocateOutput() self.assertEquals(output.get_num_ports(), 1) system.CalcOutput(context, output) if T == float: diff --git a/bindings/pydrake/systems/test/primitives_test.py b/bindings/pydrake/systems/test/primitives_test.py index 065f00a3b6c0..435b38d8f147 100644 --- a/bindings/pydrake/systems/test/primitives_test.py +++ b/bindings/pydrake/systems/test/primitives_test.py @@ -156,7 +156,7 @@ def test_vector_pass_through(self): system = PassThrough(model_value.size()) context = system.CreateDefaultContext() context.FixInputPort(0, model_value) - output = system.AllocateOutput(context) + output = system.AllocateOutput() input_eval = system.EvalVectorInput(context, 0) compare_value(self, input_eval, model_value) system.CalcOutput(context, output) @@ -168,7 +168,7 @@ def test_abstract_pass_through(self): system = PassThrough(model_value) context = system.CreateDefaultContext() context.FixInputPort(0, model_value) - output = system.AllocateOutput(context) + output = system.AllocateOutput() input_eval = system.EvalAbstractInput(context, 0) compare_value(self, input_eval, model_value) system.CalcOutput(context, output) @@ -183,7 +183,7 @@ def test_gain(self): for system in systems: context = system.CreateDefaultContext() - output = system.AllocateOutput(context) + output = system.AllocateOutput() def mytest(input, expected): context.FixInputPort(0, BasicVector(input)) @@ -197,7 +197,7 @@ def mytest(input, expected): def test_saturation(self): system = Saturation((0., -1., 3.), (1., 2., 4.)) context = system.CreateDefaultContext() - output = system.AllocateOutput(context) + output = system.AllocateOutput() def mytest(input, expected): context.FixInputPort(0, BasicVector(input)) @@ -212,7 +212,7 @@ def test_wrap_to_system(self): system = WrapToSystem(2) system.set_interval(1, 1., 2.) context = system.CreateDefaultContext() - output = system.AllocateOutput(context) + output = system.AllocateOutput() def mytest(input, expected): context.FixInputPort(0, BasicVector(input)) @@ -238,7 +238,7 @@ def test_multiplexer(self): port_size = sum([len(vec) for vec in case['data']]) self.assertEqual(mux.get_output_port(0).size(), port_size) context = mux.CreateDefaultContext() - output = mux.AllocateOutput(context) + output = mux.AllocateOutput() num_ports = len(case['data']) self.assertEqual(context.get_num_input_ports(), num_ports) for j, vec in enumerate(case['data']): diff --git a/bindings/pydrake/systems/test/rendering_test.py b/bindings/pydrake/systems/test/rendering_test.py index 93598ac87aef..bec4f7d09f35 100644 --- a/bindings/pydrake/systems/test/rendering_test.py +++ b/bindings/pydrake/systems/test/rendering_test.py @@ -147,7 +147,7 @@ def test_pose_aggregator(self): # - CalcOutput. context = aggregator.CreateDefaultContext() - output = aggregator.AllocateOutput(context) + output = aggregator.AllocateOutput() p1 = [0, 1, 2] pose1 = PoseVector() diff --git a/bindings/pydrake/systems/test/test_util_py.cc b/bindings/pydrake/systems/test/test_util_py.cc index 89bab3dfc6f5..7f4c5a5299f9 100644 --- a/bindings/pydrake/systems/test/test_util_py.cc +++ b/bindings/pydrake/systems/test/test_util_py.cc @@ -180,7 +180,7 @@ PYBIND11_MODULE(test_util, m) { state.CopyToVector() + dt * state_dot.CopyToVector()); } // Calculate output. - auto output = system.AllocateOutput(*context); + auto output = system.AllocateOutput(); system.CalcOutput(*context, output.get()); return output; }); diff --git a/bindings/pydrake/test/automotive_test.py b/bindings/pydrake/test/automotive_test.py index 9e4befba0d9a..d947daca45c3 100644 --- a/bindings/pydrake/test/automotive_test.py +++ b/bindings/pydrake/test/automotive_test.py @@ -139,7 +139,7 @@ def test_pure_pursuit(self): lane = rg.junction(0).segment(0).lane(0) pure_pursuit = PurePursuitController() context = pure_pursuit.CreateDefaultContext() - output = pure_pursuit.AllocateOutput(context) + output = pure_pursuit.AllocateOutput() # Fix the inputs. ld_value = framework.AbstractValue.Make( @@ -189,7 +189,7 @@ def test_idm_controller(self): road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() - output = idm.AllocateOutput(context) + output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() @@ -235,7 +235,7 @@ def test_simple_car(self): simple_car = SimpleCar() simulator = Simulator(simple_car) context = simulator.get_mutable_context() - output = simple_car.AllocateOutput(context) + output = simple_car.AllocateOutput() # Fix the input. command = DrivingCommand() diff --git a/systems/controllers/qp_inverse_dynamics/qp_output_translator_system.cc b/systems/controllers/qp_inverse_dynamics/qp_output_translator_system.cc index 48d6bd4f0fba..80c68793774e 100644 --- a/systems/controllers/qp_inverse_dynamics/qp_output_translator_system.cc +++ b/systems/controllers/qp_inverse_dynamics/qp_output_translator_system.cc @@ -28,8 +28,6 @@ void QpOutputTranslatorSystem::CalcActuationTorques( // Output: auto out_vector = output->get_mutable_value(); - // TODO(sherm1) This computation should be cached so it will be evaluated - // when first requested and then available for re-use. out_vector = robot_.B.transpose() * qp_output->dof_torques(); DRAKE_ASSERT(out_vector.size() == robot_.get_num_actuators()); diff --git a/systems/controllers/qp_inverse_dynamics/qp_output_translator_system.h b/systems/controllers/qp_inverse_dynamics/qp_output_translator_system.h index 287bc97e1273..b87b6cfd2a3e 100644 --- a/systems/controllers/qp_inverse_dynamics/qp_output_translator_system.h +++ b/systems/controllers/qp_inverse_dynamics/qp_output_translator_system.h @@ -54,7 +54,6 @@ class QpOutputTranslatorSystem : public systems::LeafSystem { * selection matrix that maps the actuator indices to the generalized * coordinate indices. */ - // TODO(sherm1) This should be cached so it doesn't need to be recomputed. void CalcActuationTorques(const systems::Context& context, systems::BasicVector* output) const; diff --git a/systems/framework/BUILD.bazel b/systems/framework/BUILD.bazel index eec6ce14fd83..60433dfe1d29 100644 --- a/systems/framework/BUILD.bazel +++ b/systems/framework/BUILD.bazel @@ -35,6 +35,7 @@ drake_cc_package_library( ":leaf_system", ":model_values", ":output_port", + ":output_port_base", ":output_port_value", ":parameters", ":single_output_vector_source", @@ -260,6 +261,19 @@ drake_cc_library( ], ) +drake_cc_library( + name = "output_port_base", + srcs = [ + "output_port_base.cc", + ], + hdrs = [ + "output_port_base.h", + ], + deps = [ + ":framework_common", + ], +) + drake_cc_library( name = "system_base", srcs = [ @@ -272,6 +286,7 @@ drake_cc_library( ":cache_entry", ":context_base", ":input_port_base", + ":output_port_base", ], ) @@ -280,6 +295,7 @@ drake_cc_library( srcs = ["output_port_value.cc"], hdrs = ["output_port_value.h"], deps = [ + ":framework_common", ":value", ":value_checker", ":vector", @@ -295,7 +311,6 @@ drake_cc_library( hdrs = ["context.h"], deps = [ ":context_base", - ":output_port_value", ":parameters", ":state", "//common:default_scalars", @@ -309,15 +324,14 @@ drake_cc_library( srcs = ["leaf_context.cc"], hdrs = ["leaf_context.h"], deps = [ - ":cache_and_dependency_tracker", ":context", - ":parameters", ":vector", "//common:default_scalars", "//common:essential", ], ) +# TODO(sherm1) This should be "input_port". drake_cc_library( name = "input_port_descriptor", srcs = [ @@ -335,17 +349,19 @@ drake_cc_library( drake_cc_library( name = "output_port", - srcs = ["output_port.cc"], - hdrs = ["output_port.h"], + srcs = [ + "output_port.cc", + ], + hdrs = [ + "output_port.h", + ], deps = [ ":context", - ":framework_common", + ":output_port_base", ":system_base", ":value", - ":vector", "//common:default_scalars", "//common:essential", - "//common:type_safe_index", ], ) @@ -364,6 +380,7 @@ drake_cc_library( ":event_collection", ":input_port_descriptor", ":output_port", + ":output_port_value", ":system_base", ":system_constraint", ":system_scalar_converter", @@ -384,7 +401,7 @@ drake_cc_library( srcs = ["leaf_output_port.cc"], hdrs = ["leaf_output_port.h"], deps = [ - ":framework_common", + ":leaf_context", ":output_port", ":value", ":vector", @@ -584,7 +601,6 @@ drake_cc_googletest( drake_cc_googletest( name = "cache_test", deps = [ - ":cache_and_dependency_tracker", ":context_base", "//common:essential", "//systems/framework/test_utilities", @@ -765,6 +781,7 @@ drake_cc_googletest( deps = [ ":output_port_value", "//common:essential", + "//systems/framework/test_utilities", ], ) diff --git a/systems/framework/context.h b/systems/framework/context.h index 5b29a7dfae38..34d235f30d9a 100644 --- a/systems/framework/context.h +++ b/systems/framework/context.h @@ -3,11 +3,12 @@ #include #include +#include "drake/common/drake_copyable.h" #include "drake/common/drake_optional.h" #include "drake/common/drake_throw.h" #include "drake/common/pointer_cast.h" +#include "drake/common/unused.h" #include "drake/systems/framework/context_base.h" -#include "drake/systems/framework/fixed_input_port_value.h" #include "drake/systems/framework/parameters.h" #include "drake/systems/framework/state.h" #include "drake/systems/framework/value.h" @@ -63,16 +64,33 @@ class Context : public ContextBase { /// Returns the current time in seconds. const T& get_time() const { return get_step_info().time_sec; } - /// Set the current time in seconds. - virtual void set_time(const T& time_sec) { - step_info_.time_sec = time_sec; + /// Set the current time in seconds. If this is a time change, invalidates all + /// time-dependent quantities in this context and its subcontexts. + void set_time(const T& time_sec) { + if (time_sec != get_time()) { + const int64_t change_event = this->start_new_change_event(); + PropagateTimeChange(time_sec, change_event); + } } // ========================================================================= // Accessors and Mutators for State. - virtual const State& get_state() const = 0; - virtual State& get_mutable_state() = 0; + /// Returns a const reference to the whole State. + const State& get_state() const { + return do_access_state(); + } + + /// Returns a mutable reference to the whole State, invalidating _all_ + /// state-dependent computations. If you don't mean to change the whole + /// thing, use more focused methods to get mutable access to only a portion + /// of the state so that fewer computations are invalidated. + State& get_mutable_state() { + const int64_t change_event = this->start_new_change_event(); + NoteAllStateChanged(change_event); + PropagateBulkChange(change_event, &ContextBase::NoteAllStateChanged); + return do_access_mutable_state(); + } /// Returns true if the Context has no state. bool is_stateless() const { @@ -111,23 +129,59 @@ class Context : public ContextBase { return count; } - /// Sets the continuous state to @p xc, deleting whatever was there before. - void set_continuous_state(std::unique_ptr> xc) { - get_mutable_state().set_continuous_state(std::move(xc)); - } - /// Returns a mutable reference to the continuous component of the state, - /// which may be of size zero. + /// which may be of size zero. Invalidates all continuous state-dependent + /// computations in this context and its subcontexts, recursively. ContinuousState& get_mutable_continuous_state() { - return get_mutable_state().get_mutable_continuous_state(); + const int64_t change_event = this->start_new_change_event(); + NoteAllContinuousStateChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllContinuousStateChanged); + return do_access_mutable_state().get_mutable_continuous_state(); } /// Returns a mutable reference to the continuous state vector, devoid - /// of second-order structure. The vector may be of size zero. + /// of second-order structure. The vector may be of size zero. Invalidates all + /// continuous state-dependent computations in this context and its + /// subcontexts, recursively. VectorBase& get_mutable_continuous_state_vector() { return get_mutable_continuous_state().get_mutable_vector(); } + /// Sets the continuous state to @p xc, including q, v, and z partitions. + /// The supplied vector must be the same size as the existing continuous + /// state. Invalidates all continuous state-dependent computations in this + /// context and its subcontexts, recursively. + void SetContinuousState(const Eigen::Ref>& xc) { + get_mutable_continuous_state().SetFromVector(xc); + } + + /// Sets time to @p t_sec and continuous state to @p xc. Performs a single + /// invalidation pass to avoid duplicate invalidations for computations that + /// depend on both time and state. + void SetTimeAndContinuousState(const T& t_sec, + const Eigen::Ref>& xc) { + const int64_t change_event = this->start_new_change_event(); + if (t_sec != get_time()) + PropagateTimeChange(t_sec, change_event); + NoteAllContinuousStateChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllContinuousStateChanged); + do_access_mutable_state().get_mutable_continuous_state().SetFromVector(xc); + } + + /// Sets the continuous state to @p xc, deleting whatever was there before. + /// Invalidates all continuous state-dependent computations in this context + /// and its subcontexts, recursively. + // TODO(sherm1) Shouldn't be user-callable, especially for Diagrams! + void set_continuous_state(std::unique_ptr> xc) { + const int64_t change_event = this->start_new_change_event(); + NoteAllContinuousStateChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllContinuousStateChanged); + do_access_mutable_state().set_continuous_state(std::move(xc)); + } + /// Returns a const reference to the continuous component of the state, /// which may be of size zero. const ContinuousState& get_continuous_state() const { @@ -159,9 +213,14 @@ class Context : public ContextBase { } /// Returns a mutable reference to the discrete component of the state, - /// which may be of size zero. + /// which may be of size zero. Invalidates all discrete state-dependent + /// computations in this context and its subcontexts, recursively. DiscreteValues& get_mutable_discrete_state() { - return get_mutable_state().get_mutable_discrete_state(); + const int64_t change_event = this->start_new_change_event(); + NoteAllDiscreteStateChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllDiscreteStateChanged); + return do_access_mutable_state().get_mutable_discrete_state(); } /// Returns a mutable reference to the _only_ discrete state vector. @@ -172,16 +231,25 @@ class Context : public ContextBase { } /// Returns a mutable reference to group (vector) @p index of the discrete - /// state. + /// state. Invalidates all computations that depend (directly or indirectly) + /// on this discrete state group. /// @pre @p index must identify an existing group. BasicVector& get_mutable_discrete_state(int index) { + // TODO(sherm1) Invalidate only dependents of this one discrete group. DiscreteValues& xd = get_mutable_discrete_state(); return xd.get_mutable_vector(index); } /// Sets the discrete state to @p xd, deleting whatever was there before. + /// Invalidates all discrete state-dependent computations in this context and + /// its subcontexts, recursively. + // TODO(sherm1) Shouldn't be user-callable, especially for Diagrams! void set_discrete_state(std::unique_ptr> xd) { - get_mutable_state().set_discrete_state(std::move(xd)); + const int64_t change_event = this->start_new_change_event(); + NoteAllDiscreteStateChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllDiscreteStateChanged); + do_access_mutable_state().set_discrete_state(std::move(xd)); } /// Returns a const reference to group (vector) @p index of the discrete @@ -204,22 +272,37 @@ class Context : public ContextBase { } /// Returns a mutable reference to the abstract component of the state, - /// which may be of size zero. + /// which may be of size zero. Invalidates all abstract state-dependent + /// computations in this context and its subcontexts, recursively. AbstractValues& get_mutable_abstract_state() { - return get_mutable_state().get_mutable_abstract_state(); + const int64_t change_event = this->start_new_change_event(); + NoteAllAbstractStateChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllAbstractStateChanged); + return do_access_mutable_state().get_mutable_abstract_state(); } /// Returns a mutable reference to element @p index of the abstract state. + /// Invalidates all computations that depend (directly or indirectly) on this + /// abstract state variable. /// @pre @p index must identify an existing element. template U& get_mutable_abstract_state(int index) { + // TODO(sherm1) Invalidate only dependents of this one abstract variable. AbstractValues& xa = get_mutable_abstract_state(); return xa.get_mutable_value(index).GetMutableValue(); } /// Sets the abstract state to @p xa, deleting whatever was there before. + /// Invalidates all abstract state-dependent computations in this context and + /// its subcontexts, recursively. + // TODO(sherm1) Shouldn't be user-callable, especially for Diagrams! void set_abstract_state(std::unique_ptr xa) { - get_mutable_state().set_abstract_state(std::move(xa)); + const int64_t change_event = this->start_new_change_event(); + NoteAllAbstractStateChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllAbstractStateChanged); + do_access_mutable_state().set_abstract_state(std::move(xa)); } /// Returns a const reference to the abstract component of the @@ -274,24 +357,55 @@ class Context : public ContextBase { // ========================================================================= // Accessors and Mutators for Parameters. - virtual const Parameters& get_parameters() const = 0; - virtual Parameters& get_mutable_parameters() = 0; + /// Returns a const reference to this %Context's parameters. + const Parameters& get_parameters() const { return *parameters_; } + + /// Returns a mutable reference to this %Context's parameters after + /// invalidating all parameter-dependent computations in this context and + /// all its subcontexts, recursively. This is likely to be a _lot_ of + /// invalidation -- if you are really just changing a single parameter use + /// one of the indexed methods instead. + Parameters& get_mutable_parameters() { + const int64_t change_event = this->start_new_change_event(); + NoteAllParametersChanged(change_event); + PropagateBulkChange(change_event, &ContextBase::NoteAllParametersChanged); + return *parameters_; + } + + /// Sets the parameters to @p params, deleting whatever was there before. + /// You must supply a Parameters object; null is not acceptable. Invalidates + /// all parameter-dependent computations recursively in this context and + /// its subcontexts. + // TODO(sherm1) Shouldn't be user-callable, especially for Diagrams! + void set_parameters(std::unique_ptr> params) { + DRAKE_DEMAND(params != nullptr); + const int64_t change_event = this->start_new_change_event(); + NoteAllParametersChanged(change_event); + PropagateBulkChange(change_event, &ContextBase::NoteAllParametersChanged); + parameters_ = std::move(params); + } /// Returns the number of vector-valued parameters. int num_numeric_parameters() const { - return get_parameters().num_numeric_parameters(); + return parameters_->num_numeric_parameters(); } /// Returns a const reference to the vector-valued parameter at @p index. - /// Asserts if @p index doesn't exist. + /// @pre @p index must identify an existing parameter. const BasicVector& get_numeric_parameter(int index) const { - return get_parameters().get_numeric_parameter(index); + return parameters_->get_numeric_parameter(index); } /// Returns a mutable reference to element @p index of the vector-valued - /// parameters. Asserts if @p index doesn't exist. + /// parameters. Invalidates all computations dependent on this parameter. + /// @pre @p index must identify an existing parameter. BasicVector& get_mutable_numeric_parameter(int index) { - return get_mutable_parameters().get_mutable_numeric_parameter(index); + // TODO(sherm1) Invalidate only dependents of this one parameter. + const int64_t change_event = this->start_new_change_event(); + NoteAllNumericParametersChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllNumericParametersChanged); + return parameters_->get_mutable_numeric_parameter(index); } /// Returns the number of abstract-valued parameters. @@ -300,14 +414,20 @@ class Context : public ContextBase { } /// Returns a const reference to the abstract-valued parameter at @p index. - /// Asserts if @p index doesn't exist. + /// @pre @p index must identify an existing parameter. const AbstractValue& get_abstract_parameter(int index) const { return get_parameters().get_abstract_parameter(index); } /// Returns a mutable reference to element @p index of the abstract-valued - /// parameters. Asserts if @p index doesn't exist. + /// parameters. Invalidates all computations dependent on this parameter. + /// @pre @p index must identify an existing parameter. AbstractValue& get_mutable_abstract_parameter(int index) { + // TODO(sherm1) Invalidate only dependents of this one parameter. + const int64_t change_event = this->start_new_change_event(); + NoteAllAbstractParametersChanged(change_event); + PropagateBulkChange(change_event, + &ContextBase::NoteAllAbstractParametersChanged); return get_mutable_parameters().get_mutable_abstract_parameter(index); } @@ -316,7 +436,9 @@ class Context : public ContextBase { /// Records the user's requested accuracy. If no accuracy is requested, /// computations are free to choose suitable defaults, or to refuse to - /// proceed without an explicit accuracy setting. + /// proceed without an explicit accuracy setting. If this is a change to + /// the current accuracy setting, all accuracy-dependent computations in this + /// Context and its subcontexts are invalidated. /// /// Requested accuracy is stored in the %Context for two reasons: /// - It permits all computations performed over a System to see the _same_ @@ -340,13 +462,15 @@ class Context : public ContextBase { /// The common thread among these examples is that they all share the /// same %Context, so by keeping accuracy here it can be used effectively to /// control all accuracy-dependent computations. - // TODO(edrumwri) Invalidate all cached accuracy-dependent computations, and - // propagate accuracy to all subcontexts in a diagram context. - virtual void set_accuracy(const optional& accuracy) { - accuracy_ = accuracy; + void set_accuracy(const optional& accuracy) { + if (accuracy != get_accuracy()) { + const int64_t change_event = this->start_new_change_event(); + PropagateAccuracyChange(accuracy, change_event); + } } - /// Returns the accuracy setting (if any). + /// Returns the accuracy setting (if any). Note that the return type is + /// `optional` rather than the double value itself. /// @see set_accuracy() for details. const optional& get_accuracy() const { return accuracy_; } @@ -363,14 +487,55 @@ class Context : public ContextBase { /// Requires a constructor T(double). // TODO(sherm1) Should treat fixed input port values same as parameters. void SetTimeStateAndParametersFrom(const Context& source) { - set_time(T(source.get_time())); - set_accuracy(source.get_accuracy()); - get_mutable_state().SetFrom(source.get_state()); - get_mutable_parameters().SetFrom(source.get_parameters()); + // A single change event for all these changes is much faster than doing + // each separately. + const int64_t change_event = this->start_new_change_event(); + + // These two both set the value and perform invalidations. + PropagateTimeChange(T(source.get_time()), change_event); + PropagateAccuracyChange(source.get_accuracy(), change_event); + + // Invalidation is separate from the actual value change for bulk changes. + PropagateBulkChange(change_event, &ContextBase::NoteAllStateChanged); + do_access_mutable_state().SetFrom(source.get_state()); + + PropagateBulkChange(change_event, &ContextBase::NoteAllParametersChanged); + parameters_->SetFrom(source.get_parameters()); // TODO(sherm1) Fixed input copying goes here. } + /// (Internal use only) Sets a new time and notifies time-dependent + /// quantities that they are now invalid, as part of a given change event. + void PropagateTimeChange(const T& time_sec, int64_t change_event) { + NoteTimeChanged(change_event); + step_info_.time_sec = time_sec; + DoPropagateTimeChange(time_sec, change_event); + } + + /// (Internal use only) Sets a new accuracy and notifies accuracy-dependent + /// quantities that they are now invalid, as part of a given change event. + void PropagateAccuracyChange(const optional& accuracy, + int64_t change_event) { + NoteAccuracyChanged(change_event); + accuracy_ = accuracy; + DoPropagateAccuracyChange(accuracy, change_event); + } + + /// (Internal use only) Applies the given bulk-change notification method + /// to this context, and propagates the notification to subcontexts if this + /// is a Diagram. + void PropagateBulkChange( + int64_t change_event, + void (ContextBase::*NoteBulkChange)(int64_t change_event)) { + (this->*NoteBulkChange)(change_event); + DoPropagateBulkChange(change_event, NoteBulkChange); + } + + /// (Internal use only) Returns a reference to a mutable state _without_ + /// invalidation notifications. Use get_mutable_state() instead. + State& access_mutable_state() { return do_access_mutable_state(); } + protected: Context() = default; @@ -392,10 +557,42 @@ class Context : public ContextBase { ContextBase::CloneWithoutPointers(source)); } + /// Derived context class should return a const reference to its concrete + /// State object. + virtual const State& do_access_state() const = 0; + + /// Derived context class should return a mutable reference to its concrete + /// State object _without_ any invalidation. We promise not to allow user + /// access to this object without invalidation. + virtual State& do_access_mutable_state() = 0; + /// Override to return the appropriate concrete State class to be returned /// by CloneState(). virtual std::unique_ptr> DoCloneState() const = 0; + /// Diagram contexts should override this to invoke PropagateTimeChange() + /// on their subcontexts. The default implementation does nothing. + virtual void DoPropagateTimeChange(const T& time_sec, int64_t change_event) { + unused(time_sec, change_event); + } + + /// Diagram contexts should override this to invoke PropagateAccuracyChange() + /// on their subcontexts. The default implementation does nothing. + virtual void DoPropagateAccuracyChange(const optional& accuracy, + int64_t change_event) { + unused(accuracy, change_event); + } + + /// Diagram contexts should override this to invoke PropagateBulkChange() + /// on their subcontexts, passing along the indicated method that specifies + /// the particular bulk change (e.g. whole state, all parameters, all + /// discrete state variables, etc.). The default implementation does nothing. + virtual void DoPropagateBulkChange( + int64_t change_event, + void (ContextBase::*NoteBulkChange)(int64_t change_event)) { + unused(change_event, NoteBulkChange); + } + /// Returns a const reference to current time and step information. const StepInfo& get_step_info() const { return step_info_; } @@ -405,6 +602,10 @@ class Context : public ContextBase { // Accuracy setting. optional accuracy_; + + // The parameter values p for this System; this is never null. + copyable_unique_ptr> parameters_{ + std::make_unique>()}; }; } // namespace systems diff --git a/systems/framework/context_base.cc b/systems/framework/context_base.cc index cbe4b69335fe..420b677b8d53 100644 --- a/systems/framework/context_base.cc +++ b/systems/framework/context_base.cc @@ -30,17 +30,23 @@ ContextBase::~ContextBase() {} void ContextBase::DisableCaching() const { cache_.DisableCaching(); - // TODO(sherm1) Recursive disabling of descendents goes here. + const int n = do_num_subcontexts(); + for (SubsystemIndex i(0); i < n; ++i) + do_get_subcontext(i).DisableCaching(); } void ContextBase::EnableCaching() const { cache_.EnableCaching(); - // TODO(sherm1) Recursive enabling of descendents goes here. + const int n = do_num_subcontexts(); + for (SubsystemIndex i(0); i < n; ++i) + do_get_subcontext(i).EnableCaching(); } void ContextBase::SetAllCacheEntriesOutOfDate() const { cache_.SetAllEntriesOutOfDate(); - // TODO(sherm1) Recursive update of descendents goes here. + const int n = do_num_subcontexts(); + for (SubsystemIndex i(0); i < n; ++i) + do_get_subcontext(i).SetAllCacheEntriesOutOfDate(); } std::string ContextBase::GetSystemPathname() const { @@ -79,10 +85,31 @@ void ContextBase::SetFixedInputPortValue( DRAKE_DEMAND(0 <= index && index < get_num_input_ports()); DRAKE_DEMAND(port_value != nullptr); + DependencyTracker& port_tracker = + get_mutable_tracker(input_port_tickets()[index]); + FixedInputPortValue* old_value = + input_port_values_[index].get_mutable(); + + if (old_value != nullptr) { + // All the dependency wiring is already in place. + detail::ContextBaseFixedInputAttorney::set_ticket(port_value.get(), + old_value->ticket()); + } else { + // Create a new tracker and subscribe to it. + DependencyTracker& value_tracker = graph_.CreateNewDependencyTracker( + "Value for fixed input port " + std::to_string(index)); + detail::ContextBaseFixedInputAttorney::set_ticket(port_value.get(), + value_tracker.ticket()); + port_tracker.SubscribeToPrerequisite(&value_tracker); + } + // Fill in the FixedInputPortValue object and install it. detail::ContextBaseFixedInputAttorney::set_owning_subcontext( port_value.get(), this); input_port_values_[index] = std::move(port_value); + + // Invalidate anyone who cares about this input port. + port_tracker.NoteValueChange(start_new_change_event()); } // Set up trackers for independent sources: time, accuracy, state, parameters, @@ -206,7 +233,10 @@ void ContextBase::BuildTrackerPointerMap( // First map the pointers local to this context. graph_.AppendToTrackerPointerMap(clone.get_dependency_graph(), &(*tracker_map)); - // TODO(sherm1) Recursive update of descendents goes here. + // Then recursively ask our descendants to add their information to the map. + for (SubsystemIndex i(0); i < num_subcontexts(); ++i) + get_subcontext(i).BuildTrackerPointerMap(clone.get_subcontext(i), + &(*tracker_map)); } void ContextBase::FixContextPointers( @@ -224,7 +254,10 @@ void ContextBase::FixContextPointers( } } - // TODO(sherm1) Recursive update of descendents goes here. + // Then recursively ask our descendants to repair their pointers. + for (SubsystemIndex i(0); i < num_subcontexts(); ++i) + get_mutable_subcontext(i).FixContextPointers(source.get_subcontext(i), + tracker_map); } } // namespace systems diff --git a/systems/framework/context_base.h b/systems/framework/context_base.h index 5d2677238a5c..e624056335c6 100644 --- a/systems/framework/context_base.h +++ b/systems/framework/context_base.h @@ -145,6 +145,127 @@ class ContextBase : public internal::ContextMessageInterface { return static_cast(input_port_tickets_.size()); } + /** Returns the number of output ports represented in this context. */ + int get_num_output_ports() const { + return static_cast(output_port_tickets_.size()); + } + + /** Returns the number of direct child subcontexts this context has. + A leaf context will return 0. */ + int num_subcontexts() const { return do_num_subcontexts(); } + + /** Gets const access to a particular subcontext of this diagram context. + The index must be in range [0..num_subsystems()-1]. */ + const ContextBase& get_subcontext(SubsystemIndex index) const { + return do_get_subcontext(index); + } + + /** Gets const access to a particular subcontext of this diagram context. + The index must be in range [0..num_subsystems()-1]. */ + ContextBase& get_mutable_subcontext(SubsystemIndex index) { + return const_cast(get_subcontext(index)); + } + + /** Returns `true` if this subcontext is the root of the Context tree that + it is in. If so, it has a null parent context. */ + bool is_root_context() const { return !parent_; } + + /** Returns a const reference to the %ContextBase of the root Context of the + tree containing this subcontext. */ + // TODO(sherm1) Consider precalculating this for faster access. + const ContextBase& get_root_context_base() const { + const ContextBase* node = this; + while (node->get_parent_base()) node = node->get_parent_base(); + return *node; + } + + /** Returns a mutable reference to the root Context of the tree containing + this subcontext. */ + ContextBase& get_mutable_root_context_base() { + return const_cast(get_root_context_base()); + } + + /** Starts a new change event and returns the event number which is unique + for this entire Context tree, not just this subcontext. */ + int64_t start_new_change_event() { + return get_mutable_root_context_base() + .increment_local_change_event_counter(); + } + + /** Force invalidation of any time-dependent computation. */ + void NoteTimeChanged(int64_t change_event) { + get_tracker(DependencyTicket(internal::kTimeTicket)) + .NoteValueChange(change_event); + } + + /** Force invalidation of any accuracy-dependent computation. */ + void NoteAccuracyChanged(int64_t change_event) { + get_tracker(DependencyTicket(internal::kAccuracyTicket)) + .NoteValueChange(change_event); + } + + /** Force invalidation of any state-dependent computation. */ + void NoteAllStateChanged(int64_t change_event) { + NoteAllContinuousStateChanged(change_event); + NoteAllDiscreteStateChanged(change_event); + NoteAllAbstractStateChanged(change_event); + } + + /** Force invalidation of any continuous state-dependent computation. */ + void NoteAllContinuousStateChanged(int64_t change_event) { + NoteAllQChanged(change_event); + NoteAllVChanged(change_event); + NoteAllZChanged(change_event); + } + + /** Force invalidation of any q-dependent computation. */ + void NoteAllQChanged(int64_t change_event) { + get_tracker(DependencyTicket(internal::kQTicket)) + .NoteValueChange(change_event); + } + + /** Force invalidation of any v-dependent computation. */ + void NoteAllVChanged(int64_t change_event) { + get_tracker(DependencyTicket(internal::kVTicket)) + .NoteValueChange(change_event); + } + + /** Force invalidation of any z-dependent computation. */ + void NoteAllZChanged(int64_t change_event) { + get_tracker(DependencyTicket(internal::kZTicket)) + .NoteValueChange(change_event); + } + + /** Force invalidation of any discrete state-dependent computation. */ + void NoteAllDiscreteStateChanged(int64_t change_event) { + for (auto ticket : discrete_state_tickets_) + get_tracker(ticket).NoteValueChange(change_event); + } + + /** Force invalidation of any abstract state-dependent computation. */ + void NoteAllAbstractStateChanged(int64_t change_event) { + for (auto ticket : abstract_state_tickets_) + get_tracker(ticket).NoteValueChange(change_event); + } + + /** Force invalidation of any parameter-dependent computation. */ + void NoteAllParametersChanged(int64_t change_event) { + NoteAllNumericParametersChanged(change_event); + NoteAllAbstractParametersChanged(change_event); + } + + /** Force invalidation of any numeric parameter-dependent computation. */ + void NoteAllNumericParametersChanged(int64_t change_event) { + for (auto ticket : numeric_parameter_tickets_) + get_tracker(ticket).NoteValueChange(change_event); + } + + /** Force invalidation of any abstract parameter-dependent computation. */ + void NoteAllAbstractParametersChanged(int64_t change_event) { + for (auto ticket : abstract_parameter_tickets_) + get_tracker(ticket).NoteValueChange(change_event); + } + /** Connects the input port at `index` to a FixedInputPortValue with the given abstract `value`. Returns a reference to the allocated FixedInputPortValue that will remain valid until this input port's value @@ -184,13 +305,34 @@ class ContextBase : public internal::ContextMessageInterface { } // For internal use only. -#if !defined(DRAKE_DOXYGEN_CXX) + #if !defined(DRAKE_DOXYGEN_CXX) // Add the next input port. Expected index is supplied along with the // assigned ticket. Subscribe the "all input ports" tracker to this one. void AddInputPort(InputPortIndex expected_index, DependencyTicket ticket); -#endif + // These provide access to the memorized tickets for various resources. + // These are set by SystemBase::AllocateContext(). + + std::vector& input_port_tickets() { + return input_port_tickets_; + } + std::vector& output_port_tickets() { + return output_port_tickets_; + } + std::vector& discrete_state_tickets() { + return discrete_state_tickets_; + } + std::vector& abstract_state_tickets() { + return abstract_state_tickets_; + } + std::vector& numeric_parameter_tickets() { + return numeric_parameter_tickets_; + } + std::vector& abstract_parameter_tickets() { + return abstract_parameter_tickets_; + } + #endif protected: /** Default constructor creates an empty ContextBase but initializes all the @@ -228,6 +370,21 @@ class ContextBase : public internal::ContextMessageInterface { child->set_parent(parent); } + /** (Internal use only) */ + // Gets the value of change event counter stored in this subcontext. This + // should only be used when this subcontext is serving as root. + int64_t get_local_change_event_counter() const { + return current_change_event_; + } + + /** (Internal use only) */ + // Increments the change event counter for this subcontext and returns the + // new value. This should only be used when this subcontext is serving + // as root. + int64_t increment_local_change_event_counter() { + return ++current_change_event_; + } + /** Derived classes must implement this so that it performs the complete deep copy of the context, including all base class members but not fixing up base class pointers. To do that, implement a protected copy constructor @@ -236,6 +393,19 @@ class ContextBase : public internal::ContextMessageInterface { `return unique_ptr(new DerivedType(*this));`. */ virtual std::unique_ptr DoCloneWithoutPointers() const = 0; + /** DiagramContext must override this to return the actual number + of immediate child subcontexts it contains. The default is 0. */ + virtual int do_num_subcontexts() const { return 0; } + + /** DiagramContext must override this to provide access to its contained + subcontexts. The default implementation throws a logic error. */ + virtual const ContextBase& do_get_subcontext( + SubsystemIndex index) const { + unused(index); + throw std::logic_error( + "ContextBase::do_get_subcontext: called on a leaf context."); + } + private: friend class detail::SystemBaseContextBaseAttorney; @@ -279,13 +449,21 @@ class ContextBase : public internal::ContextMessageInterface { void FixContextPointers(const ContextBase& source, const DependencyTracker::PointerMap& tracker_map); - // TODO(sherm1) Use these tickets to reconstruct the dependency graph when - // cloning or transmogrifying a Context without a System present. + // We record tickets so we can reconstruct the dependency graph when cloning + // or transmogrifying a Context without a System present. // Index by InputPortIndex. std::vector input_port_tickets_; - - // TODO(sherm1) Output port, state, and parameter tickets go here. + // Index by OutputPortIndex. + std::vector output_port_tickets_; + // Index by DiscreteStateIndex. + std::vector discrete_state_tickets_; + // Index by AbstractStateIndex. + std::vector abstract_state_tickets_; + // Index by NumericParameterIndex. + std::vector numeric_parameter_tickets_; + // Index by AbstractParameterIndex. + std::vector abstract_parameter_tickets_; // For each input port, the fixed value or null if the port is connected to // something else (in which case we need System help to get the value). @@ -302,6 +480,10 @@ class ContextBase : public internal::ContextMessageInterface { // This is the dependency graph for values within this subcontext. DependencyGraph graph_; + // This is used only when this subcontext is serving as the root + // of a context tree. + int64_t current_change_event_{0}; + // The Context of the enclosing Diagram. Null/invalid when this is the root // context. reset_on_copy parent_; diff --git a/systems/framework/dependency_tracker.h b/systems/framework/dependency_tracker.h index a5fe8d72f5a5..80044ea9048f 100644 --- a/systems/framework/dependency_tracker.h +++ b/systems/framework/dependency_tracker.h @@ -150,6 +150,17 @@ class DependencyTracker { DependencyGraph. The ticket is unique within the containing subcontext. */ DependencyTicket ticket() const { return ticket_; } + /** Returns a pointer to the CacheEntryValue if this tracker is a cache + entry tracker, otherwise nullptr. */ + // This is for validating that this tracker is associated with the right + // cache entry. Don't use this during runtime invalidation. + const CacheEntryValue* cache_entry_value() const { + DRAKE_DEMAND(cache_value_); + if (cache_value_ == &CacheEntryValue::dummy()) + return nullptr; + return cache_value_; + } + /** Notifies `this` %DependencyTracker that its managed value was directly modified or made available for mutable access. That is, this is the _initiating_ event of a value modification. All of our downstream diff --git a/systems/framework/diagram.cc b/systems/framework/diagram.cc index 8be4ea92d676..749e4307b75c 100644 --- a/systems/framework/diagram.cc +++ b/systems/framework/diagram.cc @@ -2,8 +2,5 @@ #include "drake/common/default_scalars.h" -DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::drake::systems::internal::DiagramOutput) - DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class ::drake::systems::Diagram) diff --git a/systems/framework/diagram.h b/systems/framework/diagram.h index bb58a4d01820..18a8d20adc84 100644 --- a/systems/framework/diagram.h +++ b/systems/framework/diagram.h @@ -30,60 +30,9 @@ namespace drake { namespace systems { -template -class Diagram; template class DiagramBuilder; -namespace internal { - -//============================================================================== -// DIAGRAM OUTPUT -//============================================================================== -/// DiagramOutput is an implementation of SystemOutput that holds unowned -/// OutputPortValue pointers. It is used to expose the outputs of constituent -/// systems as outputs of a Diagram. -/// -/// @tparam T The type of the output data. Must be a valid Eigen scalar. -template -class DiagramOutput : public SystemOutput { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DiagramOutput) - - DiagramOutput() = default; - - int get_num_ports() const override { return static_cast(ports_.size()); } - - OutputPortValue* get_mutable_port_value(int index) override { - DRAKE_DEMAND(index >= 0 && index < get_num_ports()); - return ports_[index]; - } - - const OutputPortValue& get_port_value(int index) const override { - DRAKE_DEMAND(index >= 0 && index < get_num_ports()); - return *ports_[index]; - } - - std::vector* get_mutable_port_values() { return &ports_; } - - protected: - // Returns a clone that has the same number of output ports, with values - // set to nullptr. - DiagramOutput* DoClone() const override { - DiagramOutput* clone = new DiagramOutput(); - clone->ports_.resize(get_num_ports()); - return clone; - } - - private: - std::vector ports_; -}; - -} // namespace internal - -//============================================================================== -// DIAGRAM -//============================================================================== /// Diagram is a System composed of one or more constituent Systems, arranged /// in a directed graph where the vertices are the constituent Systems /// themselves, and the edges connect the output of one constituent System @@ -274,19 +223,6 @@ class Diagram : public System, internal::SystemParentServiceInterface { } } - std::unique_ptr> AllocateOutput( - const Context& context) const override { - auto diagram_context = dynamic_cast*>(&context); - DRAKE_DEMAND(diagram_context != nullptr); - - // The output ports of this Diagram are output ports of its constituent - // systems. Create a DiagramOutput with that many ports. - auto output = std::make_unique>(); - output->get_mutable_port_values()->resize(output_port_ids_.size()); - ExposeSubsystemOutputs(*diagram_context, output.get()); - return std::move(output); - } - /// @cond // The three methods below are hidden from doxygen, as described in // documentation for their corresponding methods in System. @@ -874,23 +810,40 @@ class Diagram : public System, internal::SystemParentServiceInterface { } private: + // Allocates a default-constructed diagram context containing the complete + // diagram substructure of default-constructed subcontexts. std::unique_ptr DoMakeContext() const final { - const int num_systems = num_subsystems(); // Reserve inputs as specified during Diagram initialization. - auto context = std::make_unique>(num_systems); + auto context = std::make_unique>(num_subsystems()); // Recursively construct each constituent system and its subsystems, // then add to this diagram Context. - for (SubsystemIndex i(0); i < num_systems; ++i) { + for (SubsystemIndex i(0); i < num_subsystems(); ++i) { const System& sys = *registered_systems_[i]; auto subcontext = dynamic_pointer_cast_or_throw>( SystemBase::MakeContext(sys)); - auto suboutput = sys.AllocateOutput(*subcontext); - context->AddSystem(i, std::move(subcontext), std::move(suboutput)); + context->AddSystem(i, std::move(subcontext)); } - // TODO(sherm1) Move to separate interconnection phase. - // Wire up the Diagram-internal inputs and outputs. + return std::move(context); + } + + // Given a fully-populated diagram context created by MakeContext(), set up + // the inter-subcontext dependencies for input and output ports. + void DoMakeContextConnections(ContextBase* context_base) const final { + auto context = dynamic_cast*>(context_base); + + // Give all our subsystems a chance to set up their inter-subcontext + // dependencies if they are diagrams. Traversal order doesn't matter here. + for (SubsystemIndex i(0); i < num_subsystems(); ++i) { + const System& sys = *registered_systems_[i]; + Context& subcontext = context->GetMutableSubsystemContext(i); + SystemBase::MakeContextConnections(sys, &subcontext); + } + + // Connect child subsystem input ports to the child subsystem output ports + // on which they depend. Declares dependency of each input port on its + // connected output port. for (const auto& connection : connection_map_) { const OutputPortLocator& src = connection.second; const InputPortLocator& dest = connection.first; @@ -906,11 +859,30 @@ class Diagram : public System, internal::SystemParentServiceInterface { context->ExportInput(i, ConvertToContextPortIdentifier(id)); } - // TODO(sherm1) Move to final resource allocation phase. + // Connect exported child subsystem output ports to the Diagram-level output + // ports to which they have been exported. Declares dependency of each + // Diagram-level output on its child-level output. + for (OutputPortIndex i(0); i < this->get_num_output_ports(); ++i) { + const OutputPortLocator& id = output_port_ids_[i]; + context->ExportOutput(i, ConvertToContextPortIdentifier(id)); + } + } + + // Creates the diagram's composite data structures that collect its + // subsystems' resources. + void DoAcquireContextResources(ContextBase* context_base) const final { + auto context = dynamic_cast*>(context_base); + + // Depth-first acquisition of resources to make sure leaf resources are + // there before we collect them. + for (SubsystemIndex i(0); i < num_subsystems(); ++i) { + const System& sys = *registered_systems_[i]; + Context& subcontext = context->GetMutableSubsystemContext(i); + SystemBase::AcquireContextResources(sys, &subcontext); + } + context->MakeState(); context->MakeParameters(); - - return context; } // Permits child Systems to take a look at the completed Context to see @@ -933,13 +905,13 @@ class Diagram : public System, internal::SystemParentServiceInterface { // - to an input port of this Diagram, // - or not connected at all in which case we return null. const AbstractValue* EvalConnectedSubsystemInputPort( - const ContextBase& context, + const ContextBase& context_base, const InputPortBase& input_port_base) const final { auto& diagram_context = - dynamic_cast&>(context); - auto& input_port = - dynamic_cast&>(input_port_base); - const InputPortLocator id{input_port.get_system(), input_port.get_index()}; + dynamic_cast&>(context_base); + auto& system = + dynamic_cast&>(input_port_base.get_system_base()); + const InputPortLocator id{&system, input_port_base.get_index()}; // Find if this input port is exported (connected to an input port of this // containing diagram). @@ -1458,16 +1430,18 @@ class Diagram : public System, internal::SystemParentServiceInterface { const int port_index = port.second; const auto& source_output_port = sys->get_output_port(port_index); auto diagram_port = std::make_unique>( - *this, *this, OutputPortIndex(this->get_num_output_ports()), + static_cast*>(this), + static_cast(this), + OutputPortIndex(this->get_num_output_ports()), + this->assign_next_dependency_ticket(), &source_output_port, GetSystemIndexOrAbort(&source_output_port.get_system())); this->CreateOutputPort(std::move(diagram_port)); } // Returns a reference to the value in the given context, of the specified - // output port of one of this Diagram's immediate subsystems. Calculates the - // result first before returning it. - // TODO(sherm1) Replace with cached version to avoid recalculation. + // output port of one of this Diagram's immediate subsystems, recalculating + // if necessary to bring the value up to date. const AbstractValue& EvalSubsystemOutputPort( const DiagramContext& context, const OutputPortLocator& id) const { const System* const system = id.first; @@ -1477,10 +1451,7 @@ class Diagram : public System, internal::SystemParentServiceInterface { SPDLOG_TRACE(log(), "Evaluating output for subsystem {}, port {}", system->GetSystemPathname(), port_index); const Context& subsystem_context = context.GetSubsystemContext(i); - SystemOutput* subsystem_output = context.GetSubsystemOutput(i); - AbstractValue* port_output = subsystem_output->GetMutableData(port_index); - port.Calc(subsystem_context, port_output); - return *port_output; + return port.EvalAbstract(subsystem_context); } // Converts an InputPortLocator to a DiagramContext::InputPortIdentifier. @@ -1506,30 +1477,6 @@ class Diagram : public System, internal::SystemParentServiceInterface { return identifier; } - // Sets up the OutputPortValue pointers in @p output to point to the subsystem - // output values, found in @p context, that are the outputs of this Diagram. - void ExposeSubsystemOutputs(const DiagramContext& context, - internal::DiagramOutput* output) const { - // The number of output ports of this diagram must equal the number of - // ports in the provided DiagramOutput. - const int num_ports = static_cast(output_port_ids_.size()); - DRAKE_DEMAND(output->get_num_ports() == num_ports); - - for (OutputPortIndex i(0); i < num_ports; ++i) { - const OutputPortLocator& id = output_port_ids_[i]; - // For each configured output port ID, obtain from the DiagramContext the - // actual OutputPortValue that supplies its value. - const SubsystemIndex sys_index = GetSystemIndexOrAbort(id.first); - const OutputPortIndex port_index = id.second; - SystemOutput* subsystem_output = context.GetSubsystemOutput(sys_index); - OutputPortValue* output_port_value = - subsystem_output->get_mutable_port_value(port_index); - - // Then, put a pointer to that OutputPortValue in the DiagramOutput. - (*output->get_mutable_port_values())[i] = output_port_value; - } - } - // Returns true if every port mentioned in the connection map exists. bool PortsAreValid() const { for (const auto& entry : connection_map_) { @@ -1573,6 +1520,15 @@ class Diagram : public System, internal::SystemParentServiceInterface { return static_cast(registered_systems_.size()); } + // Override SystemBase virtuals. + int do_num_subsystems() const override { return num_subsystems(); } + + // Note covariant return type. + const System& do_get_subsystem(SubsystemIndex index) const override { + DRAKE_ASSERT(registered_systems_[index] != nullptr); + return *registered_systems_[index]; + } + // A map from the input ports of constituent systems, to the output ports of // the systems from which they get their values. std::map connection_map_; diff --git a/systems/framework/diagram_context.h b/systems/framework/diagram_context.h index e4c4aca48abf..be0c76471735 100644 --- a/systems/framework/diagram_context.h +++ b/systems/framework/diagram_context.h @@ -12,7 +12,7 @@ #include "drake/systems/framework/context.h" #include "drake/systems/framework/diagram_continuous_state.h" #include "drake/systems/framework/fixed_input_port_value.h" -#include "drake/systems/framework/output_port_value.h" +#include "drake/systems/framework/framework_common.h" #include "drake/systems/framework/parameters.h" #include "drake/systems/framework/state.h" #include "drake/systems/framework/supervector.h" @@ -20,6 +20,9 @@ namespace drake { namespace systems { +//============================================================================== +// DIAGRAM STATE +//============================================================================== /// DiagramState is a State, annotated with pointers to all the mutable /// substates that it spans. template @@ -103,6 +106,9 @@ class DiagramState : public State { std::vector>> owned_substates_; }; +//============================================================================== +// DIAGRAM CONTEXT +//============================================================================== /// The DiagramContext is a container for all of the data necessary to uniquely /// determine the computations performed by a Diagram. Specifically, a /// DiagramContext contains contexts and outputs for all the constituent @@ -134,22 +140,26 @@ class DiagramContext final : public Context { /// number and ordering of subcontexts is identical to the number and /// ordering of subsystems in the corresponding Diagram. explicit DiagramContext(int num_subcontexts) - : outputs_(num_subcontexts), contexts_(num_subcontexts), + : contexts_(num_subcontexts), state_(std::make_unique>(num_subcontexts)) {} + /// Returns the number of immediate child subcontexts in this %DiagramContext. + // This shadows the ContextBase method of the same name to avoid the + // virtual function call when used locally, but is functionally identical. + int num_subcontexts() const { + return static_cast(contexts_.size()); + } + /// Declares a new subsystem in the DiagramContext. Subsystems are identified /// by number. If the subsystem has already been declared, aborts. /// /// User code should not call this method. It is for use during Diagram /// context allocation only. - void AddSystem(SubsystemIndex index, std::unique_ptr> context, - std::unique_ptr> output) { + void AddSystem(SubsystemIndex index, std::unique_ptr> context) { DRAKE_DEMAND(index >= 0 && index < num_subcontexts()); DRAKE_DEMAND(contexts_[index] == nullptr); - DRAKE_DEMAND(outputs_[index] == nullptr); - Context::set_parent(context.get(), this); + ContextBase::set_parent(context.get(), this); contexts_[index] = std::move(context); - outputs_[index] = std::move(output); } /// (Internal use only) Declares that a particular input port of a child @@ -168,34 +178,85 @@ class DiagramContext final : public Context { InputPortIndex subsystem_iport_index = subsystem_input_port.second; Context& subcontext = GetMutableSubsystemContext(subsystem_index); DRAKE_DEMAND(0 <= subsystem_iport_index && - subsystem_iport_index < subcontext.get_num_input_ports()); - - // TODO(sherm1) Set up dependency of subsystem input on diagram input. - unused(input_port_index); // For now. + subsystem_iport_index < subcontext.get_num_input_ports()); + + // Get this Diagram's input port that serves as the source. + const DependencyTicket iport_ticket = + this->input_port_tickets()[input_port_index]; + DependencyTracker& iport_tracker = + this->get_mutable_tracker(iport_ticket); + const DependencyTicket subsystem_iport_ticket = + subcontext.input_port_tickets()[subsystem_iport_index]; + DependencyTracker& subsystem_iport_tracker = + subcontext.get_mutable_tracker(subsystem_iport_ticket); + subsystem_iport_tracker.SubscribeToPrerequisite(&iport_tracker); } - /// (Internal use only) Declares that the output port specified by @p src is - /// connected to the input port specified by @p dest. + /// (Internal use only) Declares that a particular output port of this + /// diagram is simply forwarded from an output port of one of its child + /// subsystems. Sets up tracking of the diagram port's dependency on the child + /// port. Aborts if the subsystem has not been added to the DiagramContext. /// /// User code should not call this method. It is for use during Diagram /// context allocation only. - void Connect(const OutputPortIdentifier& src, - const InputPortIdentifier& dest) { - // Identify and validate the source port. - SubsystemIndex src_system_index = src.first; - OutputPortIndex src_port_index = src.second; - SystemOutput* src_ports = GetSubsystemOutput(src_system_index); - DRAKE_DEMAND(src_port_index >= 0); - DRAKE_DEMAND(src_port_index < src_ports->get_num_ports()); - - // Identify and validate the destination port. - SubsystemIndex dest_system_index = dest.first; - InputPortIndex dest_port_index = dest.second; - Context& dest_context = GetMutableSubsystemContext(dest_system_index); - DRAKE_DEMAND(dest_port_index >= 0); - DRAKE_DEMAND(dest_port_index < dest_context.get_num_input_ports()); - - // TODO(sherm1) Set up dependency of input port on connected output port. + void ExportOutput(OutputPortIndex oport_index, + const OutputPortIdentifier& subsystem_oport) { + // Identify and validate the source output port. + SubsystemIndex subsystem_index = subsystem_oport.first; + OutputPortIndex subsystem_oport_index = subsystem_oport.second; + Context& subcontext = GetMutableSubsystemContext(subsystem_index); + DRAKE_DEMAND(0 <= subsystem_oport_index && + subsystem_oport_index < subcontext.get_num_output_ports()); + + // Get the child subsystem's output port tracker that serves as the source. + const DependencyTicket subsystem_oport_ticket = + subcontext.output_port_tickets()[subsystem_oport_index]; + DependencyTracker& subsystem_oport_tracker = + subcontext.get_mutable_tracker(subsystem_oport_ticket); + + // Get the diagram's output port tracker that is the destination. + const DependencyTicket oport_ticket = + this->output_port_tickets()[oport_index]; + DependencyTracker& oport_tracker = + this->get_mutable_tracker(oport_ticket); + + oport_tracker.SubscribeToPrerequisite(&subsystem_oport_tracker); + } + + /// (Internal use only) Declares that a connection exists between a peer + /// output port and input port in this Diagram, records that fact in the + /// connection map, and registers the input port's dependency + /// tracker with the output port's dependency tracker. By "peer" we mean that + /// both ports belong to immediate child subsystems of this Diagram (it is + /// also possible for both ports to belong to the same subsystem). + /// + /// User code should not call this method. It is for use during Diagram + /// context allocation and cloning only. + void Connect(const OutputPortIdentifier& oport, + const InputPortIdentifier& iport) { + // Identify and validate the source output port. + SubsystemIndex oport_system_index = oport.first; + OutputPortIndex oport_index = oport.second; + Context& oport_context = GetMutableSubsystemContext(oport_system_index); + DRAKE_DEMAND(oport_index >= 0); + DRAKE_DEMAND(oport_index < oport_context.get_num_output_ports()); + + // Identify and validate the destination input port. + SubsystemIndex iport_system_index = iport.first; + InputPortIndex iport_index = iport.second; + Context& iport_context = GetMutableSubsystemContext(iport_system_index); + DRAKE_DEMAND(iport_index >= 0); + DRAKE_DEMAND(iport_index < iport_context.get_num_input_ports()); + + DependencyTicket oport_ticket = + oport_context.output_port_tickets()[oport_index]; + DependencyTicket iport_ticket = + iport_context.input_port_tickets()[iport_index]; + DependencyTracker& oport_tracker = + oport_context.get_mutable_tracker(oport_ticket); + DependencyTracker& iport_tracker = + iport_context.get_mutable_tracker(iport_ticket); + iport_tracker.SubscribeToPrerequisite(&oport_tracker); } /// Generates the state vector for the entire diagram by wrapping the states @@ -207,7 +268,8 @@ class DiagramContext final : public Context { auto state = std::make_unique>(num_subcontexts()); for (SubsystemIndex i(0); i < num_subcontexts(); ++i) { Context* context = contexts_[i].get(); - state->set_substate(i, &context->get_mutable_state()); + // Using `access` here to avoid sending invalidations. + state->set_substate(i, &context->access_mutable_state()); } state->Finalize(); state_ = std::move(state); @@ -233,22 +295,13 @@ class DiagramContext final : public Context { &subparams.get_mutable_abstract_parameter(i)); } } - parameters_ = std::make_unique>(); - parameters_->set_numeric_parameters( + Parameters& params = this->get_mutable_parameters(); + params.set_numeric_parameters( std::make_unique>(numeric_params)); - parameters_->set_abstract_parameters( + params.set_abstract_parameters( std::make_unique(abstract_params)); } - /// Returns the output structure for a given constituent system at @p index. - /// Aborts if @p index is out of bounds, or if no system has been added to the - /// DiagramContext at that index. - SystemOutput* GetSubsystemOutput(SubsystemIndex index) const { - DRAKE_DEMAND(index >= 0 && index < num_subcontexts()); - DRAKE_DEMAND(outputs_[index] != nullptr); - return outputs_[index].get(); - } - /// Returns the context structure for a given constituent system @p index. /// Aborts if @p index is out of bounds, or if no system has been added to the /// DiagramContext at that index. @@ -269,62 +322,18 @@ class DiagramContext final : public Context { return *contexts_[index].get(); } - /// Recursively sets the time on this context and all subcontexts. - void set_time(const T& time_sec) override { - Context::set_time(time_sec); - for (auto& subcontext : contexts_) { - if (subcontext != nullptr) { - subcontext->set_time(time_sec); - } - } - } - - /// Recursively sets the accuracy on this context and all subcontexts, - /// overwriting any accuracy value set in any subcontexts. - void set_accuracy(const optional& accuracy) override { - Context::set_accuracy(accuracy); - for (auto& subcontext : contexts_) { - if (subcontext != nullptr) { - subcontext->set_accuracy(accuracy); - } - } - } - - const State& get_state() const final { - DRAKE_ASSERT(state_ != nullptr); - return *state_; - } - - State& get_mutable_state() final { - DRAKE_ASSERT(state_ != nullptr); - return *state_; - } - - const Parameters& get_parameters() const final { - return *parameters_; - } - - Parameters& get_mutable_parameters() final { - return *parameters_; - } - protected: /// Protected copy constructor takes care of the local data members and /// all base class members, but doesn't update base class pointers so is /// not a complete copy. DiagramContext(const DiagramContext& source) : Context(source), - outputs_(source.num_subcontexts()), contexts_(source.num_subcontexts()), state_(std::make_unique>(source.num_subcontexts())) { // Clone all the subsystem contexts and outputs. - for (SubsystemIndex i(0); i < source.num_subcontexts(); ++i) { + for (SubsystemIndex i(0); i < num_subcontexts(); ++i) { DRAKE_DEMAND(source.contexts_[i] != nullptr); - DRAKE_DEMAND(source.outputs_[i] != nullptr); - // When a leaf context is cloned, it will clone the data that currently - // appears on each of its input ports into a FixedInputPortValue. - AddSystem(i, Context::CloneWithoutPointers(*source.contexts_[i]), - source.outputs_[i]->Clone()); + AddSystem(i, Context::CloneWithoutPointers(*source.contexts_[i])); } // Build a superstate over the subsystem contexts. @@ -353,23 +362,62 @@ class DiagramContext final : public Context { return clone; } - int num_subcontexts() const { - DRAKE_ASSERT(contexts_.size() == outputs_.size()); - return static_cast(contexts_.size()); + const State& do_access_state() const final { + DRAKE_ASSERT(state_ != nullptr); + return *state_; + } + + State& do_access_mutable_state() final { + DRAKE_ASSERT(state_ != nullptr); + return *state_; + } + + // Recursively sets the time on all subcontexts. + void DoPropagateTimeChange(const T& time_sec, int64_t change_event) final { + for (auto& subcontext : contexts_) { + DRAKE_ASSERT(subcontext != nullptr); + subcontext->PropagateTimeChange(time_sec, change_event); + } + } + + // Recursively sets the accuracy on all subcontexts. + void DoPropagateAccuracyChange(const optional& accuracy, + int64_t change_event) final { + for (auto& subcontext : contexts_) { + DRAKE_ASSERT(subcontext != nullptr); + subcontext->PropagateAccuracyChange(accuracy, change_event); + } + } + + // Recursively notifies subcontexts of bulk changes. + void DoPropagateBulkChange( + int64_t change_event, + void (ContextBase::*NoteBulkChange)(int64_t change_event)) { + for (auto& subcontext : contexts_) { + DRAKE_ASSERT(subcontext != nullptr); + subcontext->PropagateBulkChange(change_event, NoteBulkChange); + } + } + + int do_num_subcontexts() const final { + return num_subcontexts(); // That is, the local one. + } + + // Note the covariant return type here, instead of ContextBase&. That allows + // us to use this locally to get a Context without downcasting. + const Context& do_get_subcontext( + SubsystemIndex index) const final { + DRAKE_ASSERT(index >= 0 && index < num_subcontexts()); + DRAKE_ASSERT(contexts_[index] != nullptr); + return *contexts_[index].get(); } - // The outputs are stored in SubsystemIndex order, and outputs_ is equal in - // length to the number of subsystems specified at construction time. - std::vector>> outputs_; // The contexts are stored in SubsystemIndex order, and contexts_ is equal in // length to the number of subsystems specified at construction time. std::vector>> contexts_; // The internal state of the Diagram, which includes all its subsystem states. std::unique_ptr> state_; - - // The parameters of the Diagram, which includes all subsystem parameters. - std::unique_ptr> parameters_; }; } // namespace systems diff --git a/systems/framework/diagram_output_port.h b/systems/framework/diagram_output_port.h index 9e452535b070..900b60e6b991 100644 --- a/systems/framework/diagram_output_port.h +++ b/systems/framework/diagram_output_port.h @@ -1,9 +1,11 @@ #pragma once #include +#include #include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" +#include "drake/common/drake_optional.h" #include "drake/systems/framework/diagram_context.h" #include "drake/systems/framework/framework_common.h" #include "drake/systems/framework/output_port.h" @@ -34,14 +36,16 @@ class DiagramOutputPort final : public OutputPort { output port of one of the diagram's child subsystems. @param diagram The Diagram that will own this port. - @param system_base The same Diagram cast to its messaging interface. + @param system_base The same Diagram cast to its base class. @param index The output port index to be assigned to the new port. + @param ticket The DependencyTicket to be assigned to the new port. @param source_output_port An output port of one of this diagram's child subsystems that is to be forwarded to the new port. @param source_subsystem_index The index of the child subsystem that owns `source_output_port`. @pre The `diagram` System must actually be a Diagram. + @pre `diagram` lifetime must exceed the port's; we retain the pointer here. @pre `diagram` and `system_base` must be the same object. @pre `source_output_port` must be owned by a child of `diagram`. @pre `source_subsystem_index` must be the index of that child in `diagram`. @@ -53,17 +57,19 @@ class DiagramOutputPort final : public OutputPort { // doesn't have access to a declaration of Diagram so would not be able // to static_cast up to System as is required by OutputPort. We expect // the caller to do that cast for us so take a System here. - DiagramOutputPort(const System& diagram, - const internal::SystemMessageInterface& system_base, + DiagramOutputPort(const System* diagram, + SystemBase* system_base, OutputPortIndex index, + DependencyTicket ticket, const OutputPort* source_output_port, SubsystemIndex source_subsystem_index) - : OutputPort(diagram, system_base, index, + : OutputPort(diagram, system_base, index, ticket, source_output_port->get_data_type(), source_output_port->size()), source_output_port_(source_output_port), source_subsystem_index_(source_subsystem_index) { - DRAKE_DEMAND(index.is_valid() && source_subsystem_index.is_valid()); + DRAKE_DEMAND(index.is_valid() && ticket.is_valid()); + DRAKE_DEMAND(source_subsystem_index.is_valid()); DRAKE_DEMAND(source_output_port != nullptr); } @@ -94,9 +100,17 @@ class DiagramOutputPort final : public OutputPort { // delegates to the source output port. const AbstractValue& DoEval(const Context& diagram_context) const final { const Context& subcontext = get_subcontext(diagram_context); - return source_output_port_->Eval(subcontext); + return source_output_port_->EvalAbstract(subcontext); } + // Returns the source output port's subsystem, and the ticket for that + // port's tracker. + std::pair, DependencyTicket> + DoGetPrerequisite() const final { + return std::make_pair(source_subsystem_index_, + source_output_port_->ticket()); + }; + // Digs out the right subcontext for delegation. const Context& get_subcontext(const Context& diagram_context) const { const DiagramContext* diagram_context_downcast = diff --git a/systems/framework/fixed_input_port_value.cc b/systems/framework/fixed_input_port_value.cc index 9a2f144b842a..69e512281ed3 100644 --- a/systems/framework/fixed_input_port_value.cc +++ b/systems/framework/fixed_input_port_value.cc @@ -1,10 +1,15 @@ #include "drake/systems/framework/fixed_input_port_value.h" +#include "drake/systems/framework/context_base.h" + namespace drake { namespace systems { AbstractValue* FixedInputPortValue::GetMutableData() { - // TODO(sherm1) Change event tracking goes here. + ContextBase& context = get_mutable_owning_context(); + const DependencyTracker& tracker = context.get_tracker(ticket_); + const int64_t change_event = context.start_new_change_event(); + tracker.NoteValueChange(change_event); ++serial_number_; return value_.get_mutable(); } diff --git a/systems/framework/fixed_input_port_value.h b/systems/framework/fixed_input_port_value.h index b553868f2ccf..afc2eb065f16 100644 --- a/systems/framework/fixed_input_port_value.h +++ b/systems/framework/fixed_input_port_value.h @@ -96,6 +96,9 @@ class FixedInputPortValue { time the contained value changes, or when mutable access is granted. */ int64_t serial_number() const { return serial_number_; } + /** Returns the ticket used to find the associated DependencyTracker. */ + DependencyTicket ticket() const { return ticket_; } + /** Returns a const reference to the context that owns this object. */ const ContextBase& get_owning_context() const { DRAKE_ASSERT(owning_subcontext_ != nullptr); @@ -111,10 +114,21 @@ class FixedInputPortValue { // Copy constructor is only used for cloning and is not a complete copy -- // owning_subcontext_ is left unassigned. - FixedInputPortValue(const FixedInputPortValue& source) = - default; + FixedInputPortValue(const FixedInputPortValue& source) = default; + + /** Returns a mutable reference to the context that owns this object. */ + ContextBase& get_mutable_owning_context() { + DRAKE_ASSERT(owning_subcontext_ != nullptr); + return *owning_subcontext_; + } - // Informs this FixedInputPortValue of the subcontext that owns it. + /** (Internal use only) */ + // Informs this FixedInputPortValue of its assigned DependencyTracker + // so it knows who to notify when its value changes. + void set_ticket(DependencyTicket ticket) { ticket_ = ticket; } + + /** (Internal use only) */ + // Informs this %FixedInputPortValue of the subcontext that owns it. // Aborts if this has already been done or given bad args. void set_owning_subcontext(ContextBase* owning_subcontext) { DRAKE_DEMAND(owning_subcontext != nullptr && owning_subcontext_ == nullptr); @@ -134,6 +148,10 @@ class FixedInputPortValue { // recorded the number somewhere. If the serial number matches, the value // is either unchanged since you last saw it, or an identical copy. int64_t serial_number_{-1}; + + // Index of the dependency tracker for this fixed value. The input port + // should have registered with this tracker. + DependencyTicket ticket_; }; // TODO(sherm1) Get rid of this after 8/7/2018 (three months). @@ -157,6 +175,12 @@ class ContextBaseFixedInputAttorney { DRAKE_DEMAND(owning_subcontext != nullptr && fixed != nullptr); fixed->set_owning_subcontext(owning_subcontext); } + + static void set_ticket(FixedInputPortValue* fixed, + DependencyTicket ticket) { + DRAKE_DEMAND(ticket.is_valid() && fixed != nullptr); + fixed->set_ticket(ticket); + } }; } // namespace detail diff --git a/systems/framework/leaf_context.h b/systems/framework/leaf_context.h index 5c5711d42e46..8c315abcb386 100644 --- a/systems/framework/leaf_context.h +++ b/systems/framework/leaf_context.h @@ -16,11 +16,28 @@ namespace drake { namespace systems { -/// %LeafContext contains all prerequisite data necessary to uniquely determine -/// the results of computations performed by the associated LeafSystem. -/// -/// @tparam T The mathematical type of the context, which must be a valid Eigen -/// scalar. +/** %LeafContext contains all prerequisite data necessary to uniquely determine +the results of computations performed by the associated LeafSystem. It also +contains mutable cache for holding the results of those computations, and +infrastructure for ensuring that those results are up to date with their +prerequisites. Here is an inventory: + +##### Source data +- Time t, accuracy a (from base class) +- %State values x +- %Parameter values p +- %Value sources for input ports u + - Stored values for locally-fixed input ports + - References to value sources for externally-connected input ports + +##### Cached computations +- Output port values +- %State derivatives and discrete update values +- Constraint errors +- User-specified cached computations + +@tparam T The mathematical type of the context, which must be a valid Eigen + scalar. */ template class LeafContext : public Context { public: @@ -33,38 +50,9 @@ class LeafContext : public Context { //@} LeafContext() - : state_(std::make_unique>()), - parameters_(std::make_unique>()) {} + : state_(std::make_unique>()) {} ~LeafContext() override {} - const State& get_state() const final { - DRAKE_ASSERT(state_ != nullptr); - return *state_; - } - - State& get_mutable_state() final { - DRAKE_ASSERT(state_ != nullptr); - return *state_.get(); - } - - // ========================================================================= - // Accessors and Mutators for Parameters. - - /// Sets the parameters to @p params, deleting whatever was there before. - void set_parameters(std::unique_ptr> params) { - parameters_ = std::move(params); - } - - /// Returns the entire Parameters object. - const Parameters& get_parameters() const final { - return *parameters_; - } - - /// Returns the entire Parameters object. - Parameters& get_mutable_parameters() final { - return *parameters_; - } - protected: /// Protected copy constructor takes care of the local data members and /// all base class members, but doesn't update base class pointers so is @@ -73,9 +61,6 @@ class LeafContext : public Context { // Make a deep copy of the state. state_ = source.CloneState(); - // Make deep copies of the parameters. - set_parameters(source.parameters_->Clone()); - // Everything else was handled by the Context copy constructor. } @@ -99,18 +84,25 @@ class LeafContext : public Context { xc_vector.Clone(), num_q, num_v, num_z)); // Make deep copies of the discrete and abstract states. - clone->set_discrete_state(get_state().get_discrete_state().Clone()); - clone->set_abstract_state(get_state().get_abstract_state().Clone()); + clone->set_discrete_state(state_->get_discrete_state().Clone()); + clone->set_abstract_state(state_->get_abstract_state().Clone()); return clone; } private: - // The internal state of the System. - std::unique_ptr> state_; + const State& do_access_state() const final { + DRAKE_ASSERT(state_ != nullptr); + return *state_; + } + + State& do_access_mutable_state() final { + DRAKE_ASSERT(state_ != nullptr); + return *state_.get(); + } - // The parameters of the system. - std::unique_ptr> parameters_; + // The internal state x of this LeafSystem. + std::unique_ptr> state_; }; } // namespace systems diff --git a/systems/framework/leaf_output_port.h b/systems/framework/leaf_output_port.h index 234c9dda5416..382c49b2f538 100644 --- a/systems/framework/leaf_output_port.h +++ b/systems/framework/leaf_output_port.h @@ -9,22 +9,16 @@ #include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" -#include "drake/common/never_destroyed.h" #include "drake/common/nice_type_name.h" #include "drake/systems/framework/basic_vector.h" #include "drake/systems/framework/framework_common.h" #include "drake/systems/framework/output_port.h" +#include "drake/systems/framework/system_base.h" #include "drake/systems/framework/value.h" namespace drake { namespace systems { -template -class System; - -template -class Context; - /** Adds methods for allocation, calculation, and caching of an output port's value. When created, an allocation and a calculation function must be provided. The output type of the calculation callback must match the type @@ -39,9 +33,8 @@ Instantiated templates for the following kinds of T's are provided: They are already available to link against in the containing library. No other values for T are currently supported. */ -// TODO(sherm1) Implement caching for output ports. template -class LeafOutputPort : public OutputPort { +class LeafOutputPort final : public OutputPort { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(LeafOutputPort) @@ -74,17 +67,15 @@ class LeafOutputPort : public OutputPort { concrete type as is returned by the allocator. The allocator function is not invoked here during construction of the port so it may depend on data that becomes available only after completion of the containing System or - Diagram. The `system` parameter must be the same object as the `system_base` - parameter. */ - LeafOutputPort(const System& system, - const internal::SystemMessageInterface& system_base, - OutputPortIndex index, - AllocCallback alloc_function, - CalcCallback calc_function) - : OutputPort(system, system_base, index, kAbstractValued, + Diagram. */ + LeafOutputPort(const System* system, SystemBase* system_base, + OutputPortIndex index, DependencyTicket ticket, + AllocCallback alloc_function, CalcCallback calc_function, + std::vector calc_prerequisites) + : OutputPort(system, system_base, index, ticket, kAbstractValued, 0 /* size */) { - set_allocation_function(alloc_function); - set_calculation_function(calc_function); + CompleteConstruction(std::move(alloc_function), std::move(calc_function), + std::move(calc_prerequisites)); } /** Constructs a fixed-size vector-valued output port. The supplied allocator @@ -93,21 +84,21 @@ class LeafOutputPort : public OutputPort { same underlying concrete type as is returned by the allocator. Requires the fixed size to be given explicitly here. The allocator function is not invoked here during construction of the port so it may depend on data that becomes - available only after completion of the containing System or Diagram. The - `system` parameter must be the same object as the `system_base` parameter. */ + available only after completion of the containing System or Diagram. */ // Note: there is no guarantee that the allocator can be invoked successfully // here since construction of the containing System is likely incomplete when // this method is invoked. Do not attempt to extract the size from // the allocator by calling it here. - LeafOutputPort(const System& system, - const internal::SystemMessageInterface& system_base, - OutputPortIndex index, - int fixed_size, - AllocCallback vector_alloc_function, - CalcVectorCallback vector_calc_function) - : OutputPort(system, system_base, index, kVectorValued, fixed_size) { - set_allocation_function(vector_alloc_function); - set_calculation_function(vector_calc_function); + LeafOutputPort(const System* system, SystemBase* system_base, + OutputPortIndex index, DependencyTicket ticket, int fixed_size, + AllocCallback alloc_function, + CalcVectorCallback vector_calc_function, + std::vector calc_prerequisites) + : OutputPort(system, system_base, index, ticket, kVectorValued, + fixed_size) { + CompleteConstruction(std::move(alloc_function), + ConvertVectorCallback(std::move(vector_calc_function)), + std::move(calc_prerequisites)); } /** (Advanced) Sets or replaces the evaluation function for this output @@ -119,90 +110,140 @@ class LeafOutputPort : public OutputPort { eval_function_ = eval_function; } - private: - // Sets or replaces the allocation function for this output port, using - // a function that returns an AbstractValue. - void set_allocation_function(AllocCallback alloc_function) { - alloc_function_ = alloc_function; - } - - // Sets or replaces the calculation function for this output port, using - // a function that writes into an `AbstractValue`. - void set_calculation_function(CalcCallback calc_function) { - calc_function_ = calc_function; - } + /** Returns the cache entry associated with this output port. */ + CacheIndex cache_index() const { return cache_index_; } - // Sets or replaces the calculation function for this vector-valued output + private: + // Creates an AbstractValue calculation function for this vector-valued output // port, using a function that writes into a `BasicVector`. - void set_calculation_function(CalcVectorCallback vector_calc_function) { - if (!vector_calc_function) { - calc_function_ = nullptr; - } else { - // Wrap the vector-writing function with an AbstractValue-writing - // function. - calc_function_ = [this, vector_calc_function](const Context& context, - AbstractValue* abstract) { - // The abstract value must be a Value>. - auto value = dynamic_cast>*>(abstract); - if (value == nullptr) { - std::ostringstream oss; - oss << "LeafOutputPort::Calc(): Expected a vector output type for " - << this->GetPortIdString() << " but got a " - << NiceTypeName::Get(*abstract) << " instead."; - throw std::logic_error(oss.str()); - } - vector_calc_function(context, &value->get_mutable_value()); - }; - } - } + CalcCallback ConvertVectorCallback(CalcVectorCallback vector_calc_function); + + // Handles common constructor tasks. + void CompleteConstruction( + AllocCallback alloc_function, CalcCallback calc_function, + std::vector calc_prerequisites); // Invokes the supplied allocation function if there is one, otherwise // complains. - std::unique_ptr DoAllocate() const final { - std::unique_ptr result; - - if (alloc_function_) { - result = alloc_function_(); - } else { - throw std::logic_error( - "LeafOutputPort::DoAllocate(): " + this->GetPortIdString() + - " has no allocation function so cannot be allocated."); - } - if (result.get() == nullptr) { - throw std::logic_error( - "LeafOutputPort::DoAllocate(): allocator returned a nullptr for " + - this->GetPortIdString()); - } - return result; - } + std::unique_ptr DoAllocate() const final; // Invokes the supplied calculation function if present, otherwise complains. - void DoCalc(const Context& context, AbstractValue* value) const final { - if (calc_function_) { - calc_function_(context, value); - } else { - throw std::logic_error("LeafOutputPort::DoCalcWitnessValue(): " + - this->GetPortIdString() + - " had no calculation function available."); - } - } + void DoCalc(const Context& context, AbstractValue* value) const final; // Currently just invokes the supplied evaluation function if present, - // otherwise complains. - // TODO(sherm1) Generate this automatically using the cache & DoEvaluate(). - const AbstractValue& DoEval(const Context& context) const final { - if (eval_function_) return eval_function_(context); - // TODO(sherm1) Provide proper default behavior for an output port with - // its own cache entry. - DRAKE_ABORT_MSG("LeafOutputPort::DoEval(): NOT IMPLEMENTED YET"); - static never_destroyed> dummy{0}; - return dummy.access(); - } + // otherwise forwards to this port's cache entry. + const AbstractValue& DoEval(const Context& context) const final; + + // Returns the cache entry's ticket and no subsystem. + std::pair, DependencyTicket> DoGetPrerequisite() + const final; AllocCallback alloc_function_; CalcCallback calc_function_; EvalCallback eval_function_; + + CacheIndex cache_index_; }; +// See diagram.h for DiagramOutputPort. + +template +auto LeafOutputPort::ConvertVectorCallback( + CalcVectorCallback vector_calc_function) -> CalcCallback { + if (!vector_calc_function) return nullptr; + + // Wrap the vector-writing function with an AbstractValue-writing function. + return [this, vector_calc_function](const Context& context, + AbstractValue* abstract) { + // The abstract value must be a Value>. + auto value = dynamic_cast>*>(abstract); + if (value == nullptr) { + std::ostringstream oss; + oss << "LeafOutputPort::Calc(): Expected a vector output type for " + << this->GetPortIdString() << " but got a " + << NiceTypeName::Get(*abstract) << " instead."; + throw std::logic_error(oss.str()); + } + vector_calc_function(context, &value->get_mutable_value()); + }; +} + +// Private method to be called from cached output port constructors, after the +// base class construction is complete. Records the functors and Calc() +// prerequisites, and allocates an appropriate cache entry. +template +void LeafOutputPort::CompleteConstruction( + AllocCallback alloc_function, CalcCallback calc_function, + std::vector calc_prerequisites) { + alloc_function_ = std::move(alloc_function); + calc_function_ = std::move(calc_function); + + if (calc_prerequisites.empty()) + calc_prerequisites.push_back(this->get_system_base().all_sources_ticket()); + + auto cache_alloc_function = [alloc = alloc_function_]() { return alloc(); }; + auto cache_calc_function = [calc = calc_function_]( + const ContextBase& context_base, AbstractValue* result) { + const Context& context = dynamic_cast&>(context_base); + return calc(context, result); + }; + cache_index_ = + this->get_mutable_system_base() + .DeclareCacheEntry( + "output port " + std::to_string(this->get_index()) + " cache", + std::move(cache_alloc_function), std::move(cache_calc_function), + std::move(calc_prerequisites)) + .cache_index(); +} + +template +std::unique_ptr LeafOutputPort::DoAllocate() const { + std::unique_ptr result; + + if (alloc_function_) { + result = alloc_function_(); + } else { + throw std::logic_error( + "LeafOutputPort::DoAllocate(): " + this->GetPortIdString() + + " has no allocation function so cannot be allocated."); + } + if (result.get() == nullptr) { + throw std::logic_error( + "LeafOutputPort::DoAllocate(): allocator returned a nullptr for " + + this->GetPortIdString()); + } + return result; +} + +template +void LeafOutputPort::DoCalc(const Context& context, + AbstractValue* value) const { + if (calc_function_) { + calc_function_(context, value); + } else { + throw std::logic_error("LeafOutputPort::DoCalcWitnessValue(): " + + this->GetPortIdString() + + " had no calculation function available."); + } +} + +template +const AbstractValue& LeafOutputPort::DoEval( + const Context& context) const { + if (eval_function_) + return eval_function_(context); + const CacheEntry& entry = + this->get_system_base().get_cache_entry(cache_index_); + return entry.EvalAbstract(context); +} + +template +std::pair, DependencyTicket> +LeafOutputPort::DoGetPrerequisite() const { + const CacheEntry& entry = + this->get_system_base().get_cache_entry(cache_index_); + return std::make_pair(nullopt, entry.ticket()); +} + } // namespace systems } // namespace drake diff --git a/systems/framework/leaf_system.h b/systems/framework/leaf_system.h index b1bb100debe1..f2bc0891fcf9 100644 --- a/systems/framework/leaf_system.h +++ b/systems/framework/leaf_system.h @@ -14,7 +14,6 @@ #include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" #include "drake/common/drake_deprecated.h" -#include "drake/common/drake_optional.h" #include "drake/common/eigen_types.h" #include "drake/common/number_traits.h" #include "drake/common/pointer_cast.h" @@ -128,18 +127,22 @@ class LeafSystem : public System { std::unique_ptr DoMakeContext() const final { std::unique_ptr> context = DoMakeLeafContext(); + return std::move(context); + } + + // Caller contract guarantees non null, compatible Context. + void DoAcquireContextResources(ContextBase* context_base) const final { + auto& context = dynamic_cast&>(*context_base); // Reserve continuous state via delegation to subclass. - context->set_continuous_state(this->AllocateContinuousState()); + context.set_continuous_state(this->AllocateContinuousState()); // Reserve discrete state via delegation to subclass. - context->set_discrete_state(this->AllocateDiscreteState()); - context->set_abstract_state(this->AllocateAbstractState()); + context.set_discrete_state(this->AllocateDiscreteState()); + context.set_abstract_state(this->AllocateAbstractState()); // Reserve parameters via delegation to subclass. - context->set_parameters(this->AllocateParameters()); - - // Note that the outputs are not part of the Context, but instead are - // checked by LeafSystemOutput::add_port. + context.set_parameters(this->AllocateParameters()); - return context; + // Allow derived LeafSystem to acquire Context resources. + DoAcquireLeafContextResources(&context); } // Enforce some requirements on the fully-assembled Context. @@ -217,16 +220,6 @@ class LeafSystem : public System { } } - std::unique_ptr> AllocateOutput( - const Context&) const final { - std::unique_ptr> output(new LeafSystemOutput); - for (int i = 0; i < this->get_num_output_ports(); ++i) { - const OutputPort& port = this->get_output_port(i); - output->add_port(std::make_unique(port.Allocate())); - } - return std::move(output); - } - /// Returns the AllocateContinuousState value, which must not be nullptr. std::unique_ptr> AllocateTimeDerivatives() const override { return AllocateContinuousState(); @@ -317,6 +310,13 @@ class LeafSystem : public System { return std::make_unique>(); } + /// Derived classes that need their own resource allocation beyond the + /// SystemBase- and LeafSystem-acquired ones should implement this. The + /// default implementation does nothing. + virtual void DoAcquireLeafContextResources(LeafContext* context) const { + unused(context); + } + /// Derived classes that impose restrictions on what resources are permitted /// should check those restrictions by implementing this. For example, a /// derived class might require a single input and single output. The default @@ -1552,8 +1552,10 @@ class LeafSystem : public System { typename LeafOutputPort::AllocCallback vector_allocator, typename LeafOutputPort::CalcVectorCallback vector_calculator) { auto port = std::make_unique>( - *this, *this, OutputPortIndex(this->get_num_output_ports()), - fixed_size, vector_allocator, vector_calculator); + static_cast*>(this), static_cast(this), + OutputPortIndex(this->get_num_output_ports()), + this->assign_next_dependency_ticket(), fixed_size, vector_allocator, + vector_calculator, std::vector{}); LeafOutputPort* const port_ptr = port.get(); this->CreateOutputPort(std::move(port)); return *port_ptr; @@ -1565,8 +1567,10 @@ class LeafSystem : public System { typename LeafOutputPort::AllocCallback allocator, typename LeafOutputPort::CalcCallback calculator) { auto port = std::make_unique>( - *this, *this, OutputPortIndex(this->get_num_output_ports()), - allocator, calculator); + static_cast*>(this), static_cast(this), + OutputPortIndex(this->get_num_output_ports()), + this->assign_next_dependency_ticket(), allocator, calculator, + std::vector{}); LeafOutputPort* const port_ptr = port.get(); this->CreateOutputPort(std::move(port)); return *port_ptr; diff --git a/systems/framework/output_port.h b/systems/framework/output_port.h index 419667400058..c61b9a31b461 100644 --- a/systems/framework/output_port.h +++ b/systems/framework/output_port.h @@ -4,7 +4,6 @@ #include #include #include -#include #include #include @@ -15,6 +14,8 @@ #include "drake/systems/framework/basic_vector.h" #include "drake/systems/framework/context.h" #include "drake/systems/framework/framework_common.h" +#include "drake/systems/framework/output_port_base.h" +#include "drake/systems/framework/system_base.h" #include "drake/systems/framework/value.h" namespace drake { @@ -66,11 +67,24 @@ No other values for T are currently supported. */ // TODO(sherm1) Implement caching for output ports and update the above // documentation to explain in more detail. template -class OutputPort { +class OutputPort : public OutputPortBase { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OutputPort) - virtual ~OutputPort() = default; + ~OutputPort() override = default; + + /** Returns a reference to the up-to-date value of this output port contained + in the given Context. This is the preferred way to obtain an output port's + value since it will not be recalculated once up to date. If the value is not + already up to date with respect to its prerequisites, this port's Calc() + method is used first to update the value before the reference is returned. The + Calc() method may be arbitrarily expensive, but Eval() is constant time and + _very_ fast if the value is already up to date. */ + template + const ValueType& Eval(const Context& context) const { + const AbstractValue& abstract_value = EvalAbstract(context); + return ExtractValueOrThrow(abstract_value, __func__); + } /** Allocates a concrete object suitable for holding the value to be exposed by this output port, and returns that as an AbstractValue. The returned object @@ -98,7 +112,8 @@ class OutputPort { the Allocate() method. */ void Calc(const Context& context, AbstractValue* value) const { DRAKE_DEMAND(value != nullptr); - DRAKE_ASSERT_VOID(system_base_.ThrowIfContextNotCompatible(context)); + DRAKE_ASSERT_VOID( + get_system_base().ThrowIfContextNotCompatible(context)); DRAKE_ASSERT_VOID(CheckValidOutputType(*value)); DoCalc(context, value); @@ -107,10 +122,10 @@ class OutputPort { /** Returns a reference to the value of this output port contained in the given Context. If that value is not up to date with respect to its prerequisites, the Calc() method above is used first to update the value - before the reference is returned. (Not implemented yet.) */ - // TODO(sherm1) Implement properly. - const AbstractValue& Eval(const Context& context) const { - DRAKE_ASSERT_VOID(system_base_.ThrowIfContextNotCompatible(context)); + before the reference is returned. */ + const AbstractValue& EvalAbstract(const Context& context) const { + DRAKE_ASSERT_VOID( + get_system_base().ThrowIfContextNotCompatible(context)); return DoEval(context); } @@ -121,49 +136,20 @@ class OutputPort { return system_; } - /** Returns the index of this output port within the owning System. */ - OutputPortIndex get_index() const { - return index_; - } - - /** Gets the port data type specified at port construction. */ - PortDataType get_data_type() const { return data_type_; } - - /** Returns the fixed size expected for a vector-valued output port. Not - meaningful for abstract output ports. */ - int size() const { return size_; } - protected: /** Provides derived classes the ability to set the base class members at - construction. - @param system - The System that will own this new output port (as forward-declared class). - @param system_base - The System that will own this new output port (as pure virtual interface). - @param index - The index of this port within its owning System. - @param data_type - Whether the port described is vector or abstract valued. - @param size - If the port described is vector-valued, the number of elements expected, - otherwise ignored. - @pre The `system` parameter must be the same object as the `system_base` - parameter. */ + construction. See OutputPortBase::OutputPortBase() for the meaning of these + parameters. */ // The System and SystemBase are provided separately since we don't have // access to System's declaration here so can't cast but the caller can. - OutputPort( - const System& system, - const internal::SystemMessageInterface& system_base, - OutputPortIndex index, PortDataType data_type, int size) - : system_(system), - system_base_(system_base), - index_(index), - data_type_(data_type), - size_(size) { - DRAKE_DEMAND(static_cast(&system) == &system_base); - if (size_ == kAutoSize) { - DRAKE_ABORT_MSG("Auto-size ports are not yet implemented."); - } + // They must refer to the same System. + OutputPort(const System* system, + SystemBase* system_base, + OutputPortIndex index, + DependencyTicket ticket, PortDataType data_type, int size) + : OutputPortBase(system_base, index, ticket, data_type, size), + system_{*system} { + DRAKE_DEMAND(static_cast(system) == system_base); } /** A concrete %OutputPort must provide a way to allocate a suitable object @@ -195,95 +181,133 @@ class OutputPort { /** This is useful for error messages and produces a human-readable identification of an offending output port. */ std::string GetPortIdString() const { - std::ostringstream oss; - oss << "output port " << this->get_index() << " of " - << NiceTypeName::Get(system_base_) + " System " + - system_base_.GetSystemPathname(); - return oss.str(); + const std::string port_id = + "OutputPort[" + std::to_string(this->get_index()) + "] of subsystem " + + this->get_system_base().GetSystemPathname() + "(" + + NiceTypeName::RemoveNamespaces( + this->get_system_base().GetSystemType()) + + ")."; + return port_id; } private: // Check whether the allocator returned a value that is consistent with // this port's specification. - // If this is a vector-valued port, we can check that the returned abstract - // value actually holds a BasicVector-derived object, and for fixed-size ports - // that the object has the right size. - void CheckValidAllocation(const AbstractValue& proposed) const { - if (this->get_data_type() != kVectorValued) - return; // Nothing we can check for an abstract port. - - auto proposed_vec = dynamic_cast>*>(&proposed); - if (proposed_vec == nullptr) { - std::ostringstream oss; - oss << "Allocate(): expected BasicVector output type but got " - << NiceTypeName::Get(proposed) << " for " << GetPortIdString(); - throw std::logic_error(oss.str()); - } - - if (this->size() == kAutoSize) - return; // Any size is acceptable. - - const int proposed_size = proposed_vec->get_value().size(); - if (proposed_size != this->size()) { - std::ostringstream oss; - oss << "Allocate(): expected vector output type of size " << this->size() - << " but got a vector of size " << proposed_size - << " for " << GetPortIdString(); - throw std::logic_error(oss.str()); - } - } + void CheckValidAllocation(const AbstractValue&) const; // Check that an AbstractValue provided to Calc() is suitable for this port. // (Very expensive; use in Debug only.) - // See CacheEntry::CheckValidAbstractValue; treat both methods similarly. - void CheckValidOutputType(const AbstractValue& proposed) const { - // TODO(sherm1) Consider whether we can depend on there already being an - // object of this type in the output port's CacheEntryValue so we wouldn't - // have to allocate one here. If so could also store a precomputed - // type_index there for further savings. Would need to pass in a Context. - auto good = DoAllocate(); // Expensive! - // Attempt to interpret these as BasicVectors. - auto proposed_vec = dynamic_cast>*>(&proposed); - auto good_vec = dynamic_cast>*>(good.get()); - if (proposed_vec && good_vec) { - CheckValidBasicVector(good_vec->get_value(), - proposed_vec->get_value()); - } else { - // At least one is not a BasicVector. - CheckValidAbstractValue(*good, proposed); - } - } + void CheckValidOutputType(const AbstractValue&) const; // Check that both type-erased arguments have the same underlying type. void CheckValidAbstractValue(const AbstractValue& good, - const AbstractValue& proposed) const { - if (typeid(proposed) != typeid(good)) { - std::ostringstream oss; - oss << "Calc(): expected AbstractValue output type " - << NiceTypeName::Get(good) << " but got " - << NiceTypeName::Get(proposed) << " for " << GetPortIdString(); - throw std::logic_error(oss.str()); - } - } + const AbstractValue& proposed) const; // Check that both BasicVector arguments have the same underlying type. void CheckValidBasicVector(const BasicVector& good, - const BasicVector& proposed) const { - if (typeid(proposed) != typeid(good)) { - std::ostringstream oss; - oss << "Calc(): expected BasicVector output type " - << NiceTypeName::Get(good) << " but got " - << NiceTypeName::Get(proposed) << " for " << GetPortIdString(); - throw std::logic_error(oss.str()); - } + const BasicVector& proposed) const; + + // The user told us the output port would have a particular concrete type + // but it doesn't. + template + void ThrowBadValueType(const char* func_name, + const AbstractValue& abstract) const { + std::ostringstream msg; + msg << "OutputPort::" << func_name << "(): wrong value type <" + << NiceTypeName::Get() << "> specified but actual type was <" + << abstract.GetNiceTypeName() << ">." + << "\n-- " << GetPortIdString(); + throw std::logic_error(msg.str()); + } + + // Pull a value of a given type from an abstract value or issue a nice + // message if the type is not correct. + template + const ValueType& ExtractValueOrThrow(const AbstractValue& abstract, + const char* func_name) const { + const ValueType* value = abstract.MaybeGetValue(); + if (!value) + ThrowBadValueType(func_name, abstract); + return *value; } const System& system_; - const internal::SystemMessageInterface& system_base_; - const OutputPortIndex index_; - const PortDataType data_type_; - const int size_; }; +// If this is a vector-valued port, we can check that the returned abstract +// value actually holds a BasicVector-derived object, and for fixed-size ports +// that the object has the right size. +template +void OutputPort::CheckValidAllocation(const AbstractValue& proposed) const { + if (this->get_data_type() != kVectorValued) + return; // Nothing we can check for an abstract port. + + auto proposed_vec = dynamic_cast>*>(&proposed); + if (proposed_vec == nullptr) { + std::ostringstream oss; + oss << "Allocate(): expected BasicVector output type but got " + << NiceTypeName::Get(proposed) << " for " << GetPortIdString(); + throw std::logic_error(oss.str()); + } + + if (this->size() == kAutoSize) + return; // Any size is acceptable. + + const int proposed_size = proposed_vec->get_value().size(); + if (proposed_size != this->size()) { + std::ostringstream oss; + oss << "Allocate(): expected vector output type of size " << this->size() + << " but got a vector of size " << proposed_size + << " for " << GetPortIdString(); + throw std::logic_error(oss.str()); + } +} + +// Check that an AbstractValue provided to Calc() is suitable for this port. +// (Very expensive; use in Debug only.) +// See CacheEntry::CheckValidAbstractValue; treat both methods similarly. +template +void OutputPort::CheckValidOutputType(const AbstractValue& proposed) const { + // TODO(sherm1) Consider whether we can depend on there already being an + // object of this type in the output port's CacheEntryValue so we wouldn't + // have to allocate one here. If so could also store a precomputed + // type_index there for further savings. Would need to pass in a Context. + auto good = DoAllocate(); // Expensive! + // Attempt to interpret these as BasicVectors. + auto proposed_vec = dynamic_cast>*>(&proposed); + auto good_vec = dynamic_cast>*>(good.get()); + if (proposed_vec && good_vec) { + CheckValidBasicVector(good_vec->get_value(), proposed_vec->get_value()); + } else { + // At least one is not a BasicVector. + CheckValidAbstractValue(*good, proposed); + } +} + +// Check that both type-erased arguments have the same underlying type. +template +void OutputPort::CheckValidAbstractValue( + const AbstractValue& good, const AbstractValue& proposed) const { + if (typeid(proposed) != typeid(good)) { + std::ostringstream oss; + oss << "Calc(): expected AbstractValue output type " + << NiceTypeName::Get(good) << " but got " << NiceTypeName::Get(proposed) + << " for " << GetPortIdString(); + throw std::logic_error(oss.str()); + } +} + +template +void OutputPort::CheckValidBasicVector( + const BasicVector& good, const BasicVector& proposed) const { + if (typeid(proposed) != typeid(good)) { + std::ostringstream oss; + oss << "Calc(): expected BasicVector output type " + << NiceTypeName::Get(good) << " but got " << NiceTypeName::Get(proposed) + << " for " << GetPortIdString(); + throw std::logic_error(oss.str()); + } +} + } // namespace systems } // namespace drake diff --git a/systems/framework/output_port_base.cc b/systems/framework/output_port_base.cc new file mode 100644 index 000000000000..86b19dd1d904 --- /dev/null +++ b/systems/framework/output_port_base.cc @@ -0,0 +1,24 @@ +#include "drake/systems/framework/output_port_base.h" + +#include "drake/common/drake_assert.h" +#include "drake/systems/framework/framework_common.h" + +namespace drake { +namespace systems { + +OutputPortBase::OutputPortBase( + SystemBase* owning_subsystem, + OutputPortIndex index, DependencyTicket ticket, PortDataType data_type, + int size) + : owning_subsystem_(*owning_subsystem), + index_(index), + ticket_(ticket), + data_type_(data_type), + size_(size) { + DRAKE_DEMAND(owning_subsystem != nullptr); + if (size_ == kAutoSize) + DRAKE_ABORT_MSG("Auto-size ports are not yet implemented."); +} + +} // namespace systems +} // namespace drake diff --git a/systems/framework/output_port_base.h b/systems/framework/output_port_base.h new file mode 100644 index 000000000000..7696e6e62e58 --- /dev/null +++ b/systems/framework/output_port_base.h @@ -0,0 +1,106 @@ +#pragma once + +#include +#include + +#include "drake/common/drake_optional.h" +#include "drake/systems/framework/framework_common.h" + +namespace drake { +namespace systems { + +class SystemBase; + +/** An OutputPort belongs to a System and represents the properties of one of +that System's output ports. OutputPort objects are assigned OutputPortIndex +values in the order they are declared; these are unique within a single System. +A DependencyTicket is assigned so that downstream dependents can subscribe to +the value of this port. Also, each output port has a single prerequisite to +which it must be subscribed. For leaf systems, that prerequisite is a cache +entry that holds the most-recently-calculated port value. For diagrams, it is +the output port of a child subsystem which has been exported to be an output +of its parent. + +%OutputPortBase handles the scalar type-independent aspects of an OutputPort. */ +class OutputPortBase { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OutputPortBase) + + virtual ~OutputPortBase() = default; + + /** Returns the index of this output port within the owning System. For a + Diagram, this will be the index within the Diagram, _not_ the index within + the LeafSystem whose output port was forwarded. */ + OutputPortIndex get_index() const { + return index_; + } + + /** Returns the DependencyTicket for this output port within the owning + System. */ + DependencyTicket ticket() const { + return ticket_; + } + + /** Gets the port data type specified at port construction. */ + PortDataType get_data_type() const { return data_type_; } + + /** Returns the fixed size expected for a vector-valued output port. Not + meaningful for abstract output ports. */ + int size() const { return size_; } + + /** Returns a reference to the System that owns this output port. Note that + for a diagram output port this will be the diagram, not the leaf system whose + output port was forwarded. */ + const SystemBase& get_system_base() const { + return owning_subsystem_; + } + + /** Returns the prerequisite for this output port -- either a cache entry + in this System, or an output port of a child System. */ + std::pair, DependencyTicket> GetPrerequisite() + const { + return DoGetPrerequisite(); + }; + + protected: + /** Provides derived classes the ability to set the base class members at + construction. + + @param owning_subsystem + The System that owns this output port. + @param index + The index to be assigned to this OutputPort. + @param ticket + The DependencyTicket to be assigned to this OutputPort. + @param data_type + Whether the port described is vector or abstract valued. + @param size + If the port described is vector-valued, the number of elements expected, + otherwise ignored. */ + OutputPortBase(SystemBase* owning_subsystem, + OutputPortIndex index, DependencyTicket ticket, + PortDataType data_type, int size); + + SystemBase& get_mutable_system_base() { + return owning_subsystem_; + } + + /** Concrete output ports must implement this to return the prerequisite + dependency ticket for this port, which may be in the current System or one + of its immediate child subsystems. */ + virtual std::pair, DependencyTicket> + DoGetPrerequisite() const = 0; + + private: + // Associated System and System resources. + SystemBase& owning_subsystem_; + const OutputPortIndex index_; + const DependencyTicket ticket_; + + // Port details. + const PortDataType data_type_; + const int size_; +}; + +} // namespace systems +} // namespace drake diff --git a/systems/framework/output_port_value.cc b/systems/framework/output_port_value.cc index 8da4174df12d..118a8b1ef50a 100644 --- a/systems/framework/output_port_value.cc +++ b/systems/framework/output_port_value.cc @@ -2,23 +2,5 @@ #include "drake/common/default_scalars.h" -namespace drake { -namespace systems { - -OutputPortValue::~OutputPortValue() {} - -std::unique_ptr OutputPortValue::Clone() const { - if (data_ != nullptr) { - return std::make_unique(data_->Clone()); - } - return nullptr; -} - -} // namespace systems -} // namespace drake - DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( class ::drake::systems::SystemOutput) - -DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS( - class ::drake::systems::LeafSystemOutput) diff --git a/systems/framework/output_port_value.h b/systems/framework/output_port_value.h index b827ca358608..d41e9ac92eee 100644 --- a/systems/framework/output_port_value.h +++ b/systems/framework/output_port_value.h @@ -8,191 +8,88 @@ #include "drake/common/drake_assert.h" #include "drake/common/drake_copyable.h" -#include "drake/common/pointer_cast.h" #include "drake/systems/framework/basic_vector.h" +#include "drake/systems/framework/framework_common.h" #include "drake/systems/framework/value.h" #include "drake/systems/framework/value_checker.h" namespace drake { namespace systems { -/// %OutputPortValue contains the value of a single System output port. -/// Input ports of other Systems may depend on an %OutputPortValue. When an -/// %OutputPortValue is deleted, it will automatically notify the listeners that -/// depend on it to disconnect, meaning those ports will resolve to nullptr. -class OutputPortValue { - public: - // OutputPortValue objects are neither copyable nor moveable. - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OutputPortValue) - - /// Constructs a vector-valued OutputPortValue. - /// Takes ownership of @p vec. - /// - /// @tparam T The type of the vector data. Must be a valid Eigen scalar. - /// @tparam V The type of @p vec itself. Must implement BasicVector. - template