Disable unused dependency trackers for better performance. (RobotLoco…

…motion#21134)

systems/framework/cache_doxygen.h

@@ -336,7 +336,10 @@ _Notes_
    variables and numeric/abstract parameters.
 3. Until issue [#9171](https://github.com/RobotLocomotion/drake/issues/9171)
    is resolved we don't know which non-`v` state variables or which parameters
-   may affect kinematics, so we have to depend on all of them.
+   may affect kinematics, so we have to depend on all of them. However,
+   we do not propagate downstream notifications from state variable trackers
+   if a LeafContext does not have any of those state variables (see next section
+   for more information).
 4. Input ports are dependent on the source that provides their values. That
    may be the output port of a peer subsystem, the input port of the parent
    diagram, or a locally-stored fixed input port value.
@@ -362,18 +365,23 @@ discrete state variables, and subscribes to each of the individual discrete
 state trackers. That way if a q is modified, xc gets notified automatically.
 Similarly a change to a single discrete state variable notifies xd. Once those
 subscriptions are set up, no additional code is required in Drake to propagate
-the notifications. Let's call this the "up" direction, where a low-level entity
-notifies its "parent" composite entity.
-
-More subtly, we also have to issue notifications in the "down" direction. That's
-because the Context provides methods like SetContinuousState() and
+the notifications. As an optimization, we do not propagate notifications from
+a state variable source tracker (q, v, z, xd, xa) in LeafContexts that do not
+have any of that particular kind of state variable -- see issue
+[#21133](https://github.com/RobotLocomotion/drake/issues/21133) for
+why this is an important optimization.
+
+Let's call the above the "forward" direction,where a source entity notifies a
+subscribing composite entity.
+More subtly, we also have to issue notifications in the "backward" direction.
+That's because the Context provides methods like SetContinuousState() and
 get_mutable_state() which effectively change a group of source objects. That
 _could_ be handled with more subscriptions, at the cost of introducing cycles
 into the dependency DAG (the "change event" would serve to break those cycles
 during notification sweeps). Instead, since we always know the constituents at
 the time a high-level modification request is made, we simply have bespoke code
-that notifies the lowest-level constituents, then depend on the "up" direction
-subscriptions to get the composite trackers notified. See the
+that notifies the lowest-level constituents, then depend on the "forward"
+direction subscriptions to get the composite trackers notified. See the
 @ref context_base_change_notification_methods "notifications methods" in
 ContextBase.
 

systems/framework/context.cc

@@ -154,16 +154,46 @@ std::unique_ptr<Context<T>> Context<T>::CloneWithoutPointers(
 
 template <typename T>
 void Context<T>::init_continuous_state(std::unique_ptr<ContinuousState<T>> xc) {
+  // Don't propagate notifications through variables we don't have.
+  if (xc->num_q() == 0) {
+    get_mutable_tracker(DependencyTicket(internal::kQTicket))
+        .suppress_notifications();
+  }
+  if (xc->num_v() == 0) {
+    get_mutable_tracker(DependencyTicket(internal::kVTicket))
+        .suppress_notifications();
+  }
+  if (xc->num_z() == 0) {
+    get_mutable_tracker(DependencyTicket(internal::kZTicket))
+        .suppress_notifications();
+  }
+  if (xc->size() == 0) {  // i.e. nq+nv+nz
+    get_mutable_tracker(DependencyTicket(internal::kXcTicket))
+        .suppress_notifications();
+  }
+
   do_access_mutable_state().set_continuous_state(std::move(xc));
 }
 
 template <typename T>
 void Context<T>::init_discrete_state(std::unique_ptr<DiscreteValues<T>> xd) {
+  // Don't propagate notifications through variables we don't have.
+  if (xd->num_groups() == 0) {
+    get_mutable_tracker(DependencyTicket(internal::kXdTicket))
+        .suppress_notifications();
+  }
+
   do_access_mutable_state().set_discrete_state(std::move(xd));
 }
 
 template <typename T>
 void Context<T>::init_abstract_state(std::unique_ptr<AbstractValues> xa) {
+  // Don't propagate notifications through variables we don't have.
+  if (xa->size() == 0) {
+    get_mutable_tracker(DependencyTicket(internal::kXaTicket))
+        .suppress_notifications();
+  }
+
   do_access_mutable_state().set_abstract_state(std::move(xa));
 }
 

systems/framework/context.h

@@ -542,7 +542,9 @@ class Context : public ContextBase {
   _all_ state-dependent computations so requiring out of date notifications
   to be made for all such computations. If you don't mean to change the
   whole state, use more focused methods to modify only a portion of the
-  state. See class documentation for more information. */
+  state. See class documentation for more information.
+  @warning You _must not_ use the returned reference to modify the size,
+  number, or types of state variables. */
   State<T>& get_mutable_state();
 
   /** Returns a mutable reference to the continuous component of the state,
@@ -559,7 +561,9 @@ class Context : public ContextBase {
 
   /** Returns a mutable reference to the discrete component of the state,
   which may be of size zero. Sends out of date notifications for all
-  discrete-state-dependent computations. */
+  discrete-state-dependent computations.
+  @warning You _must not_ use the returned reference to modify the size or
+  number of discrete state variables. */
   DiscreteValues<T>& get_mutable_discrete_state();
 
   /** Returns a mutable reference to the _only_ discrete state vector.
@@ -585,7 +589,9 @@ class Context : public ContextBase {
 
   /** Returns a mutable reference to the abstract component of the state,
   which may be of size zero. Sends out of date notifications for all
-  abstract-state-dependent computations. */
+  abstract-state-dependent computations.
+  @warning You _must not_ use the returned reference to modify the size,
+  number, or types of abstract state variables. */
   AbstractValues& get_mutable_abstract_state();
 
   // TODO(sherm1) Invalidate only dependents of this one abstract variable.
@@ -606,7 +612,9 @@ class Context : public ContextBase {
   date notifications for all parameter-dependent computations. If you don't
   mean to change all the parameters, use the indexed methods to modify only
   some of the parameters so that fewer computations are invalidated and
-  fewer notifications need be sent. */
+  fewer notifications need be sent.
+  @warning You _must not_ use the returned reference to modify the size,
+  number, or types of parameters. */
   Parameters<T>& get_mutable_parameters();
 
   // TODO(sherm1) Invalidate only dependents of this one parameter.

systems/framework/dependency_tracker.cc

@@ -26,10 +26,10 @@ void DependencyTracker::NoteValueChange(int64_t change_event) const {
   DRAKE_ASSERT(change_event > 0);
 
   ++num_value_change_notifications_received_;
-  if (last_change_event_ == change_event) {
+  if (last_change_event_ == change_event || suppress_notifications_) {
     ++num_ignored_notifications_;
     DRAKE_LOGGER_DEBUG(
-        "... ignoring repeated value change notification same change event.");
+        "... ignoring repeated or suppressed value change notification.");
     return;
   }
   last_change_event_ = change_event;
@@ -52,10 +52,10 @@ void DependencyTracker::NotePrerequisiteChange(
   DRAKE_ASSERT(HasPrerequisite(prerequisite));  // Expensive.
 
   ++num_prerequisite_notifications_received_;
-  if (last_change_event_ == change_event) {
+  if (last_change_event_ == change_event || suppress_notifications_) {
     ++num_ignored_notifications_;
     DRAKE_LOGGER_DEBUG(
-        "{}... ignoring repeated prereq change notification same change event.",
+        "{}... ignoring repeated or suppressed prereq change notification.",
         Indent(depth));
     return;
   }

systems/framework/dependency_tracker.h

@@ -176,6 +176,20 @@ class DependencyTracker {
     return cache_value_;
   }
 
+  /** (Internal use only) Instructs this tracker to suppress notifications to
+  downstream subscribers. This can be used by the framework during Context
+  allocation to disable built-in trackers that have nothing to track. For
+  example, if there are no q's we can improve performance and avoid spurious
+  notifications to q-subscribers like configuration_tracker by disabling q's
+  tracker. */
+  void suppress_notifications() {
+    suppress_notifications_ = true;
+  }
+
+  /** Returns true if suppress_notifications() has been called on this
+  tracker. */
+  bool notifications_are_suppressed() const { return suppress_notifications_; }
+
   /** 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
@@ -353,6 +367,7 @@ class DependencyTracker {
       clone->cache_value_ = nullptr;
     clone->subscribers_.resize(num_subscribers(), nullptr);
     clone->prerequisites_.resize(num_prerequisites(), nullptr);
+    clone->suppress_notifications_ = suppress_notifications_;
     return clone;
   }
 
@@ -410,6 +425,8 @@ class DependencyTracker {
   std::vector<const DependencyTracker*> subscribers_;
   std::vector<const DependencyTracker*> prerequisites_;
 
+  bool suppress_notifications_{false};
+
   // Used for short-circuiting repeated notifications. Does not otherwise change
   // the result; hence mutable is OK. All legitimate change events must be
   // greater than zero, so this will never match.

systems/framework/discrete_values.h

@@ -76,6 +76,8 @@ class DiscreteValues {
   /// Adds an additional group that owns the given @p datum, which must be
   /// non-null. Returns the assigned group number, counting up from 0 for the
   /// first group.
+  /// @warning Do not use this method to add groups to a %DiscreteValues
+  /// object that is owned by an existing Context.
   int AppendGroup(std::unique_ptr<BasicVector<T>> datum) {
     if (datum == nullptr) {
       throw std::logic_error(

systems/framework/state.h

@@ -20,9 +20,9 @@ namespace systems {
 ///
 /// A %State `x` contains three types of state variables:
 ///
-/// - ContinuousState `xc`
-/// - DiscreteState   `xd`
-/// - AbstractState   `xa`
+/// - Continuous state `xc`
+/// - Discrete state   `xd`
+/// - Abstract state   `xa`
 ///
 /// @tparam_default_scalar
 template <typename T>
@@ -34,6 +34,13 @@ class State {
   State();
   virtual ~State();
 
+  // TODO(sherm1) Consider making set_{continuous,discrete,abstract}_state()
+  //  functions private with attorney access only to make it impossible for
+  //  users to attempt resizing State within an existing Context.
+
+  /// (Advanced) Defines the continuous state variables for this %State.
+  /// @warning Do not use this function to resize continuous state in a
+  /// %State that is owned by an existing Context.
   void set_continuous_state(std::unique_ptr<ContinuousState<T>> xc) {
     DRAKE_DEMAND(xc != nullptr);
     continuous_state_ = std::move(xc);
@@ -49,6 +56,9 @@ class State {
     return *continuous_state_.get();
   }
 
+  /// (Advanced) Defines the discrete state variables for this %State.
+  /// @warning Do not use this function to resize discrete state in a
+  /// %State that is owned by an existing Context.
   void set_discrete_state(std::unique_ptr<DiscreteValues<T>> xd) {
     DRAKE_DEMAND(xd != nullptr);
     discrete_state_ = std::move(xd);
@@ -74,6 +84,9 @@ class State {
     return xd.get_mutable_vector(index);
   }
 
+  /// (Advanced) Defines the abstract state variables for this %State.
+  /// @warning Do not use this function to resize or change types of abstract
+  /// state variables in a %State that is owned by an existing Context.
   void set_abstract_state(std::unique_ptr<AbstractValues> xa) {
     DRAKE_DEMAND(xa != nullptr);
     abstract_state_ = std::move(xa);

systems/framework/test/dependency_tracker_test.cc

@@ -577,6 +577,52 @@ TEST_F(HandBuiltDependencies, Clone) {
   ExpectStatsMatch(&e0_tracker, entry0_stats);
 }
 
+// Check that we can make a DependencyTracker suppress notifications to
+// its subscribers, and that the suppress_notifications flag is copied
+// when a Context is cloned.
+TEST_F(HandBuiltDependencies, SuppressNotifications) {
+  // Refer to diagram above to see the interconnections.
+
+  EXPECT_FALSE(middle1_->notifications_are_suppressed());
+  ExpectAllStatsMatch();  // Everything is zero here.
+
+  // This should notify downstream1 and middle1. Then middle1
+  // notifies downstream1 again, downstream2, and entry0.
+  upstream1_->NoteValueChange(1LL);
+  EXPECT_EQ(upstream1_->num_notifications_sent(), 2);
+  EXPECT_EQ(middle1_->num_prerequisite_change_events(), 1);
+  EXPECT_EQ(middle1_->num_ignored_notifications(), 0);
+  EXPECT_EQ(middle1_->num_notifications_sent(), 3);
+  EXPECT_EQ(downstream1_->num_prerequisite_change_events(), 2);
+  EXPECT_EQ(downstream2_->num_prerequisite_change_events(), 1);
+  EXPECT_EQ(downstream2_->num_notifications_sent(), 1);
+  EXPECT_EQ(entry0_tracker_->num_prerequisite_change_events(), 2);
+
+  // Now suppress notifications from middle1 and repeat the above. Make
+  // sure no one downstream of middle1 gets notified.
+  middle1_->suppress_notifications();
+  EXPECT_TRUE(middle1_->notifications_are_suppressed());
+  upstream1_->NoteValueChange(2LL);
+  EXPECT_EQ(upstream1_->num_notifications_sent(), 4);  // +2
+  EXPECT_EQ(middle1_->num_prerequisite_change_events(), 2);  // +1
+  EXPECT_EQ(middle1_->num_ignored_notifications(), 1);  // should ignore now
+  EXPECT_EQ(middle1_->num_notifications_sent(), 3);  // +0
+  EXPECT_EQ(downstream1_->num_prerequisite_change_events(), 3);  // +1
+  EXPECT_EQ(downstream2_->num_prerequisite_change_events(), 1);  // +0
+  EXPECT_EQ(downstream2_->num_notifications_sent(), 1);  // +0
+  EXPECT_EQ(entry0_tracker_->num_prerequisite_change_events(), 2);  // +0
+
+  // Check that the clone preserves the suppress_notifications flag.
+  auto clone_context = context_.Clone();
+  const DependencyTracker& clone_middle1_ =
+      clone_context->get_dependency_graph().get_tracker(middle1_->ticket());
+  EXPECT_TRUE(clone_middle1_.notifications_are_suppressed());
+  // Check a random one to make sure they didn't all get suppressed!
+  const DependencyTracker& clone_upstream1_ =
+      clone_context->get_dependency_graph().get_tracker(upstream1_->ticket());
+  EXPECT_FALSE(clone_upstream1_.notifications_are_suppressed());
+}
+
 }  // namespace
 }  // namespace systems
 }  // namespace drake

systems/framework/test/diagram_context_test.cc

@@ -138,8 +138,10 @@ class DiagramContextTest : public ::testing::Test {
   // Some tests below need to know which of the subsystems above have particular
   // resources. They use kNumSystems to identify the "parent" subsystem which
   // inherits all the resources of its children.
+  static std::set<int> has_continuous_state() { return {2, 3, kNumSystems}; }
   static std::set<int> has_discrete_state() { return {4, kNumSystems}; }
   static std::set<int> has_abstract_state() { return {5, kNumSystems}; }
+  static std::set<int> has_state() { return {2, 3, 4, 5, kNumSystems}; }
   static std::set<int> has_numeric_parameter() { return {6, kNumSystems}; }
   static std::set<int> has_abstract_parameter() { return {7, kNumSystems}; }
   static std::set<int> has_parameter() { return {6, 7, kNumSystems}; }
@@ -346,9 +348,9 @@ TEST_F(DiagramContextTest, MutableStateNotifications) {
 
   // Changing the whole state should affect all the substates.
   context_->get_mutable_state();  // Return value ignored.
-  VerifyNotifications("get_mutable_state: x",
+  VerifyNotifications("get_mutable_state: x", has_state(),
                       SystemBase::all_state_ticket(), &x_before);
-  VerifyNotifications("get_mutable_state: xc",
+  VerifyNotifications("get_mutable_state: xc", has_continuous_state(),
                       SystemBase::xc_ticket(), &xc_before);
   VerifyNotifications("get_mutable_state: xd", has_discrete_state(),
                       SystemBase::xd_ticket(), &xd_before);
@@ -357,20 +359,23 @@ TEST_F(DiagramContextTest, MutableStateNotifications) {
 
   // Changing continuous state should affect only x and xc.
   context_->get_mutable_continuous_state();  // Return value ignored.
-  VerifyNotifications("get_mutable_continuous_state: x",
+  VerifyNotifications("get_mutable_continuous_state: x", has_continuous_state(),
                       SystemBase::all_state_ticket(), &x_before);
   VerifyNotifications("get_mutable_continuous_state: xc",
-                      SystemBase::xc_ticket(), &xc_before);
+                      has_continuous_state(), SystemBase::xc_ticket(),
+                      &xc_before);
   VerifyNotifications("get_mutable_continuous_state: xd", {},  // None.
                       SystemBase::xd_ticket(), &xd_before);
   VerifyNotifications("get_mutable_continuous_state: xa", {},  // None.
                       SystemBase::xa_ticket(), &xa_before);
 
   context_->get_mutable_continuous_state_vector();  // Return value ignored.
   VerifyNotifications("get_mutable_continuous_state_vector: x",
-                      SystemBase::all_state_ticket(), &x_before);
+                      has_continuous_state(), SystemBase::all_state_ticket(),
+                      &x_before);
   VerifyNotifications("get_mutable_continuous_state_vector: xc",
-                      SystemBase::xc_ticket(), &xc_before);
+                      has_continuous_state(), SystemBase::xc_ticket(),
+                      &xc_before);
   VerifyNotifications("get_mutable_continuous_state_vector: xd", {},  // None.
                       SystemBase::xd_ticket(), &xd_before);
   VerifyNotifications("get_mutable_continuous_state_vector: xa", {},  // None.
@@ -379,9 +384,9 @@ TEST_F(DiagramContextTest, MutableStateNotifications) {
   const Eigen::Vector2d new_xc1(3.25, 5.5);
   context_->SetContinuousState(VectorX<double>(new_xc1));
   VerifyContinuousStateValue(new_xc1);
-  VerifyNotifications("SetContinuousState: x",
+  VerifyNotifications("SetContinuousState: x", has_continuous_state(),
                       SystemBase::all_state_ticket(), &x_before);
-  VerifyNotifications("SetContinuousState: xc",
+  VerifyNotifications("SetContinuousState: xc", has_continuous_state(),
                       SystemBase::xc_ticket(), &xc_before);
   VerifyNotifications("SetContinuousState: xd", {},  // None.
                       SystemBase::xd_ticket(), &xd_before);
@@ -399,9 +404,9 @@ TEST_F(DiagramContextTest, MutableStateNotifications) {
   // Make sure notifications got delivered.
   VerifyNotifications("SetTimeAndContinuousState: t",
                       SystemBase::time_ticket(), &t_before);
-  VerifyNotifications("SetTimeAndContinuousState: x",
+  VerifyNotifications("SetTimeAndContinuousState: x", has_continuous_state(),
                       SystemBase::all_state_ticket(), &x_before);
-  VerifyNotifications("SetTimeAndContinuousState: xc",
+  VerifyNotifications("SetTimeAndContinuousState: xc", has_continuous_state(),
                       SystemBase::xc_ticket(), &xc_before);
   VerifyNotifications("SetTimeAndContinuousState: xd", {},  // None.
                       SystemBase::xd_ticket(), &xd_before);
@@ -580,10 +585,11 @@ TEST_F(DiagramContextTest, MutableEverythingNotifications) {
   VerifyNotifications("SetTimeStateAndParametersFrom: pa",
                       has_abstract_parameter(),
                       SystemBase::pa_ticket(), &pa_before);
-  VerifyNotifications("SetTimeStateAndParametersFrom: x",
+  VerifyNotifications("SetTimeStateAndParametersFrom: x", has_state(),
                       SystemBase::all_state_ticket(), &x_before);
   VerifyNotifications("SetTimeStateAndParametersFrom: xc",
-                      SystemBase::xc_ticket(), &xc_before);
+                      has_continuous_state(), SystemBase::xc_ticket(),
+                      &xc_before);
   VerifyNotifications("SetTimeStateAndParametersFrom: xd", has_discrete_state(),
                       SystemBase::xd_ticket(), &xd_before);
   VerifyNotifications("SetTimeStateAndParametersFrom: xa", has_abstract_state(),