Drake
diagram.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <algorithm>
4 #include <functional>
5 #include <limits>
6 #include <map>
7 #include <memory>
8 #include <set>
9 #include <stdexcept>
10 #include <string>
11 #include <utility>
12 #include <vector>
13 
16 #include "drake/common/symbolic.h"
28 
29 namespace drake {
30 namespace systems {
31 
32 template <typename T>
34 
35 /// Diagram is a System composed of one or more constituent Systems, arranged
36 /// in a directed graph where the vertices are the constituent Systems
37 /// themselves, and the edges connect the output of one constituent System
38 /// to the input of another. To construct a Diagram, use a DiagramBuilder.
39 ///
40 /// Each System in the Diagram must have a unique, non-empty name.
41 ///
42 /// @tparam T The mathematical scalar type. Must be a valid Eigen scalar.
43 template <typename T>
44 class Diagram : public System<T>, internal::SystemParentServiceInterface {
45  public:
46  // Diagram objects are neither copyable nor moveable.
48 
49  using InputPortLocator = std::pair<const System<T>*, InputPortIndex>;
50  using OutputPortLocator = std::pair<const System<T>*, OutputPortIndex>;
51 
52  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
53  template <typename U>
54  explicit Diagram(const Diagram<U>& other)
55  : Diagram(other.template ConvertScalarType<T>()) {}
56 
57  ~Diagram() override {}
58 
59  /// Returns the list of contained Systems.
60  std::vector<const systems::System<T>*> GetSystems() const {
61  std::vector<const systems::System<T>*> result;
62  result.reserve(registered_systems_.size());
63  for (const auto& system : registered_systems_) {
64  result.push_back(system.get());
65  }
66  return result;
67  }
68 
69  std::multimap<int, int> GetDirectFeedthroughs() const final {
70  std::multimap<int, int> pairs;
71  for (InputPortIndex u(0); u < this->get_num_input_ports(); ++u) {
72  for (OutputPortIndex v(0); v < this->get_num_output_ports(); ++v) {
73  if (DoHasDirectFeedthrough(u, v)) {
74  pairs.emplace(u, v);
75  }
76  }
77  }
78  return pairs;
79  };
80 
81  /// Allocates a DiagramEventCollection for this Diagram.
82  /// @sa System::AllocateCompositeEventCollection().
83  std::unique_ptr<CompositeEventCollection<T>>
85  const int num_systems = num_subsystems();
86  std::vector<std::unique_ptr<CompositeEventCollection<T>>> subevents(
87  num_systems);
88  for (SubsystemIndex i(0); i < num_systems; ++i) {
89  subevents[i] = registered_systems_[i]->AllocateCompositeEventCollection();
90  }
91 
92  return std::make_unique<DiagramCompositeEventCollection<T>>(
93  std::move(subevents));
94  }
95 
96  void SetDefaultState(const Context<T>& context,
97  State<T>* state) const override {
98  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
99  DRAKE_DEMAND(diagram_context != nullptr);
100 
101  auto diagram_state = dynamic_cast<DiagramState<T>*>(state);
102  DRAKE_DEMAND(diagram_state != nullptr);
103 
104  // Set default state of each constituent system.
105  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
106  auto& subcontext = diagram_context->GetSubsystemContext(i);
107  auto& substate = diagram_state->get_mutable_substate(i);
108  registered_systems_[i]->SetDefaultState(subcontext, &substate);
109  }
110  }
111 
112  void SetDefaultParameters(const Context<T>& context,
113  Parameters<T>* params) const override {
114  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
115  DRAKE_DEMAND(diagram_context != nullptr);
116 
117  int numeric_parameter_offset = 0;
118  int abstract_parameter_offset = 0;
119 
120  // Set default parameters of each constituent system.
121  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
122  auto& subcontext = diagram_context->GetSubsystemContext(i);
123 
124  if (!subcontext.num_numeric_parameters() &&
125  !subcontext.num_abstract_parameters()) {
126  // Then there is no work to do for this subcontext.
127  continue;
128  }
129 
130  // Make a new Parameters<T> structure with pointers to the mutable
131  // subsystem parameter values. This does not make a copy of the
132  // underlying data.
133  // TODO(russt): Consider implementing a DiagramParameters, analogous to
134  // DiagramState, to avoid these dynamic allocations if they prove
135  // expensive.
136 
137  std::vector<BasicVector<T>*> numeric_params;
138  for (int j = 0; j < subcontext.num_numeric_parameters(); ++j) {
139  numeric_params.push_back(&params->get_mutable_numeric_parameter(
140  numeric_parameter_offset + j));
141  }
142  numeric_parameter_offset += subcontext.num_numeric_parameters();
143 
144  std::vector<AbstractValue*> abstract_params;
145  for (int j = 0; j < subcontext.num_abstract_parameters(); ++j) {
146  abstract_params.push_back(&params->get_mutable_abstract_parameter(
147  abstract_parameter_offset + j));
148  }
149  abstract_parameter_offset += subcontext.num_abstract_parameters();
150 
151  Parameters<T> subparameters;
152  subparameters.set_numeric_parameters(
153  std::make_unique<DiscreteValues<T>>(numeric_params));
154  subparameters.set_abstract_parameters(
155  std::make_unique<AbstractValues>(abstract_params));
156 
157  registered_systems_[i]->SetDefaultParameters(subcontext, &subparameters);
158  }
159  }
160 
161  void SetRandomState(const Context<T>& context, State<T>* state,
162  RandomGenerator* generator) const override {
163  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
164  DRAKE_DEMAND(diagram_context != nullptr);
165 
166  auto diagram_state = dynamic_cast<DiagramState<T>*>(state);
167  DRAKE_DEMAND(diagram_state != nullptr);
168 
169  // Set state of each constituent system.
170  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
171  auto& subcontext = diagram_context->GetSubsystemContext(i);
172  auto& substate = diagram_state->get_mutable_substate(i);
173  registered_systems_[i]->SetRandomState(subcontext, &substate, generator);
174  }
175  }
176 
177  void SetRandomParameters(const Context<T>& context, Parameters<T>* params,
178  RandomGenerator* generator) const override {
179  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
180  DRAKE_DEMAND(diagram_context != nullptr);
181 
182  int numeric_parameter_offset = 0;
183  int abstract_parameter_offset = 0;
184 
185  // Set parameters of each constituent system.
186  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
187  auto& subcontext = diagram_context->GetSubsystemContext(i);
188 
189  if (!subcontext.num_numeric_parameters() &&
190  !subcontext.num_abstract_parameters()) {
191  // Then there is no work to do for this subcontext.
192  continue;
193  }
194 
195  // Make a new Parameters<T> structure with pointers to the mutable
196  // subsystem parameter values. This does not make a copy of the
197  // underlying data.
198  // TODO(russt): This code is duplicated from SetDefaultParameters.
199  // Consider extracting it to a helper method (waiting for the rule of
200  // three).
201 
202  std::vector<BasicVector<T>*> numeric_params;
203  std::vector<AbstractValue*> abstract_params;
204  for (int j = 0; j < subcontext.num_numeric_parameters(); ++j) {
205  numeric_params.push_back(&params->get_mutable_numeric_parameter(
206  numeric_parameter_offset + j));
207  }
208  numeric_parameter_offset += subcontext.num_numeric_parameters();
209  for (int j = 0; j < subcontext.num_abstract_parameters(); ++j) {
210  abstract_params.push_back(&params->get_mutable_abstract_parameter(
211  abstract_parameter_offset + j));
212  }
213  abstract_parameter_offset += subcontext.num_abstract_parameters();
214  Parameters<T> subparameters;
215  subparameters.set_numeric_parameters(
216  std::make_unique<DiscreteValues<T>>(numeric_params));
217  subparameters.set_abstract_parameters(
218  std::make_unique<AbstractValues>(abstract_params));
219 
220  registered_systems_[i]->SetRandomParameters(subcontext, &subparameters,
221  generator);
222  }
223  }
224 
225  /// @cond
226  // The three methods below are hidden from doxygen, as described in
227  // documentation for their corresponding methods in System.
228  std::unique_ptr<EventCollection<PublishEvent<T>>>
229  AllocateForcedPublishEventCollection() const final {
230  return AllocateForcedEventCollection<PublishEvent<T>>(
232  }
233 
234  std::unique_ptr<EventCollection<DiscreteUpdateEvent<T>>>
235  AllocateForcedDiscreteUpdateEventCollection() const final {
236  return AllocateForcedEventCollection<DiscreteUpdateEvent<T>>(
238  }
239 
240  std::unique_ptr<EventCollection<UnrestrictedUpdateEvent<T>>>
241  AllocateForcedUnrestrictedUpdateEventCollection() const final {
242  return AllocateForcedEventCollection<UnrestrictedUpdateEvent<T>>(
244  }
245  /// @endcond
246 
247  /// Aggregates the time derivatives from each subsystem into a
248  /// DiagramContinuousState.
249  std::unique_ptr<ContinuousState<T>> AllocateTimeDerivatives() const override {
250  std::vector<std::unique_ptr<ContinuousState<T>>> sub_derivatives;
251  for (const auto& system : registered_systems_) {
252  sub_derivatives.push_back(system->AllocateTimeDerivatives());
253  }
254  return std::make_unique<DiagramContinuousState<T>>(
255  std::move(sub_derivatives));
256  }
257 
258  /// Aggregates the discrete update variables from each subsystem into a
259  /// DiagramDiscreteVariables.
260  std::unique_ptr<DiscreteValues<T>> AllocateDiscreteVariables()
261  const override {
262  std::vector<std::unique_ptr<DiscreteValues<T>>> sub_discretes;
263  for (const auto& system : registered_systems_) {
264  sub_discretes.push_back(system->AllocateDiscreteVariables());
265  }
266  return std::make_unique<DiagramDiscreteValues<T>>(std::move(sub_discretes));
267  }
268 
269  void DoCalcTimeDerivatives(const Context<T>& context,
270  ContinuousState<T>* derivatives) const override {
271  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
272  DRAKE_DEMAND(diagram_context != nullptr);
273 
274  auto diagram_derivatives =
275  dynamic_cast<DiagramContinuousState<T>*>(derivatives);
276  DRAKE_DEMAND(diagram_derivatives != nullptr);
277  const int n = diagram_derivatives->num_substates();
278  DRAKE_DEMAND(num_subsystems() == n);
279 
280  // Evaluate the derivatives of each constituent system.
281  for (SubsystemIndex i(0); i < n; ++i) {
282  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
283  ContinuousState<T>& subderivatives =
284  diagram_derivatives->get_mutable_substate(i);
285  registered_systems_[i]->CalcTimeDerivatives(subcontext, &subderivatives);
286  }
287  }
288 
289  /// Retrieves the state derivatives for a particular subsystem from the
290  /// derivatives for the entire diagram. Aborts if @p subsystem is not
291  /// actually a subsystem of this diagram. Returns a 0-length ContinuousState
292  /// if @p subsystem has none.
294  const ContinuousState<T>& derivatives, const System<T>* subsystem) const {
295  DRAKE_DEMAND(subsystem != nullptr);
296  auto diagram_derivatives =
297  dynamic_cast<const DiagramContinuousState<T>*>(&derivatives);
298  DRAKE_DEMAND(diagram_derivatives != nullptr);
299  const SubsystemIndex i = GetSystemIndexOrAbort(subsystem);
300  return diagram_derivatives->get_substate(i);
301  }
302 
303  /// Returns a constant reference to the subcontext that corresponds to the
304  /// system @p subsystem.
305  /// Classes inheriting from %Diagram need access to this method in order to
306  /// pass their constituent subsystems the appropriate subcontext. Aborts if
307  /// @p subsystem is not actually a subsystem of this diagram.
308  const Context<T>& GetSubsystemContext(const System<T>& subsystem,
309  const Context<T>& context) const {
310  auto ret = DoGetTargetSystemContext(subsystem, &context);
311  DRAKE_DEMAND(ret != nullptr);
312  return *ret;
313  }
314 
315  /// Returns the subcontext that corresponds to the system @p subsystem.
316  /// Classes inheriting from %Diagram need access to this method in order to
317  /// pass their constituent subsystems the appropriate subcontext. Aborts if
318  /// @p subsystem is not actually a subsystem of this diagram.
320  Context<T>* context) const {
321  auto ret = DoGetMutableTargetSystemContext(subsystem, context);
322  DRAKE_DEMAND(ret != nullptr);
323  return *ret;
324  }
325 
326  /// Returns the const subsystem composite event collection from @p events
327  /// that corresponds to @p subsystem. Aborts if @p subsystem is not a
328  /// subsystem of this diagram.
331  const CompositeEventCollection<T>& events) const {
332  auto ret = DoGetTargetSystemCompositeEventCollection(subsystem, &events);
333  DRAKE_DEMAND(ret != nullptr);
334  return *ret;
335  }
336 
337  /// Returns the mutable subsystem composite event collection that corresponds
338  /// to @p subsystem. Aborts if @p subsystem is not a subsystem of this
339  /// diagram.
341  const System<T>& subsystem, CompositeEventCollection<T>* events) const {
343  subsystem, events);
344  DRAKE_DEMAND(ret != nullptr);
345  return *ret;
346  }
347 
348  /// Retrieves the state for a particular subsystem from the context for the
349  /// entire diagram. Invalidates all entries in that subsystem's cache that
350  /// depend on State. Aborts if @p subsystem is not actually a subsystem of
351  /// this diagram.
352  ///
353  /// TODO(david-german-tri): Provide finer-grained accessors for finer-grained
354  /// invalidation.
356  Context<T>* context) const {
357  Context<T>& subcontext = GetMutableSubsystemContext(subsystem, context);
358  return subcontext.get_mutable_state();
359  }
360 
361  /// Retrieves the state for a particular subsystem from the @p state for the
362  /// entire diagram. Aborts if @p subsystem is not actually a subsystem of this
363  /// diagram.
365  State<T>* state) const {
366  auto ret = DoGetMutableTargetSystemState(subsystem, state);
367  DRAKE_DEMAND(ret != nullptr);
368  return *ret;
369  }
370 
371  /// Retrieves the state for a particular subsystem from the @p state for the
372  /// entire diagram. Aborts if @p subsystem is not actually a subsystem of this
373  /// diagram.
374  const State<T>& GetSubsystemState(const System<T>& subsystem,
375  const State<T>& state) const {
376  auto ret = DoGetTargetSystemState(subsystem, &state);
377  DRAKE_DEMAND(ret != nullptr);
378  return *ret;
379  }
380 
381  //----------------------------------------------------------------------------
382  /// @name Graphviz methods
383  //@{
384 
385  /// Returns a Graphviz fragment describing this Diagram. To obtain a complete
386  /// Graphviz graph, call System<T>::GetGraphvizString.
387  void GetGraphvizFragment(std::stringstream* dot) const override {
388  // Open the Diagram.
389  const int64_t id = this->GetGraphvizId();
390  *dot << "subgraph cluster" << id << "diagram" " {" << std::endl;
391  *dot << "color=black" << std::endl;
392  *dot << "concentrate=true" << std::endl;
393  std::string name = this->get_name();
394  if (name.empty()) name = std::to_string(id);
395  *dot << "label=\"" << name << "\";" << std::endl;
396 
397  // Add a cluster for the input port nodes.
398  *dot << "subgraph cluster" << id << "inputports" << " {" << std::endl;
399  *dot << "rank=same" << std::endl;
400  *dot << "color=lightgrey" << std::endl;
401  *dot << "style=filled" << std::endl;
402  *dot << "label=\"input ports\"" << std::endl;
403  for (int i = 0; i < this->get_num_input_ports(); ++i) {
404  this->GetGraphvizInputPortToken(this->get_input_port(i), dot);
405  *dot << "[color=blue, label=\"u" << i << "\"];" << std::endl;
406  }
407  *dot << "}" << std::endl;
408 
409  // Add a cluster for the output port nodes.
410  *dot << "subgraph cluster" << id << "outputports" << " {" << std::endl;
411  *dot << "rank=same" << std::endl;
412  *dot << "color=lightgrey" << std::endl;
413  *dot << "style=filled" << std::endl;
414  *dot << "label=\"output ports\"" << std::endl;
415  for (int i = 0; i < this->get_num_output_ports(); ++i) {
416  this->GetGraphvizOutputPortToken(this->get_output_port(i), dot);
417  *dot << "[color=green, label=\"y" << i << "\"];" << std::endl;
418  }
419  *dot << "}" << std::endl;
420 
421  // Add a cluster for the subsystems.
422  *dot << "subgraph cluster" << id << "subsystems" << " {" << std::endl;
423  *dot << "color=white" << std::endl;
424  *dot << "label=\"\"" << std::endl;
425  // -- Add the subsystems themselves.
426  for (const auto& subsystem : registered_systems_) {
427  subsystem->GetGraphvizFragment(dot);
428  }
429  // -- Add the connections as edges.
430  for (const auto& edge : connection_map_) {
431  const OutputPortLocator& src = edge.second;
432  const System<T>* src_sys = src.first;
433  const InputPortLocator& dest = edge.first;
434  const System<T>* dest_sys = dest.first;
435  src_sys->GetGraphvizOutputPortToken(src_sys->get_output_port(src.second),
436  dot);
437  *dot << " -> ";
438  dest_sys->GetGraphvizInputPortToken(dest_sys->get_input_port(dest.second),
439  dot);
440  *dot << ";" << std::endl;
441  }
442 
443  // -- Add edges from the input and output port nodes to the subsystems that
444  // actually service that port. These edges are highlighted in blue
445  // (input) and green (output), matching the port nodes.
446  for (int i = 0; i < this->get_num_input_ports(); ++i) {
447  const auto& port_id = input_port_ids_[i];
448  this->GetGraphvizInputPortToken(this->get_input_port(i), dot);
449  *dot << " -> ";
450  port_id.first->GetGraphvizInputPortToken(
451  port_id.first->get_input_port(port_id.second), dot);
452  *dot << " [color=blue];" << std::endl;
453  }
454 
455  for (int i = 0; i < this->get_num_output_ports(); ++i) {
456  const auto& port_id = output_port_ids_[i];
457  port_id.first->GetGraphvizOutputPortToken(
458  port_id.first->get_output_port(port_id.second), dot);
459  *dot << " -> ";
460  this->GetGraphvizOutputPortToken(this->get_output_port(i), dot);
461  *dot << " [color=green];" << std::endl;
462  }
463  *dot << "}" << std::endl;
464 
465  // Close the diagram.
466  *dot << "}" << std::endl;
467  }
468 
470  std::stringstream* dot) const override {
471  DRAKE_DEMAND(port.get_system() == this);
472  *dot << "_" << this->GetGraphvizId() << "_u" << port.get_index();
473  }
474 
476  std::stringstream* dot) const override {
477  DRAKE_DEMAND(&port.get_system() == this);
478  *dot << "_" << this->GetGraphvizId() << "_y" << port.get_index();
479  }
480 
481  //@}
482 
483  /// Returns the index of the given @p sys in this diagram, or aborts if @p sys
484  /// is not a member of the diagram.
486  auto it = system_index_map_.find(sys);
487  DRAKE_DEMAND(it != system_index_map_.end());
488  return it->second;
489  }
490 
491  int get_num_continuous_states() const final {
492  int num_states = 0;
493  for (const auto& system : registered_systems_) {
494  num_states += system->get_num_continuous_states();
495  }
496 
497  return num_states;
498  }
499 
500  protected:
501  /// Constructs an uninitialized Diagram. Subclasses that use this constructor
502  /// are obligated to call DiagramBuilder::BuildInto(this). Provides scalar-
503  /// type conversion support only if every contained subsystem provides the
504  /// same support.
507  SystemTypeTag<systems::Diagram>{},
509 
510  /// (Advanced) Constructs an uninitialized Diagram. Subclasses that use this
511  /// constructor are obligated to call DiagramBuilder::BuildInto(this).
512  ///
513  /// Declares scalar-type conversion support using @p converter. Support for
514  /// a given pair of types `T, U` to convert from and to will be enabled only
515  /// if every contained subsystem supports that pair.
516  ///
517  /// See @ref system_scalar_conversion for detailed background and examples
518  /// related to scalar-type conversion support.
519  explicit Diagram(SystemScalarConverter converter)
520  : System<T>(std::move(converter)) {}
521 
522  /// For the subsystem associated with @p witness_func, gets its subcontext
523  /// from @p context, passes the subcontext to @p witness_func' Evaulate
524  /// method and returns the result. Aborts if the subsystem is not part of
525  /// this Diagram.
527  const WitnessFunction<T>& witness_func) const final {
528  const System<T>& system = witness_func.get_system();
529  const Context<T>& subcontext = GetSubsystemContext(system, context);
530  return witness_func.CalcWitnessValue(subcontext);
531  }
532 
533  /// For the subsystem associated with `witness_func`, gets its mutable
534  /// sub composite event collection from `events`, and passes it to
535  /// `witness_func`'s AddEventToCollection method. This method also modifies
536  /// `event` by updating the pointers to "diagram" continuous state to point to
537  /// the ContinuousState pointers for the associated subsystem instead. Aborts
538  /// if the subsystem is not part of this Diagram.
540  Event<T>* event,
541  CompositeEventCollection<T>* events) const final {
542  DRAKE_DEMAND(events);
543  DRAKE_DEMAND(event);
544  DRAKE_DEMAND(event->get_event_data());
545 
546  // Get the event data- it will need to be modified.
547  auto data = dynamic_cast<WitnessTriggeredEventData<T>*>(
548  event->get_mutable_event_data());
549  DRAKE_DEMAND(data);
550 
551  // Get the vector of events corresponding to the subsystem.
552  const System<T>& subsystem = data->triggered_witness()->get_system();
553  CompositeEventCollection<T>& subevents =
555 
556  // Get the continuous states at both window endpoints.
557  auto diagram_xc0 = dynamic_cast<const DiagramContinuousState<T>*>(
558  data->xc0());
559  DRAKE_DEMAND(diagram_xc0 != nullptr);
560  auto diagram_xcf = dynamic_cast<const DiagramContinuousState<T>*>(
561  data->xcf());
562  DRAKE_DEMAND(diagram_xcf != nullptr);
563 
564  // Modify the pointer to the event data to point to the sub-system
565  // continuous states.
566  data->set_xc0(DoGetTargetSystemContinuousState(subsystem, diagram_xc0));
567  data->set_xcf(DoGetTargetSystemContinuousState(subsystem, diagram_xcf));
568 
569  // Add the event to the collection.
570  event->add_to_composite(&subevents);
571  }
572 
573  /// Provides witness functions of subsystems that are active at the beginning
574  /// of a continuous time interval. The vector of witness functions is not
575  /// ordered in a particular manner.
576  void DoGetWitnessFunctions(const Context<T>& context,
577  std::vector<const WitnessFunction<T>*>* witnesses) const final {
578  // A temporary vector is necessary since the vector of witnesses is
579  // declared to be empty on entry to DoGetWitnessFunctions().
580  std::vector<const WitnessFunction<T>*> temp_witnesses;
581 
582  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
583  DRAKE_DEMAND(diagram_context != nullptr);
584 
585  SubsystemIndex index(0);
586 
587  for (const auto& system : registered_systems_) {
588  DRAKE_ASSERT(index == GetSystemIndexOrAbort(system.get()));
589  temp_witnesses.clear();
590  system->GetWitnessFunctions(diagram_context->GetSubsystemContext(index),
591  &temp_witnesses);
592  witnesses->insert(witnesses->end(), temp_witnesses.begin(),
593  temp_witnesses.end());
594  ++index;
595  }
596  }
597 
598  /// Returns a pointer to mutable context if @p target_system is a sub system
599  /// of this, nullptr is returned otherwise.
601  const System<T>& target_system, Context<T>* context) const final {
602  if (&target_system == this)
603  return context;
604 
605  return GetSubsystemStuff<Context<T>, DiagramContext<T>>(
606  target_system, context,
609  }
610 
611  /// Returns a pointer to const context if @p target_system is a subsystem
612  /// of this, nullptr is returned otherwise.
614  const System<T>& target_system, const Context<T>* context) const final {
615  if (&target_system == this)
616  return context;
617 
618  return GetSubsystemStuff<const Context<T>, const DiagramContext<T>>(
619  target_system, context,
622  }
623 
624  /// Returns a pointer to mutable state if @p target_system is a subsystem
625  /// of this, nullptr is returned otherwise.
627  const System<T>& target_system, State<T>* state) const final {
628  if (&target_system == this)
629  return state;
630 
631  return GetSubsystemStuff<State<T>, DiagramState<T>>(
632  target_system, state,
635  }
636 
637  /// Returns a pointer to const state if @p target_system is a subsystem
638  /// of this, nullptr is returned otherwise.
640  const System<T>& target_system,
641  const ContinuousState<T>* xc) const final {
642  if (&target_system == this)
643  return xc;
644 
645  return GetSubsystemStuff<const ContinuousState<T>,
647  target_system, xc,
650  }
651 
652  /// Returns a pointer to const state if @p target_system is a subsystem
653  /// of this, nullptr is returned otherwise.
655  const System<T>& target_system, const State<T>* state) const final {
656  if (&target_system == this)
657  return state;
658 
659  return GetSubsystemStuff<const State<T>, const DiagramState<T>>(
660  target_system, state,
663  }
664 
665  /// Returns a pointer to mutable composite event collection if
666  /// @p target_system is a subsystem of this, nullptr is returned otherwise.
668  const System<T>& target_system,
669  CompositeEventCollection<T>* events) const final {
670  if (&target_system == this)
671  return events;
672 
673  return GetSubsystemStuff<CompositeEventCollection<T>,
675  target_system, events,
678  }
679 
680  /// Returns a pointer to const composite event collection if
681  /// @p target_system is a subsystem of this, nullptr is returned otherwise.
683  const System<T>& target_system,
684  const CompositeEventCollection<T>* events) const final {
685  if (&target_system == this)
686  return events;
687 
688  return GetSubsystemStuff<const CompositeEventCollection<T>,
690  target_system, events,
693  }
694 
695  /// The @p generalized_velocity vector must have the same size and ordering as
696  /// the generalized velocity in the ContinuousState that this Diagram reserves
697  /// in its context.
699  const Context<T>& context,
700  const Eigen::Ref<const VectorX<T>>& generalized_velocity,
701  VectorBase<T>* qdot) const override {
702  // Check that the dimensions of the continuous state in the context match
703  // the dimensions of the provided generalized velocity and configuration
704  // derivatives.
705  const ContinuousState<T>& xc = context.get_continuous_state();
706  const int nq = xc.get_generalized_position().size();
707  const int nv = xc.get_generalized_velocity().size();
708  DRAKE_DEMAND(nq == qdot->size());
709  DRAKE_DEMAND(nv == generalized_velocity.size());
710 
711  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
712  DRAKE_DEMAND(diagram_context != nullptr);
713 
714  // Iterate over the subsystems, asking each subsystem to map its subslice of
715  // velocity to configuration derivatives. This approach is valid because the
716  // DiagramContinuousState guarantees that the subsystem states are
717  // concatenated in order.
718  int v_index = 0; // The next index to read in generalized_velocity.
719  int q_index = 0; // The next index to write in qdot.
720  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
721  // Find the continuous state of subsystem i.
722  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
723  const ContinuousState<T>& sub_xc = subcontext.get_continuous_state();
724 
725  // Select the chunk of generalized_velocity belonging to subsystem i.
726  const int num_v = sub_xc.get_generalized_velocity().size();
727  const Eigen::Ref<const VectorX<T>>& v_slice =
728  generalized_velocity.segment(v_index, num_v);
729 
730  // Select the chunk of qdot belonging to subsystem i.
731  const int num_q = sub_xc.get_generalized_position().size();
732  Subvector<T> dq_slice(qdot, q_index, num_q);
733 
734  // Delegate the actual mapping to subsystem i itself.
735  registered_systems_[i]->MapVelocityToQDot(subcontext, v_slice, &dq_slice);
736 
737  // Advance the indices.
738  v_index += num_v;
739  q_index += num_q;
740  }
741  }
742 
743  /// The @p generalized_velocity vector must have the same size and ordering as
744  /// the generalized velocity in the ContinuousState that this Diagram reserves
745  /// in its context.
746  void DoMapQDotToVelocity(const Context<T>& context,
747  const Eigen::Ref<const VectorX<T>>& qdot,
748  VectorBase<T>* generalized_velocity) const override {
749  // Check that the dimensions of the continuous state in the context match
750  // the dimensions of the provided generalized velocity and configuration
751  // derivatives.
752  const ContinuousState<T>& xc = context.get_continuous_state();
753  const int nq = xc.get_generalized_position().size();
754  const int nv = xc.get_generalized_velocity().size();
755  DRAKE_DEMAND(nq == qdot.size());
756  DRAKE_DEMAND(nv == generalized_velocity->size());
757 
758  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
759  DRAKE_DEMAND(diagram_context != nullptr);
760 
761  // Iterate over the subsystems, asking each subsystem to map its subslice of
762  // configuration derivatives to velocity. This approach is valid because the
763  // DiagramContinuousState guarantees that the subsystem states are
764  // concatenated in order.
765  int q_index = 0; // The next index to read in qdot.
766  int v_index = 0; // The next index to write in generalized_velocity.
767  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
768  // Find the continuous state of subsystem i.
769  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
770  const ContinuousState<T>& sub_xc = subcontext.get_continuous_state();
771 
772  // Select the chunk of qdot belonging to subsystem i.
773  const int num_q = sub_xc.get_generalized_position().size();
774  const Eigen::Ref<const VectorX<T>>& dq_slice =
775  qdot.segment(q_index, num_q);
776 
777  // Select the chunk of generalized_velocity belonging to subsystem i.
778  const int num_v = sub_xc.get_generalized_velocity().size();
779  Subvector<T> v_slice(generalized_velocity, v_index, num_v);
780 
781  // Delegate the actual mapping to subsystem i itself.
782  registered_systems_[i]->MapQDotToVelocity(subcontext, dq_slice, &v_slice);
783 
784  // Advance the indices.
785  v_index += num_v;
786  q_index += num_q;
787  }
788  }
789 
790  /// Computes the next update time based on the configured actions, for scalar
791  /// types that are arithmetic, or aborts for scalar types that are not
792  /// arithmetic.
793  void DoCalcNextUpdateTime(const Context<T>& context,
794  CompositeEventCollection<T>* event_info,
795  T* time) const override {
796  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
797  auto info = dynamic_cast<DiagramCompositeEventCollection<T>*>(event_info);
798  DRAKE_DEMAND(diagram_context != nullptr);
799  DRAKE_DEMAND(info != nullptr);
800 
801  *time = std::numeric_limits<double>::infinity();
802 
803  // Iterate over the subsystems, and harvest the most imminent updates.
804  std::vector<T> times(num_subsystems());
805  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
806  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
807  CompositeEventCollection<T>& subinfo =
808  info->get_mutable_subevent_collection(i);
809  const T sub_time =
810  registered_systems_[i]->CalcNextUpdateTime(subcontext, &subinfo);
811  times[i] = sub_time;
812 
813  if (sub_time < *time) {
814  *time = sub_time;
815  }
816  }
817 
818  // For all the subsystems whose next update time is bigger than *time,
819  // clear their event collections.
820  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
821  if (times[i] > *time)
822  info->get_mutable_subevent_collection(i).Clear();
823  }
824  }
825 
827  const InputPort<T>& input_port) const override {
828  // Ask the subsystem to perform the allocation.
829  const InputPortLocator& id = input_port_ids_[input_port.get_index()];
830  const System<T>* subsystem = id.first;
831  const InputPortIndex subindex = id.second;
832  return subsystem->AllocateInputVector(subsystem->get_input_port(subindex))
833  .release();
834  }
835 
837  const InputPort<T>& input_port) const override {
838  // Ask the subsystem to perform the allocation.
839  const InputPortLocator& id = input_port_ids_[input_port.get_index()];
840  const System<T>* subsystem = id.first;
841  const InputPortIndex subindex = id.second;
842  return subsystem->AllocateInputAbstract(subsystem->get_input_port(subindex))
843  .release();
844  }
845 
846  private:
847  // Allocates a default-constructed diagram context containing the complete
848  // diagram substructure of default-constructed subcontexts.
849  std::unique_ptr<ContextBase> DoAllocateContext() const final {
850  // Reserve inputs as specified during Diagram initialization.
851  auto context = std::make_unique<DiagramContext<T>>(num_subsystems());
852  this->InitializeContextBase(&*context);
853 
854  // Recursively construct each constituent system and its subsystems,
855  // then add to this diagram Context.
856  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
857  const System<T>& system = *registered_systems_[i];
858  auto subcontext = dynamic_pointer_cast_or_throw<Context<T>>(
859  system.AllocateContext());
860  context->AddSystem(i, std::move(subcontext));
861  }
862 
863  // Creates this diagram's composite data structures that collect its
864  // subsystems' resources, which must have already been allocated above.
865  // No dependencies are set up in these two calls.
866  context->MakeParameters();
867  context->MakeState();
868 
869  // Subscribe each of the Diagram's composite-entity dependency trackers to
870  // the trackers for the corresponding constituent entities in the child
871  // subsystems. This ensures that changes made at the subcontext level are
872  // propagated correctly to the diagram context level. That includes state
873  // and parameter changes, as well as local changes that affect composite
874  // diagram-level computations like xcdot and pe.
875  context->SubscribeDiagramCompositeTrackersToChildrens();
876 
877  // Peer-to-peer connections wire a child subsystem's input port to a
878  // child subsystem's output port. Subscribe each child input port to the
879  // child output port on which it depends.
880  for (const auto& connection : connection_map_) {
881  const OutputPortLocator& src = connection.second;
882  const InputPortLocator& dest = connection.first;
883  context->SubscribeInputPortToOutputPort(
884  ConvertToContextPortIdentifier(src),
885  ConvertToContextPortIdentifier(dest));
886  }
887 
888  // Diagram-external input ports are exported from child subsystems (meaning
889  // the Diagram input is fed into the input of one of its children.
890  // Subscribe the child subsystem's input port to its parent Diagram's input
891  // port on which it depends.
892  for (InputPortIndex i(0); i < this->get_num_input_ports(); ++i) {
893  const InputPortLocator& id = input_port_ids_[i];
894  context->SubscribeExportedInputPortToDiagramPort(
895  i, ConvertToContextPortIdentifier(id));
896  }
897 
898  // Diagram-external output ports are exported from child subsystem output
899  // ports. Subscribe each Diagram-level output to the child-level output on
900  // which it depends.
901  for (OutputPortIndex i(0); i < this->get_num_output_ports(); ++i) {
902  const OutputPortLocator& id = output_port_ids_[i];
903  context->SubscribeDiagramPortToExportedOutputPort(
904  i, ConvertToContextPortIdentifier(id));
905  }
906 
907  // TODO(sherm1) Remove this line and the corresponding one in
908  // LeafSystem to enable caching by default in Drake (issue #9205).
909  context->DisableCaching();
910 
911  return context;
912  }
913 
914  // Evaluates the value of the specified subsystem input
915  // port in the given context. The port has already been determined _not_ to
916  // be a fixed port, so it must be connected either
917  // - to the output port of a peer subsystem, or
918  // - to an input port of this Diagram,
919  // - or not connected at all in which case we return null.
920  const AbstractValue* EvalConnectedSubsystemInputPort(
921  const ContextBase& context_base,
922  const InputPortBase& input_port_base) const final {
923  auto& diagram_context =
924  dynamic_cast<const DiagramContext<T>&>(context_base);
925  auto& system =
926  dynamic_cast<const System<T>&>(input_port_base.get_system_base());
927  const InputPortLocator id{&system, input_port_base.get_index()};
928 
929  // Find if this input port is exported (connected to an input port of this
930  // containing diagram).
931  // TODO(sherm1) Fix this. Shouldn't have to search.
932  const auto external_it =
933  std::find(input_port_ids_.begin(), input_port_ids_.end(), id);
934  const bool is_exported = (external_it != input_port_ids_.end());
935 
936  // Find if this input port is connected to an output port.
937  // TODO(sherm1) Fix this. Shouldn't have to search.
938  const auto upstream_it = connection_map_.find(id);
939  const bool is_connected = (upstream_it != connection_map_.end());
940 
941  if (!(is_exported || is_connected))
942  return nullptr;
943 
944  DRAKE_DEMAND(is_exported ^ is_connected);
945 
946  if (is_exported) {
947  // The upstream source is an input to this whole Diagram; evaluate that
948  // input port and use the result as the value for this one.
949  const InputPortIndex i(external_it - input_port_ids_.begin());
950  return this->EvalAbstractInput(diagram_context, i);
951  }
952 
953  // The upstream source is an output port of one of this Diagram's child
954  // subsystems; evaluate it.
955  // TODO(david-german-tri): Add online algebraic loop detection here.
956  DRAKE_ASSERT(is_connected);
957  const OutputPortLocator& prerequisite = upstream_it->second;
958  return &this->EvalSubsystemOutputPort(diagram_context, prerequisite);
959  }
960 
961  std::string GetParentPathname() const final {
962  return this->GetSystemPathname();
963  }
964 
965  // Returns true if there might be direct feedthrough from the given
966  // @p input_port of the Diagram to the given @p output_port of the Diagram.
967  bool DoHasDirectFeedthrough(int input_port, int output_port) const {
968  DRAKE_ASSERT(input_port >= 0);
969  DRAKE_ASSERT(input_port < this->get_num_input_ports());
970  DRAKE_ASSERT(output_port >= 0);
971  DRAKE_ASSERT(output_port < this->get_num_output_ports());
972 
973  const InputPortLocator& target_input_id = input_port_ids_[input_port];
974 
975  // Search the graph for a direct-feedthrough connection from the output_port
976  // back to the input_port. Maintain a set of the output port identifiers
977  // that are known to have a direct-feedthrough path to the output_port.
978  std::set<OutputPortLocator> active_set;
979  active_set.insert(output_port_ids_[output_port]);
980  while (!active_set.empty()) {
981  const OutputPortLocator current_output_id = *active_set.begin();
982  size_t removed_count = active_set.erase(current_output_id);
983  DRAKE_ASSERT(removed_count == 1);
984  const System<T>* sys = current_output_id.first;
985  for (InputPortIndex i(0); i < sys->get_num_input_ports(); ++i) {
986  if (sys->HasDirectFeedthrough(i, current_output_id.second)) {
987  const InputPortLocator curr_input_id(sys, i);
988  if (curr_input_id == target_input_id) {
989  // We've found a direct-feedthrough path to the input_port.
990  return true;
991  } else {
992  // We've found an intermediate input port has a direct-feedthrough
993  // path to the output_port. Add the upstream output port (if there
994  // is one) to the active set.
995  auto it = connection_map_.find(curr_input_id);
996  if (it != connection_map_.end()) {
997  const OutputPortLocator& upstream_output = it->second;
998  active_set.insert(upstream_output);
999  }
1000  }
1001  }
1002  }
1003  }
1004  // If there are no intermediate output ports with a direct-feedthrough path
1005  // to the output_port, there is no direct feedthrough to it from the
1006  // input_port.
1007  return false;
1008  }
1009 
1010  template <typename EventType>
1011  std::unique_ptr<EventCollection<EventType>> AllocateForcedEventCollection(
1012  std::function<
1013  std::unique_ptr<EventCollection<EventType>>(const System<T>*)>
1014  allocater_func) const {
1015  const int num_systems = num_subsystems();
1016  auto ret = std::make_unique<DiagramEventCollection<EventType>>(num_systems);
1017  for (SubsystemIndex i(0); i < num_systems; ++i) {
1018  std::unique_ptr<EventCollection<EventType>> subevent_collection =
1019  allocater_func(registered_systems_[i].get());
1020  ret->set_and_own_subevent_collection(i, std::move(subevent_collection));
1021  }
1022  return ret;
1023  }
1024 
1025  // For each subsystem, if there is a publish event in its corresponding
1026  // subevent collection, calls its Publish method with the appropriate
1027  // subcontext and subevent collection.
1028  void DispatchPublishHandler(
1029  const Context<T>& context,
1030  const EventCollection<PublishEvent<T>>& event_info) const final {
1031  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1032  DRAKE_DEMAND(diagram_context);
1034  dynamic_cast<const DiagramEventCollection<PublishEvent<T>>&>(
1035  event_info);
1036 
1037  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1038  const EventCollection<PublishEvent<T>>& subinfo =
1039  info.get_subevent_collection(i);
1040 
1041  if (subinfo.HasEvents()) {
1042  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1043  registered_systems_[i]->Publish(subcontext, subinfo);
1044  }
1045  }
1046  }
1047 
1048  // For each subsystem, if there is a discrete update event in its
1049  // corresponding subevent collection, calls its CalcDiscreteVariableUpdates
1050  // method with the appropriate subcontext, subevent collection and
1051  // substate.
1052  void DispatchDiscreteVariableUpdateHandler(
1053  const Context<T>& context,
1054  const EventCollection<DiscreteUpdateEvent<T>>& event_info,
1055  DiscreteValues<T>* discrete_state) const final {
1056  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1057  DRAKE_DEMAND(diagram_context);
1058  auto diagram_discrete =
1059  dynamic_cast<DiagramDiscreteValues<T>*>(discrete_state);
1060  DRAKE_DEMAND(diagram_discrete);
1061 
1062  // As a baseline, initialize all the discrete variables to their
1063  // current values.
1064  diagram_discrete->CopyFrom(context.get_discrete_state());
1065 
1067  dynamic_cast<const DiagramEventCollection<DiscreteUpdateEvent<T>>&>(
1068  event_info);
1069 
1070  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1071  const EventCollection<DiscreteUpdateEvent<T>>& subinfo =
1072  info.get_subevent_collection(i);
1073 
1074  if (subinfo.HasEvents()) {
1075  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1076  DiscreteValues<T>& subdiscrete =
1077  diagram_discrete->get_mutable_subdiscrete(i);
1078 
1079  registered_systems_[i]->CalcDiscreteVariableUpdates(subcontext, subinfo,
1080  &subdiscrete);
1081  }
1082  }
1083  }
1084 
1085  // For each subsystem, if there is an unrestricted update event in its
1086  // corresponding subevent collection, calls its CalcUnrestrictedUpdate
1087  // method with the appropriate subcontext, subevent collection and substate.
1088  void DispatchUnrestrictedUpdateHandler(
1089  const Context<T>& context,
1090  const EventCollection<UnrestrictedUpdateEvent<T>>& event_info,
1091  State<T>* state) const final {
1092  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1093  DRAKE_DEMAND(diagram_context);
1094  auto diagram_state = dynamic_cast<DiagramState<T>*>(state);
1095  DRAKE_DEMAND(diagram_state != nullptr);
1096 
1097  // No need to set state to context's state, since it has already been done
1098  // in System::CalcUnrestrictedUpdate().
1099 
1102  event_info);
1103 
1104  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1106  info.get_subevent_collection(i);
1107 
1108  if (subinfo.HasEvents()) {
1109  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1110  State<T>& substate = diagram_state->get_mutable_substate(i);
1111 
1112  registered_systems_[i]->CalcUnrestrictedUpdate(subcontext, subinfo,
1113  &substate);
1114  }
1115  }
1116  }
1117 
1118  // Tries to recursively find @p target_system's BaseStuff
1119  // (context / state / etc). nullptr is returned if @p target_system is not
1120  // a subsystem of this diagram. This template function should only be used
1121  // to reduce code repetition for DoGetMutableTargetSystemContext(),
1122  // DoGetTargetSystemContext(), DoGetMutableTargetSystemState(), and
1123  // DoGetTargetSystemState().
1124  // @param target_system The subsystem of interest.
1125  // @param my_stuff BaseStuff that's associated with this diagram.
1126  // @param recursive_getter A member function of System that returns sub
1127  // context or state. Should be one of the four functions listed above.
1128  // @param get_child_stuff A member function of DiagramContext or DiagramState
1129  // that returns context or state given the index of the subsystem.
1130  //
1131  // @tparam BaseStuff Can be Context<T>, const Context<T>, State<T> and
1132  // const State<T>.
1133  // @tparam DerivedStuff Can be DiagramContext<T>,
1134  // const DiagramContext<T>, DiagramState<T> and const DiagramState<T>.
1135  //
1136  // @pre @p target_system cannot be `this`. The caller should check for this
1137  // edge case.
1138  template <typename BaseStuff, typename DerivedStuff>
1139  BaseStuff* GetSubsystemStuff(
1140  const System<T>& target_system, BaseStuff* my_stuff,
1141  std::function<BaseStuff*(const System<T>*, const System<T>&, BaseStuff*)>
1142  recursive_getter,
1143  std::function<BaseStuff&(DerivedStuff*, SubsystemIndex)> get_child_stuff)
1144  const {
1145  static_assert(
1146  std::is_same<BaseStuff,
1148  "BaseStuff cannot be a pointer");
1149  static_assert(
1150  std::is_same<DerivedStuff,
1152  "DerivedStuff cannot be a pointer");
1153 
1154  DRAKE_DEMAND(my_stuff != nullptr);
1155  DRAKE_DEMAND(&target_system != this);
1156  DerivedStuff& my_stuff_as_derived = dynamic_cast<DerivedStuff&>(*my_stuff);
1157 
1158  SubsystemIndex index(0);
1159  for (const auto& child : registered_systems_) {
1160  BaseStuff& child_stuff = get_child_stuff(&my_stuff_as_derived, index);
1161 
1162  BaseStuff* const target_stuff =
1163  recursive_getter(child.get(), target_system, &child_stuff);
1164 
1165  if (target_stuff != nullptr) {
1166  return target_stuff;
1167  }
1168  ++index;
1169  }
1170 
1171  return nullptr;
1172  }
1173 
1174  // Uses this Diagram<T> to manufacture a Diagram<NewType>::Blueprint,
1175  // using system scalar conversion.
1176  //
1177  // @tparam NewType The scalar type to which to convert.
1178  template <typename NewType>
1179  std::unique_ptr<typename Diagram<NewType>::Blueprint> ConvertScalarType()
1180  const {
1181  std::vector<std::unique_ptr<System<NewType>>> new_systems;
1182  // Recursively convert all the subsystems.
1183  std::map<const System<T>*, const System<NewType>*> old_to_new_map;
1184  for (const auto& old_system : registered_systems_) {
1185  // Convert old_system to new_system using the old_system's converter.
1186  std::unique_ptr<System<NewType>> new_system =
1187  old_system->get_system_scalar_converter().
1188  template Convert<NewType>(*old_system);
1189  DRAKE_DEMAND(new_system != nullptr);
1190 
1191  // Update our mapping and take ownership.
1192  old_to_new_map[old_system.get()] = new_system.get();
1193  new_systems.push_back(std::move(new_system));
1194  }
1195 
1196  // Set up the blueprint.
1197  auto blueprint = std::make_unique<typename Diagram<NewType>::Blueprint>();
1198  // Make all the inputs and outputs.
1199  for (const InputPortLocator& id : input_port_ids_) {
1200  const System<NewType>* new_system = old_to_new_map[id.first];
1201  const InputPortIndex port = id.second;
1202  blueprint->input_port_ids.emplace_back(new_system, port);
1203  }
1204  for (InputPortIndex i{0}; i < this->get_num_input_ports(); i++) {
1205  blueprint->input_port_names.emplace_back(
1206  this->get_input_port(i).get_name());
1207  }
1208  for (const OutputPortLocator& id : output_port_ids_) {
1209  const System<NewType>* new_system = old_to_new_map[id.first];
1210  const OutputPortIndex port = id.second;
1211  blueprint->output_port_ids.emplace_back(new_system, port);
1212  }
1213  // Make all the connections.
1214  for (const auto& edge : connection_map_) {
1215  const InputPortLocator& old_dest = edge.first;
1216  const System<NewType>* const dest_system = old_to_new_map[old_dest.first];
1217  const InputPortIndex dest_port = old_dest.second;
1218  const typename Diagram<NewType>::InputPortLocator new_dest{dest_system,
1219  dest_port};
1220 
1221  const OutputPortLocator& old_src = edge.second;
1222  const System<NewType>* const src_system = old_to_new_map[old_src.first];
1223  const OutputPortIndex src_port = old_src.second;
1224  const typename Diagram<NewType>::OutputPortLocator new_src{src_system,
1225  src_port};
1226 
1227  blueprint->connection_map[new_dest] = new_src;
1228  }
1229  // Move the new systems into the blueprint.
1230  blueprint->systems = std::move(new_systems);
1231 
1232  return blueprint;
1233  }
1234 
1235  std::map<PeriodicEventData, std::vector<const Event<T>*>,
1236  PeriodicEventDataComparator> DoGetPeriodicEvents() const override {
1237  std::map<PeriodicEventData,
1238  std::vector<const Event<T>*>,
1239  PeriodicEventDataComparator> periodic_events_map;
1240 
1241  for (int i = 0; i < num_subsystems(); ++i) {
1242  auto sub_map = registered_systems_[i]->GetPeriodicEvents();
1243  for (const auto& sub_attr_events : sub_map) {
1244  const auto& sub_vec = sub_attr_events.second;
1245  auto& vec = periodic_events_map[sub_attr_events.first];
1246  vec.insert(vec.end(), sub_vec.begin(), sub_vec.end());
1247  }
1248  }
1249 
1250  return periodic_events_map;
1251  }
1252 
1253  void DoGetPerStepEvents(
1254  const Context<T>& context,
1255  CompositeEventCollection<T>* event_info) const override {
1256  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1257  auto info = dynamic_cast<DiagramCompositeEventCollection<T>*>(event_info);
1258  DRAKE_DEMAND(diagram_context != nullptr);
1259  DRAKE_DEMAND(info != nullptr);
1260 
1261  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1262  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1263  CompositeEventCollection<T>& subinfo =
1264  info->get_mutable_subevent_collection(i);
1265 
1266  registered_systems_[i]->GetPerStepEvents(subcontext, &subinfo);
1267  }
1268  }
1269 
1270  void DoGetInitializationEvents(
1271  const Context<T>& context,
1272  CompositeEventCollection<T>* event_info) const override {
1273  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1274  auto info = dynamic_cast<DiagramCompositeEventCollection<T>*>(event_info);
1275  DRAKE_DEMAND(diagram_context != nullptr);
1276  DRAKE_DEMAND(info != nullptr);
1277 
1278  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1279  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1280  CompositeEventCollection<T>& subinfo =
1281  info->get_mutable_subevent_collection(i);
1282 
1283  registered_systems_[i]->GetInitializationEvents(subcontext, &subinfo);
1284  }
1285  }
1286 
1287  // A structural outline of a Diagram, produced by DiagramBuilder.
1288  struct Blueprint {
1289  // The ordered subsystem ports that are inputs to the entire diagram.
1290  std::vector<InputPortLocator> input_port_ids;
1291  // The names should be the same length and ordering as the ids.
1292  std::vector<std::string> input_port_names;
1293 
1294  // The ordered subsystem ports that are outputs of the entire diagram.
1295  std::vector<OutputPortLocator> output_port_ids;
1296  // A map from the input ports of constituent systems to the output ports
1297  // on which they depend. This graph is possibly cyclic, but must not
1298  // contain an algebraic loop.
1299  std::map<InputPortLocator, OutputPortLocator> connection_map;
1300  // All of the systems to be included in the diagram.
1301  std::vector<std::unique_ptr<System<T>>> systems;
1302  };
1303 
1304  // Constructs a Diagram from the Blueprint that a DiagramBuilder produces.
1305  // This constructor is private because only DiagramBuilder calls it. The
1306  // constructor takes the systems from the blueprint.
1307  explicit Diagram(std::unique_ptr<Blueprint> blueprint) : Diagram() {
1308  Initialize(std::move(blueprint));
1309  }
1310 
1311  // Validates the given @p blueprint and sets up the Diagram accordingly.
1312  void Initialize(std::unique_ptr<Blueprint> blueprint) {
1313  // We must be given something to own.
1314  DRAKE_DEMAND(!blueprint->systems.empty());
1315  // We must not already own any subsystems.
1316  DRAKE_DEMAND(registered_systems_.empty());
1317 
1318  // Move the data from the blueprint into private member variables.
1319  connection_map_ = std::move(blueprint->connection_map);
1320  input_port_ids_ = std::move(blueprint->input_port_ids);
1321  output_port_ids_ = std::move(blueprint->output_port_ids);
1322  registered_systems_ = std::move(blueprint->systems);
1323 
1324  // Generate a map from the System pointer to its index in the registered
1325  // order.
1326  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1327  system_index_map_[registered_systems_[i].get()] = i;
1328  SystemBase::set_parent_service(registered_systems_[i].get(), this);
1329  }
1330 
1331  // Generate constraints for the diagram from the constraints on the
1332  // subsystems.
1333  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1334  const auto sys = registered_systems_[i].get();
1335  for (SystemConstraintIndex j(0); j < sys->get_num_constraints(); ++j) {
1336  const auto c = &(sys->get_constraint(j));
1337  typename SystemConstraint<T>::CalcCallback diagram_calc =
1338  [this, sys, c](const Context<T>& context, VectorX<T>* value) {
1339  c->Calc(this->GetSubsystemContext(*sys, context), value);
1340  };
1341  this->AddConstraint(std::make_unique<SystemConstraint<T>>(
1342  diagram_calc, c->size(), c->type(),
1343  sys->get_name() + ":" + c->description()));
1344  }
1345  }
1346 
1347  // Every system must appear exactly once.
1348  DRAKE_DEMAND(registered_systems_.size() == system_index_map_.size());
1349  // Every port named in the connection_map_ must actually exist.
1350  DRAKE_ASSERT(PortsAreValid());
1351  // Every subsystem must have a unique name.
1352  DRAKE_THROW_UNLESS(NamesAreUniqueAndNonEmpty());
1353 
1354  // Add the inputs to the Diagram topology, and check their invariants.
1355  DRAKE_DEMAND(input_port_ids_.size() ==
1356  blueprint->input_port_names.size());
1357  std::vector<std::string>::iterator name_iter =
1358  blueprint->input_port_names.begin();
1359  for (const InputPortLocator& id : input_port_ids_) {
1360  ExportInput(id, *name_iter++);
1361  }
1362  for (const OutputPortLocator& id : output_port_ids_) {
1363  ExportOutput(id);
1364  }
1365 
1366  // Identify the intersection of the subsystems' scalar conversion support.
1367  // Remove all conversions that at least one subsystem did not support.
1368  SystemScalarConverter& this_scalar_converter =
1369  SystemImpl::get_mutable_system_scalar_converter(this);
1370  for (const auto& system : registered_systems_) {
1371  this_scalar_converter.RemoveUnlessAlsoSupportedBy(
1372  system->get_system_scalar_converter());
1373  }
1374 
1376  AllocateForcedEventCollection<PublishEvent<T>>(
1379  AllocateForcedEventCollection<DiscreteUpdateEvent<T>>(
1382  AllocateForcedEventCollection<UnrestrictedUpdateEvent<T>>(
1384  }
1385 
1386  // Exposes the given port as an input of the Diagram.
1387  void ExportInput(const InputPortLocator& port, const std::string& name) {
1388  const System<T>* const sys = port.first;
1389  const int port_index = port.second;
1390  // Fail quickly if this system is not part of the diagram.
1391  GetSystemIndexOrAbort(sys);
1392 
1393  // Add this port to our externally visible topology.
1394  const auto& subsystem_input_port = sys->get_input_port(port_index);
1395  this->DeclareInputPort(subsystem_input_port.get_data_type(),
1396  subsystem_input_port.size(), name,
1397  subsystem_input_port.get_random_type());
1398  }
1399 
1400  // Exposes the given subsystem output port as an output of the Diagram.
1401  void ExportOutput(const OutputPortLocator& port) {
1402  const System<T>* const sys = port.first;
1403  const int port_index = port.second;
1404  const auto& source_output_port = sys->get_output_port(port_index);
1405  // TODO(sherm1) Use implicit_cast when available (from abseil).
1406  auto diagram_port = std::make_unique<DiagramOutputPort<T>>(
1407  this, // implicit_cast<const System<T>*>(this)
1408  this, // implicit_cast<SystemBase*>(this)
1411  &source_output_port,
1412  GetSystemIndexOrAbort(&source_output_port.get_system()));
1413  this->AddOutputPort(std::move(diagram_port));
1414  }
1415 
1416  // Returns a reference to the value in the given context, of the specified
1417  // output port of one of this Diagram's immediate subsystems, recalculating
1418  // if necessary to bring the value up to date.
1419  const AbstractValue& EvalSubsystemOutputPort(
1420  const DiagramContext<T>& context, const OutputPortLocator& id) const {
1421  const System<T>* const system = id.first;
1422  const OutputPortIndex port_index(id.second);
1423  const OutputPort<T>& port = system->get_output_port(port_index);
1424  const SubsystemIndex i = GetSystemIndexOrAbort(system);
1425  SPDLOG_TRACE(log(), "Evaluating output for subsystem {}, port {}",
1426  system->GetSystemPathname(), port_index);
1427  const Context<T>& subsystem_context = context.GetSubsystemContext(i);
1428  return port.EvalAbstract(subsystem_context);
1429  }
1430 
1431  // Converts an InputPortLocator to a DiagramContext::InputPortIdentifier.
1432  // The DiagramContext::InputPortIdentifier contains the index of the System in
1433  // the diagram, instead of an actual pointer to the System.
1434  // TODO(sherm1) Should just use the (SystemIndex,PortIndex) form everywhere.
1436  ConvertToContextPortIdentifier(const InputPortLocator& locator) const {
1437  typename DiagramContext<T>::InputPortIdentifier identifier;
1438  identifier.first = GetSystemIndexOrAbort(locator.first);
1439  identifier.second = locator.second;
1440  return identifier;
1441  }
1442 
1443  // Converts an OutputPortLocator to a DiagramContext::OutputPortIdentifier.
1444  // The DiagramContext::OutputPortIdentifier contains the index of the System
1445  // in the diagram, instead of an actual pointer to the System.
1447  ConvertToContextPortIdentifier(const OutputPortLocator& locator) const {
1448  typename DiagramContext<T>::OutputPortIdentifier identifier;
1449  identifier.first = GetSystemIndexOrAbort(locator.first);
1450  identifier.second = locator.second;
1451  return identifier;
1452  }
1453 
1454  // Returns true if every port mentioned in the connection map exists.
1455  bool PortsAreValid() const {
1456  for (const auto& entry : connection_map_) {
1457  const InputPortLocator& dest = entry.first;
1458  const OutputPortLocator& src = entry.second;
1459  if (dest.second < 0 || dest.second >= dest.first->get_num_input_ports()) {
1460  return false;
1461  }
1462  if (src.second < 0 || src.second >= src.first->get_num_output_ports()) {
1463  return false;
1464  }
1465  }
1466  return true;
1467  }
1468 
1469  // Returns true if every subsystem has a unique, non-empty name.
1470  // O(N * log(N)) in the number of subsystems.
1471  bool NamesAreUniqueAndNonEmpty() const {
1472  std::set<std::string> names;
1473  for (const auto& system : registered_systems_) {
1474  const std::string& name = system->get_name();
1475  if (name.empty()) {
1476  // This can only happen if someone blanks out the name *after* adding
1477  // it to DiagramBuilder; if an empty name is given to DiagramBuilder,
1478  // a default non-empty name is automatically assigned.
1479  log()->error("Subsystem of type {} has no name",
1480  NiceTypeName::Get(*system));
1481  // We skip names.insert here, so that the return value will be false.
1482  continue;
1483  }
1484  if (names.find(name) != names.end()) {
1485  log()->error("Non-unique name \"{}\" for subsystem of type {}", name,
1486  NiceTypeName::Get(*system));
1487  }
1488  names.insert(name);
1489  }
1490  return names.size() == registered_systems_.size();
1491  }
1492 
1493  int num_subsystems() const {
1494  return static_cast<int>(registered_systems_.size());
1495  }
1496 
1497  // A map from the input ports of constituent systems, to the output ports of
1498  // the systems from which they get their values.
1499  std::map<InputPortLocator, OutputPortLocator> connection_map_;
1500 
1501  // The Systems in this Diagram, which are owned by this Diagram, in the order
1502  // they were registered. Index by SubsystemIndex.
1503  std::vector<std::unique_ptr<System<T>>> registered_systems_;
1504 
1505  // Map to quickly satisfy "What is the subsystem index of the child system?"
1506  std::map<const System<T>*, SubsystemIndex> system_index_map_;
1507 
1508  // The ordered inputs and outputs of this Diagram. Index by InputPortIndex
1509  // and OutputPortIndex.
1510  std::vector<InputPortLocator> input_port_ids_;
1511  std::vector<OutputPortLocator> output_port_ids_;
1512 
1513  // For all T, Diagram<T> considers DiagramBuilder<T> a friend, so that the
1514  // builder can set the internal state correctly.
1515  friend class DiagramBuilder<T>;
1516 
1517  // For any T1 & T2, Diagram<T1> considers Diagram<T2> a friend, so that
1518  // Diagram can provide transmogrification methods across scalar types.
1519  // See Diagram<T>::ConvertScalarType.
1520  template <typename> friend class Diagram;
1521 };
1522 
1523 } // namespace systems
1524 } // namespace drake
const EventCollection< EventType > & get_subevent_collection(int index) const
Returns a const pointer to subsystem&#39;s EventCollection at index.
Definition: event_collection.h:221
const ContinuousState< T > * DoGetTargetSystemContinuousState(const System< T > &target_system, const ContinuousState< T > *xc) const final
Returns a pointer to const state if target_system is a subsystem of this, nullptr is returned otherwi...
Definition: diagram.h:639
Context< T > * DoGetMutableTargetSystemContext(const System< T > &target_system, Context< T > *context) const final
Returns a pointer to mutable context if target_system is a sub system of this, nullptr is returned ot...
Definition: diagram.h:600
void DoMapQDotToVelocity(const Context< T > &context, const Eigen::Ref< const VectorX< T >> &qdot, VectorBase< T > *generalized_velocity) const override
The generalized_velocity vector must have the same size and ordering as the generalized velocity in t...
Definition: diagram.h:746
virtual int size() const =0
Returns the number of elements in the vector.
const ContinuousState< T > & GetSubsystemDerivatives(const ContinuousState< T > &derivatives, const System< T > *subsystem) const
Retrieves the state derivatives for a particular subsystem from the derivatives for the entire diagra...
Definition: diagram.h:293
DiscreteValues is a container for numerical but non-continuous state and parameters.
Definition: discrete_values.h:33
std::multimap< int, int > GetDirectFeedthroughs() const final
Reports all direct feedthroughs from input ports to output ports.
Definition: diagram.h:69
const SystemScalarConverter & get_system_scalar_converter() const
(Advanced) Returns the SystemScalarConverter for this object.
Definition: system.h:1407
int i
Definition: reset_after_move_test.cc:51
BasicVector< T > & get_mutable_numeric_parameter(int index)
Returns the vector-valued parameter at index.
Definition: parameters.h:82
MixedIntegerRotationConstraintGenerator::ReturnType ret
Definition: mixed_integer_rotation_constraint_test.cc:188
void GetGraphvizOutputPortToken(const OutputPort< T > &port, std::stringstream *dot) const override
Appends a fragment to the dot stream identifying the graphviz node representing port.
Definition: diagram.h:475
double value
Definition: wrap_test_util_py.cc:12
void SetDefaultState(const Context< T > &context, State< T > *state) const override
Assigns default values to all elements of the state.
Definition: diagram.h:96
T DoCalcWitnessValue(const Context< T > &context, const WitnessFunction< T > &witness_func) const final
For the subsystem associated with witness_func, gets its subcontext from context, passes the subconte...
Definition: diagram.h:526
void InitializeContextBase(ContextBase *context) const
This method must be invoked from within derived class DoAllocateContext() implementations right after...
Definition: system_base.cc:55
const Context< T > & GetSubsystemContext(SubsystemIndex index) const
Returns the context structure for a given constituent system index.
Definition: diagram_context.h:338
int get_num_output_ports() const
Returns the number of output ports currently allocated in this System.
Definition: system_base.h:178
std::pair< const System< double > *, OutputPortIndex > OutputPortLocator
Definition: diagram.h:50
Definition: bullet_model.cc:22
const AbstractValue & EvalAbstract(const Context< T > &context) const
Returns a reference to the value of this output port contained in the given Context.
Definition: output_port.h:128
std::mt19937 RandomGenerator
Defines the implementation of the stdc++ concept UniformRandomBitGenerator to be used by the Systems ...
Definition: system.h:73
void DoCalcNextUpdateTime(const Context< T > &context, CompositeEventCollection< T > *event_info, T *time) const override
Computes the next update time based on the configured actions, for scalar types that are arithmetic...
Definition: diagram.h:793
Diagram()
Constructs an uninitialized Diagram.
Definition: diagram.h:505
void set_numeric_parameters(std::unique_ptr< DiscreteValues< T >> numeric_params)
Definition: parameters.h:90
virtual void GetGraphvizOutputPortToken(const OutputPort< T > &port, std::stringstream *dot) const
Appends a fragment to the dot stream identifying the graphviz node representing port.
Definition: system.h:1236
virtual void GetGraphvizInputPortToken(const InputPort< T > &port, std::stringstream *dot) const
Appends a fragment to the dot stream identifying the graphviz node representing port.
Definition: system.h:1229
STL namespace.
Context is an abstract class template that represents all the typed values that are used in a System&#39;...
Definition: context.h:40
~Diagram() override
Definition: diagram.h:57
void DoGetWitnessFunctions(const Context< T > &context, std::vector< const WitnessFunction< T > * > *witnesses) const final
Provides witness functions of subsystems that are active at the beginning of a continuous time interv...
Definition: diagram.h:576
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:46
This class represents an unrestricted update event.
Definition: event.h:482
DiagramContinuousState is a ContinuousState consisting of Supervectors xc, q, v, z over the correspon...
Definition: diagram_continuous_state.h:32
std::unique_ptr< ContinuousState< T > > AllocateTimeDerivatives() const override
Aggregates the time derivatives from each subsystem into a DiagramContinuousState.
Definition: diagram.h:249
const InputPort< T > & get_input_port(int port_index) const
Returns the typed input port at index port_index.
Definition: system.h:1084
Abstract base class that represents an event.
Definition: event.h:166
VectorBase is an abstract base class that real-valued signals between Systems and real-valued System ...
Definition: vector_base.h:27
std::unique_ptr< DiscreteValues< T > > AllocateDiscreteVariables() const override
Aggregates the discrete update variables from each subsystem into a DiagramDiscreteVariables.
Definition: diagram.h:260
#define SPDLOG_TRACE(logger,...)
Definition: text_logging.h:135
Context< T > & GetMutableSubsystemContext(const System< T > &subsystem, Context< T > *context) const
Returns the subcontext that corresponds to the system subsystem.
Definition: diagram.h:319
std::unique_ptr< Context< T > > AllocateContext() const
Returns a Context<T> suitable for use with this System<T>.
Definition: system.h:95
void AddOutputPort(std::unique_ptr< OutputPortBase > port)
(Internal use only) Adds an already-constructed output port to this System.
Definition: system_base.h:770
#define DRAKE_THROW_UNLESS(condition)
Evaluates condition and iff the value is false will throw an exception with a message showing at leas...
Definition: drake_throw.h:23
std::vector< double > times
Definition: pick_and_place_state_machine.cc:37
logging::logger * log()
Retrieve an instance of a logger to use for logging; for example: drake::log()->info("potato!") ...
Definition: text_logging.cc:42
A concrete class that holds all simultaneous homogeneous events for a Diagram.
Definition: event_collection.h:158
std::string to_string(const drake::geometry::Identifier< Tag > &id)
Enables use of identifiers with to_string.
Definition: identifier.h:215
std::vector< double > vector
Definition: translator_test.cc:20
CompositeEventCollection for a Diagram.
Definition: event_collection.h:627
DiagramBuilder is a factory class for Diagram.
Definition: diagram.h:33
std::string id
Definition: loader.cc:37
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:71
CompositeEventCollection< T > & GetMutableSubsystemCompositeEventCollection(const System< T > &subsystem, CompositeEventCollection< T > *events) const
Returns the mutable subsystem composite event collection that corresponds to subsystem.
Definition: diagram.h:340
std::vector< Number > result
Definition: ipopt_solver.cc:151
std::pair< SubsystemIndex, InputPortIndex > InputPortIdentifier
Identifies a child subsystem&#39;s input port.
Definition: diagram_context.h:129
Provides non-templatized Context functionality shared by the templatized derived classes.
Definition: context_base.h:38
const State< T > & GetSubsystemState(const System< T > &subsystem, const State< T > &state) const
Retrieves the state for a particular subsystem from the state for the entire diagram.
Definition: diagram.h:374
TypeSafeIndex< class OutputPortTag > OutputPortIndex
Serves as the local index for the output ports of a given System.
Definition: framework_common.h:45
const System< T > * get_system() const
Returns a reference to the System that owns this input port.
Definition: input_port.h:47
virtual bool HasEvents() const =0
Returns false if and only if this collection contains no events.
std::vector< const systems::System< T > * > GetSystems() const
Returns the list of contained Systems.
Definition: diagram.h:60
const InputPort< T > & DeclareInputPort(PortDataType type, int size, const std::string &name="", optional< RandomDistribution > random_type=nullopt)
Adds a port with the specified type and size to the input topology.
Definition: system.h:1642
#define DRAKE_ASSERT(condition)
DRAKE_ASSERT(condition) is similar to the built-in assert(condition) from the C++ system header <cas...
Definition: drake_assert.h:37
DiagramState is a State, annotated with pointers to all the mutable substates that it spans...
Definition: diagram_context.h:27
void SetRandomParameters(const Context< T > &context, Parameters< T > *params, RandomGenerator *generator) const override
Assigns random values to all parameters.
Definition: diagram.h:177
A token describing an event that recurs on a fixed period.
Definition: event.h:52
void DoCalcTimeDerivatives(const Context< T > &context, ContinuousState< T > *derivatives) const override
Override this if you have any continuous state variables xc in your concrete System to calculate thei...
Definition: diagram.h:269
const SystemBase & get_system_base() const
Returns a reference to the SystemBase that owns this input port.
Definition: input_port_base.h:42
OutputPortIndex get_index() const
Returns the index of this output port within the owning System.
Definition: output_port_base.h:25
Provides Drake&#39;s assertion implementation.
void RemoveUnlessAlsoSupportedBy(const SystemScalarConverter &other)
Removes from this converter all pairs where other.IsConvertible<T, U> is false.
Definition: system_scalar_converter.cc:45
This is the entry point for all text logging within Drake.
State< T > & GetMutableSubsystemState(const System< T > &subsystem, Context< T > *context) const
Retrieves the state for a particular subsystem from the context for the entire diagram.
Definition: diagram.h:355
void set_forced_unrestricted_update_events(std::unique_ptr< EventCollection< UnrestrictedUpdateEvent< T >>> forced)
Definition: system.h:2042
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
InputPortIndex get_index() const
Returns the index of this input port within the owning System.
Definition: input_port_base.h:31
The DiagramContext is a container for all of the data necessary to uniquely determine the computation...
Definition: diagram_context.h:118
void SetRandomState(const Context< T > &context, State< T > *state, RandomGenerator *generator) const override
Assigns random values to all elements of the state.
Definition: diagram.h:161
void GetGraphvizInputPortToken(const InputPort< T > &port, std::stringstream *dot) const override
Appends a fragment to the dot stream identifying the graphviz node representing port.
Definition: diagram.h:469
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
void SetDefaultParameters(const Context< T > &context, Parameters< T > *params) const override
Assigns default values to all parameters.
Definition: diagram.h:112
const VectorBase< T > & get_generalized_position() const
Returns a const reference to the subset of the state vector that is generalized position q...
Definition: continuous_state.h:173
Diagram is a System composed of one or more constituent Systems, arranged in a directed graph where t...
Definition: diagram.h:44
Base class for all System functionality that is dependent on the templatized scalar type T for input...
Definition: input_port.h:15
const Context< T > * DoGetTargetSystemContext(const System< T > &target_system, const Context< T > *context) const final
Returns a pointer to const context if target_system is a subsystem of this, nullptr is returned other...
Definition: diagram.h:613
bool HasDirectFeedthrough(int output_port) const
Returns true if there might be direct-feedthrough from any input port to the given output_port...
Definition: system.h:306
ReferenceType type
Definition: loader.cc:34
AbstractValue * DoAllocateInputAbstract(const InputPort< T > &input_port) const override
Allocates an abstract input of the leaf type that the System requires on the port specified by input_...
Definition: diagram.h:836
void set_forced_publish_events(std::unique_ptr< EventCollection< PublishEvent< T >>> forced)
Definition: system.h:2032
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
State< T > * DoGetMutableTargetSystemState(const System< T > &target_system, State< T > *state) const final
Returns a pointer to mutable state if target_system is a subsystem of this, nullptr is returned other...
Definition: diagram.h:626
void GetGraphvizFragment(std::stringstream *dot) const override
Returns a Graphviz fragment describing this Diagram.
Definition: diagram.h:387
void AddTriggeredWitnessFunctionToCompositeEventCollection(Event< T > *event, CompositeEventCollection< T > *events) const final
For the subsystem associated with witness_func, gets its mutable sub composite event collection from ...
Definition: diagram.h:539
const std::string & get_name() const
Returns the name last supplied to set_name(), if any.
Definition: system_base.h:50
const State< T > * DoGetTargetSystemState(const System< T > &target_system, const State< T > *state) const final
Returns a pointer to const state if target_system is a subsystem of this, nullptr is returned otherwi...
Definition: diagram.h:654
A fully type-erased container class.
Definition: value.h:101
This class bundles an instance of each EventCollection<EventType> into one object that stores the het...
Definition: event.h:27
const double time
Definition: robot_plan_interpolator_test.cc:64
const AbstractValue * EvalAbstractInput(const ContextBase &context, int port_index) const
Returns the value of the input port with the given port_index as an AbstractValue, which is permitted for ports of any type.
Definition: system_base.h:127
int64_t GetGraphvizId() const
Returns an opaque integer that uniquely identifies this system in the Graphviz output.
Definition: system.h:1243
Structure for comparing two PeriodicEventData objects for use in a map container, using an arbitrary ...
Definition: event.h:317
std::unique_ptr< CompositeEventCollection< T > > AllocateCompositeEventCollection() const final
Allocates a DiagramEventCollection for this Diagram.
Definition: diagram.h:84
static void set_parent_service(SystemBase *child, const internal::SystemParentServiceInterface *parent_service)
(Internal use only) Declares that parent_service is the service interface of the Diagram that owns th...
Definition: system_base.h:851
const CompositeEventCollection< T > * DoGetTargetSystemCompositeEventCollection(const System< T > &target_system, const CompositeEventCollection< T > *events) const final
Returns a pointer to const composite event collection if target_system is a subsystem of this...
Definition: diagram.h:682
void set_abstract_parameters(std::unique_ptr< AbstractValues > abstract_params)
Definition: parameters.h:126
void DoMapVelocityToQDot(const Context< T > &context, const Eigen::Ref< const VectorX< T >> &generalized_velocity, VectorBase< T > *qdot) const override
The generalized_velocity vector must have the same size and ordering as the generalized velocity in t...
Definition: diagram.h:698
Parameters is a container for variables that parameterize a System so that it can represent a family ...
Definition: parameters.h:26
void error(const char *, const Args &...)
Definition: text_logging.h:121
AbstractValue & get_mutable_abstract_parameter(int index)
Returns the abstract-valued parameter at index.
Definition: parameters.h:104
BasicVector< T > * DoAllocateInputVector(const InputPort< T > &input_port) const override
Allocates an input vector of the leaf type that the System requires on the port specified by input_po...
Definition: diagram.h:826
int get_num_continuous_states() const final
Returns the dimension of the continuous state vector that has been declared until now...
Definition: diagram.h:491
std::pair< const System< double > *, InputPortIndex > InputPortLocator
Definition: diagram.h:49
CompositeEventCollection< T > * DoGetMutableTargetSystemCompositeEventCollection(const System< T > &target_system, CompositeEventCollection< T > *events) const final
Returns a pointer to mutable composite event collection if target_system is a subsystem of this...
Definition: diagram.h:667
Subvector is a concrete class template that implements VectorBase by providing a sliced view of a Vec...
Definition: subvector.h:20
std::vector< std::string > names
Definition: translator_test.cc:19
int get_num_input_ports() const
Returns the number of input ports currently allocated in this System.
Definition: system_base.h:172
const Context< T > & GetSubsystemContext(const System< T > &subsystem, const Context< T > &context) const
Returns a constant reference to the subcontext that corresponds to the system subsystem.
Definition: diagram.h:308
std::unique_ptr< AbstractValue > AllocateInputAbstract(const InputPort< T > &input_port) const
Given an input port, allocates the abstract storage.
Definition: system.h:125
const VectorBase< T > & get_generalized_velocity() const
Returns a const reference to the subset of the continuous state vector that is generalized velocity v...
Definition: continuous_state.h:185
std::string GetSystemPathname() const final
Generates and returns a human-readable full path name of this subsystem, for use in messages and logg...
Definition: system_base.cc:21
This class represents a publish event.
Definition: event.h:332
std::unique_ptr< BasicVector< T > > AllocateInputVector(const InputPort< T > &input_port) const
Given an input port, allocates the vector storage.
Definition: system.h:112
std::pair< SubsystemIndex, OutputPortIndex > OutputPortIdentifier
Identifies a child subsystem&#39;s output port.
Definition: diagram_context.h:131
The argument to Convert need not be the exact type S that was used to populate the SystemScalarConver...
DependencyTicket assign_next_dependency_ticket()
(Internal use only) Assigns the next unused dependency ticket number, unique only within a particular...
Definition: system_base.h:841
SubsystemIndex GetSystemIndexOrAbort(const System< T > *sys) const
Returns the index of the given sys in this diagram, or aborts if sys is not a member of the diagram...
Definition: diagram.h:485
Provides public header files of Drake&#39;s symbolic library.
State< T > & get_mutable_state()
Returns a mutable reference to the whole State, potentially invalidating all state-dependent computat...
Definition: context.h:329
boolean< T > T
Definition: drake_bool_deprecated.h:26
A SystemConstraint is a generic base-class for constraints on Systems.
Definition: system_constraint.h:56
ContinuousState is a view of, and optionally a container for, all the continuous state variables xc o...
Definition: continuous_state.h:76
Diagram(SystemScalarConverter converter)
(Advanced) Constructs an uninitialized Diagram.
Definition: diagram.h:519
Helper class to convert a System<U> into a System<T>, intended for internal use by the System framewo...
Definition: system_scalar_converter.h:35
const DiscreteValues< T > & get_discrete_state() const
Returns a reference to the entire discrete state, which may consist of multiple discrete state vector...
Definition: context.h:130
const OutputPort< T > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:1091
static const std::string & Get()
Attempts to return a nicely demangled and canonicalized type name that is the same on all platforms...
Definition: nice_type_name.h:53
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
DiagramDiscreteValues is a DiscreteValues container comprised recursively of a sequence of child Disc...
Definition: diagram_discrete_values.h:27
SystemConstraintIndex AddConstraint(std::unique_ptr< SystemConstraint< T >> constraint)
Adds an already-created constraint to the list of constraints for this System.
Definition: system.h:1675
A tag object that denotes a System subclass S in function signatures.
Definition: system_type_tag.h:29
const System< T > & get_system() const
Returns a reference to the System that owns this output port.
Definition: output_port.h:137
void set_forced_discrete_update_events(std::unique_ptr< EventCollection< DiscreteUpdateEvent< T >>> forced)
Definition: system.h:2037
const ContinuousState< T > & get_continuous_state() const
Returns a const reference to the continuous component of the state, which may be of size zero...
Definition: context.h:113
Class that stores a function that is able to help determine the time and state at which a step of the...
Definition: event.h:16
This extends InputPortBase with some scalar type-dependent methods.
Definition: input_port.h:22
std::string name
Definition: geometry_base_test.cc:343
int size() const
Definition: system_constraint.h:119
const CompositeEventCollection< T > & GetSubsystemCompositeEventCollection(const System< T > &subsystem, const CompositeEventCollection< T > &events) const
Returns the const subsystem composite event collection from events that corresponds to subsystem...
Definition: diagram.h:330
std::function< void(const Context< T > &context, VectorX< T > *value)> CalcCallback
This is the signature of a stateless function that evaluates the value of the constraint function f: ...
Definition: system_constraint.h:67
There are three concrete event types for any System: publish, discrete state update, and unrestricted state update, listed in order of increasing ability to change the state (i.e., zero to all).
Definition: event.h:21
State< T > & GetMutableSubsystemState(const System< T > &subsystem, State< T > *state) const
Retrieves the state for a particular subsystem from the state for the entire diagram.
Definition: diagram.h:364
This class represents a discrete update event.
Definition: event.h:404
Class for storing data from a witness function triggering to be passed to event handlers.
Definition: event.h:90
Provides careful macros to selectively enable or disable the special member functions for copy-constr...
An InputPort is a System resource that describes the kind of input a System accepts, on a given port.
Definition: input_port_base.h:22
int data
Definition: value_test.cc:20