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 
14 #include "drake/common/drake_assert.h"
15 #include "drake/common/drake_copyable.h"
16 #include "drake/common/number_traits.h"
17 #include "drake/common/symbolic.h"
18 #include "drake/common/text_logging.h"
19 #include "drake/systems/framework/diagram_context.h"
20 #include "drake/systems/framework/diagram_continuous_state.h"
21 #include "drake/systems/framework/diagram_discrete_values.h"
22 #include "drake/systems/framework/discrete_values.h"
23 #include "drake/systems/framework/event.h"
24 #include "drake/systems/framework/state.h"
25 #include "drake/systems/framework/subvector.h"
26 #include "drake/systems/framework/system.h"
27 #include "drake/systems/framework/system_constraint.h"
28 
29 namespace drake {
30 namespace systems {
31 
32 template <typename T>
33 class Diagram;
34 template <typename T>
36 
37 namespace internal {
38 
39 //==============================================================================
40 // DIAGRAM OUTPUT PORT
41 //==============================================================================
42 /// Holds information about the subsystem output port that has been exported to
43 /// become one of this Diagram's output ports. The actual methods for
44 /// determining the port's value are supplied by the LeafSystem that ultimately
45 /// underlies the source port, although that may be any number of levels down.
46 template <typename T>
47 class DiagramOutputPort : public OutputPort<T> {
48  public:
50 
51  /// Construct a %DiagramOutputPort for the given `diagram` that exports the
52  /// indicated port. That port's owning system must be a subsystem of the
53  /// diagram.
54  DiagramOutputPort(const Diagram<T>& diagram,
55  const OutputPort<T>* source_output_port)
56  : OutputPort<T>(
57  diagram, diagram, OutputPortIndex(diagram.get_num_output_ports()),
58  source_output_port->get_data_type(), source_output_port->size()),
59  source_output_port_(source_output_port),
60  subsystem_index_(
61  diagram.GetSystemIndexOrAbort(&source_output_port->get_system())) {}
62 
63  ~DiagramOutputPort() final = default;
64 
65  /// Obtain a reference to the subsystem output port that was exported to
66  /// create this diagram port. Note that the source may itself be a diagram
67  /// output port.
69  return *source_output_port_;
70  }
71 
72  private:
73  // These forward to the source system output port.
74  std::unique_ptr<AbstractValue> DoAllocate() const final {
75  return source_output_port_->Allocate();
76  }
77 
78  // Pass in just the source System's Context, not the whole Diagram context
79  // we're given.
80  void DoCalc(
81  const Context<T>& context, AbstractValue* value) const final {
82  const Context<T>& subcontext = get_subcontext(context);
83  return source_output_port_->Calc(subcontext, value);
84  }
85 
86  const AbstractValue& DoEval(const Context<T>& context) const final {
87  const Context<T>& subcontext = get_subcontext(context);
88  return source_output_port_->Eval(subcontext);
89  }
90 
91  // Dig out the right subcontext for delegation.
92  const Context<T>& get_subcontext(const Context<T>& context) const {
93  const DiagramContext<T>* diagram_context =
94  dynamic_cast<const DiagramContext<T>*>(&context);
95  DRAKE_DEMAND(diagram_context != nullptr);
96  return diagram_context->GetSubsystemContext(subsystem_index_);
97  }
98 
99  const OutputPort<T>* const source_output_port_;
100  const SubsystemIndex subsystem_index_;
101 };
102 
103 //==============================================================================
104 // DIAGRAM OUTPUT
105 //==============================================================================
106 /// DiagramOutput is an implementation of SystemOutput that holds unowned
107 /// OutputPortValue pointers. It is used to expose the outputs of constituent
108 /// systems as outputs of a Diagram.
109 ///
110 /// @tparam T The type of the output data. Must be a valid Eigen scalar.
111 template <typename T>
112 class DiagramOutput : public SystemOutput<T> {
113  public:
115 
116  DiagramOutput() = default;
117 
118  int get_num_ports() const override { return static_cast<int>(ports_.size()); }
119 
121  DRAKE_DEMAND(index >= 0 && index < get_num_ports());
122  return ports_[index];
123  }
124 
125  const OutputPortValue& get_port_value(int index) const override {
126  DRAKE_DEMAND(index >= 0 && index < get_num_ports());
127  return *ports_[index];
128  }
129 
130  std::vector<OutputPortValue*>* get_mutable_port_values() { return &ports_; }
131 
132  protected:
133  // Returns a clone that has the same number of output ports, with values
134  // set to nullptr.
135  DiagramOutput<T>* DoClone() const override {
136  DiagramOutput<T>* clone = new DiagramOutput<T>();
137  clone->ports_.resize(get_num_ports());
138  return clone;
139  }
140 
141  private:
142  std::vector<OutputPortValue*> ports_;
143 };
144 
145 } // namespace internal
146 
147 //==============================================================================
148 // DIAGRAM
149 //==============================================================================
150 /// Diagram is a System composed of one or more constituent Systems, arranged
151 /// in a directed graph where the vertices are the constituent Systems
152 /// themselves, and the edges connect the output of one constituent System
153 /// to the input of another. To construct a Diagram, use a DiagramBuilder.
154 ///
155 /// Each System in the Diagram must have a unique, non-empty name.
156 ///
157 /// @tparam T The mathematical scalar type. Must be a valid Eigen scalar.
158 template <typename T>
159 class Diagram : public System<T>, internal::SystemParentServiceInterface {
160  public:
161  // Diagram objects are neither copyable nor moveable.
163 
164  using InputPortLocator = std::pair<const System<T>*, InputPortIndex>;
165  using OutputPortLocator = std::pair<const System<T>*, OutputPortIndex>;
166 
167  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
168  template <typename U>
169  explicit Diagram(const Diagram<U>& other)
170  : Diagram(other.template ConvertScalarType<T>()) {}
171 
172  ~Diagram() override {}
173 
174  /// Returns the list of contained Systems.
175  std::vector<const systems::System<T>*> GetSystems() const {
176  std::vector<const systems::System<T>*> result;
177  result.reserve(registered_systems_.size());
178  for (const auto& system : registered_systems_) {
179  result.push_back(system.get());
180  }
181  return result;
182  }
183 
184  std::multimap<int, int> GetDirectFeedthroughs() const final {
185  std::multimap<int, int> pairs;
186  for (InputPortIndex u(0); u < this->get_num_input_ports(); ++u) {
187  for (OutputPortIndex v(0); v < this->get_num_output_ports(); ++v) {
188  if (DoHasDirectFeedthrough(u, v)) {
189  pairs.emplace(u, v);
190  }
191  }
192  }
193  return pairs;
194  };
195 
196  /// Allocates a DiagramEventCollection for this Diagram.
197  /// @sa System::AllocateCompositeEventCollection().
198  std::unique_ptr<CompositeEventCollection<T>>
200  const int num_systems = num_subsystems();
201  std::vector<std::unique_ptr<CompositeEventCollection<T>>> subevents(
202  num_systems);
203  for (SubsystemIndex i(0); i < num_systems; ++i) {
204  subevents[i] = registered_systems_[i]->AllocateCompositeEventCollection();
205  }
206 
207  return std::make_unique<DiagramCompositeEventCollection<T>>(
208  std::move(subevents));
209  }
210 
211  void SetDefaultState(const Context<T>& context,
212  State<T>* state) const override {
213  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
214  DRAKE_DEMAND(diagram_context != nullptr);
215 
216  auto diagram_state = dynamic_cast<DiagramState<T>*>(state);
217  DRAKE_DEMAND(diagram_state != nullptr);
218 
219  // Set default state of each constituent system.
220  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
221  auto& subcontext = diagram_context->GetSubsystemContext(i);
222  auto& substate = diagram_state->get_mutable_substate(i);
223  registered_systems_[i]->SetDefaultState(subcontext, &substate);
224  }
225  }
226 
227  void SetDefaultParameters(const Context<T>& context,
228  Parameters<T>* params) const override {
229  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
230  DRAKE_DEMAND(diagram_context != nullptr);
231 
232  int numeric_parameter_offset = 0;
233  int abstract_parameter_offset = 0;
234 
235  // Set default parameters of each constituent system.
236  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
237  auto& subcontext = diagram_context->GetSubsystemContext(i);
238 
239  if (!subcontext.num_numeric_parameters() &&
240  !subcontext.num_abstract_parameters()) {
241  // Then there is no work to do for this subcontext.
242  continue;
243  }
244 
245  // Make a new Parameters<T> structure with pointers to the mutable
246  // subsystem parameter values. This does not make a copy of the
247  // underlying data.
248  // TODO(russt): Consider implementing a DiagramParameters, analogous to
249  // DiagramState, to avoid these dynamic allocations if they prove
250  // expensive.
251 
252  std::vector<BasicVector<T>*> numeric_params;
253  for (int j = 0; j < subcontext.num_numeric_parameters(); ++j) {
254  numeric_params.push_back(&params->get_mutable_numeric_parameter(
255  numeric_parameter_offset + j));
256  }
257  numeric_parameter_offset += subcontext.num_numeric_parameters();
258 
259  std::vector<AbstractValue*> abstract_params;
260  for (int j = 0; j < subcontext.num_abstract_parameters(); ++j) {
261  abstract_params.push_back(&params->get_mutable_abstract_parameter(
262  abstract_parameter_offset + j));
263  }
264  abstract_parameter_offset += subcontext.num_abstract_parameters();
265 
266  Parameters<T> subparameters;
267  subparameters.set_numeric_parameters(
268  std::make_unique<DiscreteValues<T>>(numeric_params));
269  subparameters.set_abstract_parameters(
270  std::make_unique<AbstractValues>(abstract_params));
271 
272  registered_systems_[i]->SetDefaultParameters(subcontext, &subparameters);
273  }
274  }
275 
276  void SetRandomState(const Context<T>& context, State<T>* state,
277  RandomGenerator* generator) const override {
278  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
279  DRAKE_DEMAND(diagram_context != nullptr);
280 
281  auto diagram_state = dynamic_cast<DiagramState<T>*>(state);
282  DRAKE_DEMAND(diagram_state != nullptr);
283 
284  // Set state of each constituent system.
285  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
286  auto& subcontext = diagram_context->GetSubsystemContext(i);
287  auto& substate = diagram_state->get_mutable_substate(i);
288  registered_systems_[i]->SetRandomState(subcontext, &substate, generator);
289  }
290  }
291 
292  void SetRandomParameters(const Context<T>& context, Parameters<T>* params,
293  RandomGenerator* generator) const override {
294  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
295  DRAKE_DEMAND(diagram_context != nullptr);
296 
297  int numeric_parameter_offset = 0;
298  int abstract_parameter_offset = 0;
299 
300  // Set parameters of each constituent system.
301  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
302  auto& subcontext = diagram_context->GetSubsystemContext(i);
303 
304  if (!subcontext.num_numeric_parameters() &&
305  !subcontext.num_abstract_parameters()) {
306  // Then there is no work to do for this subcontext.
307  continue;
308  }
309 
310  // Make a new Parameters<T> structure with pointers to the mutable
311  // subsystem parameter values. This does not make a copy of the
312  // underlying data.
313  // TODO(russt): This code is duplicated from SetDefaultParameters.
314  // Consider extracting it to a helper method (waiting for the rule of
315  // three).
316 
317  std::vector<BasicVector<T>*> numeric_params;
318  std::vector<AbstractValue*> abstract_params;
319  for (int j = 0; j < subcontext.num_numeric_parameters(); ++j) {
320  numeric_params.push_back(&params->get_mutable_numeric_parameter(
321  numeric_parameter_offset + j));
322  }
323  numeric_parameter_offset += subcontext.num_numeric_parameters();
324  for (int j = 0; j < subcontext.num_abstract_parameters(); ++j) {
325  abstract_params.push_back(&params->get_mutable_abstract_parameter(
326  abstract_parameter_offset + j));
327  }
328  abstract_parameter_offset += subcontext.num_abstract_parameters();
329  Parameters<T> subparameters;
330  subparameters.set_numeric_parameters(
331  std::make_unique<DiscreteValues<T>>(numeric_params));
332  subparameters.set_abstract_parameters(
333  std::make_unique<AbstractValues>(abstract_params));
334 
335  registered_systems_[i]->SetRandomParameters(subcontext, &subparameters,
336  generator);
337  }
338  }
339 
340  std::unique_ptr<SystemOutput<T>> AllocateOutput(
341  const Context<T>& context) const override {
342  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
343  DRAKE_DEMAND(diagram_context != nullptr);
344 
345  // The output ports of this Diagram are output ports of its constituent
346  // systems. Create a DiagramOutput with that many ports.
347  auto output = std::make_unique<internal::DiagramOutput<T>>();
348  output->get_mutable_port_values()->resize(output_port_ids_.size());
349  ExposeSubsystemOutputs(*diagram_context, output.get());
350  return std::move(output);
351  }
352 
353  /// @cond
354  // The three methods below are hidden from doxygen, as described in
355  // documentation for their corresponding methods in System.
356  std::unique_ptr<EventCollection<PublishEvent<T>>>
357  AllocateForcedPublishEventCollection() const final {
358  return AllocateForcedEventCollection<PublishEvent<T>>(
360  }
361 
362  std::unique_ptr<EventCollection<DiscreteUpdateEvent<T>>>
363  AllocateForcedDiscreteUpdateEventCollection() const final {
364  return AllocateForcedEventCollection<DiscreteUpdateEvent<T>>(
366  }
367 
368  std::unique_ptr<EventCollection<UnrestrictedUpdateEvent<T>>>
369  AllocateForcedUnrestrictedUpdateEventCollection() const final {
370  return AllocateForcedEventCollection<UnrestrictedUpdateEvent<T>>(
372  }
373  /// @endcond
374 
375  /// Aggregates the time derivatives from each subsystem into a
376  /// DiagramContinuousState.
377  std::unique_ptr<ContinuousState<T>> AllocateTimeDerivatives() const override {
378  std::vector<std::unique_ptr<ContinuousState<T>>> sub_derivatives;
379  for (const auto& system : registered_systems_) {
380  sub_derivatives.push_back(system->AllocateTimeDerivatives());
381  }
382  return std::unique_ptr<ContinuousState<T>>(
383  new DiagramContinuousState<T>(std::move(sub_derivatives)));
384  }
385 
386  /// Aggregates the discrete update variables from each subsystem into a
387  /// DiagramDiscreteVariables.
388  std::unique_ptr<DiscreteValues<T>> AllocateDiscreteVariables()
389  const override {
390  std::vector<std::unique_ptr<DiscreteValues<T>>> sub_discretes;
391  for (const auto& system : registered_systems_) {
392  sub_discretes.push_back(system->AllocateDiscreteVariables());
393  }
394  return std::unique_ptr<DiscreteValues<T>>(
395  new DiagramDiscreteValues<T>(std::move(sub_discretes)));
396  }
397 
398  void DoCalcTimeDerivatives(const Context<T>& context,
399  ContinuousState<T>* derivatives) const override {
400  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
401  DRAKE_DEMAND(diagram_context != nullptr);
402 
403  auto diagram_derivatives =
404  dynamic_cast<DiagramContinuousState<T>*>(derivatives);
405  DRAKE_DEMAND(diagram_derivatives != nullptr);
406  const int n = diagram_derivatives->num_substates();
407  DRAKE_DEMAND(num_subsystems() == n);
408 
409  // Evaluate the derivatives of each constituent system.
410  for (SubsystemIndex i(0); i < n; ++i) {
411  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
412  ContinuousState<T>& subderivatives =
413  diagram_derivatives->get_mutable_substate(i);
414  registered_systems_[i]->CalcTimeDerivatives(subcontext, &subderivatives);
415  }
416  }
417 
418  /// Retrieves the state derivatives for a particular subsystem from the
419  /// derivatives for the entire diagram. Aborts if @p subsystem is not
420  /// actually a subsystem of this diagram. Returns a 0-length ContinuousState
421  /// if @p subsystem has none.
423  const ContinuousState<T>& derivatives, const System<T>* subsystem) const {
424  DRAKE_DEMAND(subsystem != nullptr);
425  auto diagram_derivatives =
426  dynamic_cast<const DiagramContinuousState<T>*>(&derivatives);
427  DRAKE_DEMAND(diagram_derivatives != nullptr);
428  const SubsystemIndex i = GetSystemIndexOrAbort(subsystem);
429  return diagram_derivatives->get_substate(i);
430  }
431 
432  /// Returns a constant reference to the subcontext that corresponds to the
433  /// system @p subsystem.
434  /// Classes inheriting from %Diagram need access to this method in order to
435  /// pass their constituent subsystems the apropriate subcontext. Aborts if
436  /// @p subsystem is not actually a subsystem of this diagram.
437  const Context<T>& GetSubsystemContext(const System<T>& subsystem,
438  const Context<T>& context) const {
439  auto ret = DoGetTargetSystemContext(subsystem, &context);
440  DRAKE_DEMAND(ret != nullptr);
441  return *ret;
442  }
443 
444  /// Returns the subcontext that corresponds to the system @p subsystem.
445  /// Classes inheriting from %Diagram need access to this method in order to
446  /// pass their constituent subsystems the apropriate subcontext. Aborts if
447  /// @p subsystem is not actually a subsystem of this diagram.
449  Context<T>* context) const {
450  auto ret = DoGetMutableTargetSystemContext(subsystem, context);
451  DRAKE_DEMAND(ret != nullptr);
452  return *ret;
453  }
454 
455  /// Returns the const subsystem composite event collection from @p events
456  /// that corresponds to @p subsystem. Aborts if @p subsystem is not a
457  /// subsystem of this diagram.
460  const CompositeEventCollection<T>& events) const {
461  auto ret = DoGetTargetSystemCompositeEventCollection(subsystem, &events);
462  DRAKE_DEMAND(ret != nullptr);
463  return *ret;
464  }
465 
466  /// Returns the mutable subsystem composite event collection that corresponds
467  /// to @p subsystem. Aborts if @p subsystem is not a subsystem of this
468  /// diagram.
470  const System<T>& subsystem, CompositeEventCollection<T>* events) const {
471  auto ret = DoGetMutableTargetSystemCompositeEventCollection(
472  subsystem, events);
473  DRAKE_DEMAND(ret != nullptr);
474  return *ret;
475  }
476 
477  /// Retrieves the state for a particular subsystem from the context for the
478  /// entire diagram. Invalidates all entries in that subsystem's cache that
479  /// depend on State. Aborts if @p subsystem is not actually a subsystem of
480  /// this diagram.
481  ///
482  /// TODO(david-german-tri): Provide finer-grained accessors for finer-grained
483  /// invalidation.
485  Context<T>* context) const {
486  Context<T>& subcontext = GetMutableSubsystemContext(subsystem, context);
487  return subcontext.get_mutable_state();
488  }
489 
490  /// Retrieves the state for a particular subsystem from the @p state for the
491  /// entire diagram. Aborts if @p subsystem is not actually a subsystem of this
492  /// diagram.
494  State<T>* state) const {
495  auto ret = DoGetMutableTargetSystemState(subsystem, state);
496  DRAKE_DEMAND(ret != nullptr);
497  return *ret;
498  }
499 
500  /// Retrieves the state for a particular subsystem from the @p state for the
501  /// entire diagram. Aborts if @p subsystem is not actually a subsystem of this
502  /// diagram.
503  const State<T>& GetSubsystemState(const System<T>& subsystem,
504  const State<T>& state) const {
505  auto ret = DoGetTargetSystemState(subsystem, &state);
506  DRAKE_DEMAND(ret != nullptr);
507  return *ret;
508  }
509 
510  //----------------------------------------------------------------------------
511  /// @name Graphviz methods
512  //@{
513 
514  /// Returns a Graphviz fragment describing this Diagram. To obtain a complete
515  /// Graphviz graph, call System<T>::GetGraphvizString.
516  void GetGraphvizFragment(std::stringstream* dot) const override {
517  // Open the Diagram.
518  const int64_t id = this->GetGraphvizId();
519  *dot << "subgraph cluster" << id << "diagram" " {" << std::endl;
520  *dot << "color=black" << std::endl;
521  *dot << "concentrate=true" << std::endl;
522  std::string name = this->get_name();
523  if (name.empty()) name = std::to_string(id);
524  *dot << "label=\"" << name << "\";" << std::endl;
525 
526  // Add a cluster for the input port nodes.
527  *dot << "subgraph cluster" << id << "inputports" << " {" << std::endl;
528  *dot << "rank=same" << std::endl;
529  *dot << "color=lightgrey" << std::endl;
530  *dot << "style=filled" << std::endl;
531  *dot << "label=\"input ports\"" << std::endl;
532  for (int i = 0; i < this->get_num_input_ports(); ++i) {
533  this->GetGraphvizInputPortToken(this->get_input_port(i), dot);
534  *dot << "[color=blue, label=\"u" << i << "\"];" << std::endl;
535  }
536  *dot << "}" << std::endl;
537 
538  // Add a cluster for the output port nodes.
539  *dot << "subgraph cluster" << id << "outputports" << " {" << std::endl;
540  *dot << "rank=same" << std::endl;
541  *dot << "color=lightgrey" << std::endl;
542  *dot << "style=filled" << std::endl;
543  *dot << "label=\"output ports\"" << std::endl;
544  for (int i = 0; i < this->get_num_output_ports(); ++i) {
545  this->GetGraphvizOutputPortToken(this->get_output_port(i), dot);
546  *dot << "[color=green, label=\"y" << i << "\"];" << std::endl;
547  }
548  *dot << "}" << std::endl;
549 
550  // Add a cluster for the subsystems.
551  *dot << "subgraph cluster" << id << "subsystems" << " {" << std::endl;
552  *dot << "color=white" << std::endl;
553  *dot << "label=\"\"" << std::endl;
554  // -- Add the subsystems themselves.
555  for (const auto& subsystem : registered_systems_) {
556  subsystem->GetGraphvizFragment(dot);
557  }
558  // -- Add the connections as edges.
559  for (const auto& edge : connection_map_) {
560  const OutputPortLocator& src = edge.second;
561  const System<T>* src_sys = src.first;
562  const InputPortLocator& dest = edge.first;
563  const System<T>* dest_sys = dest.first;
564  src_sys->GetGraphvizOutputPortToken(src_sys->get_output_port(src.second),
565  dot);
566  *dot << " -> ";
567  dest_sys->GetGraphvizInputPortToken(dest_sys->get_input_port(dest.second),
568  dot);
569  *dot << ";" << std::endl;
570  }
571 
572  // -- Add edges from the input and output port nodes to the subsystems that
573  // actually service that port. These edges are higlighted in blue
574  // (input) and green (output), matching the port nodes.
575  for (int i = 0; i < this->get_num_input_ports(); ++i) {
576  const auto& port_id = input_port_ids_[i];
577  this->GetGraphvizInputPortToken(this->get_input_port(i), dot);
578  *dot << " -> ";
579  port_id.first->GetGraphvizInputPortToken(
580  port_id.first->get_input_port(port_id.second), dot);
581  *dot << " [color=blue];" << std::endl;
582  }
583 
584  for (int i = 0; i < this->get_num_output_ports(); ++i) {
585  const auto& port_id = output_port_ids_[i];
586  port_id.first->GetGraphvizOutputPortToken(
587  port_id.first->get_output_port(port_id.second), dot);
588  *dot << " -> ";
589  this->GetGraphvizOutputPortToken(this->get_output_port(i), dot);
590  *dot << " [color=green];" << std::endl;
591  }
592  *dot << "}" << std::endl;
593 
594  // Close the diagram.
595  *dot << "}" << std::endl;
596  }
597 
599  std::stringstream* dot) const override {
600  DRAKE_DEMAND(port.get_system() == this);
601  *dot << "_" << this->GetGraphvizId() << "_u" << port.get_index();
602  }
603 
605  std::stringstream* dot) const override {
606  DRAKE_DEMAND(&port.get_system() == this);
607  *dot << "_" << this->GetGraphvizId() << "_y" << port.get_index();
608  }
609 
610  //@}
611 
612  /// Returns the index of the given @p sys in this diagram, or aborts if @p sys
613  /// is not a member of the diagram.
615  auto it = system_index_map_.find(sys);
616  DRAKE_DEMAND(it != system_index_map_.end());
617  return it->second;
618  }
619 
620  protected:
621  /// Constructs an uninitialized Diagram. Subclasses that use this constructor
622  /// are obligated to call DiagramBuilder::BuildInto(this). Provides scalar-
623  /// type conversion support only if every contained subsystem provides the
624  /// same support.
625  Diagram() : System<T>(
627  SystemTypeTag<systems::Diagram>{},
629 
630  /// (Advanced) Constructs an uninitialized Diagram. Subclasses that use this
631  /// constructor are obligated to call DiagramBuilder::BuildInto(this).
632  ///
633  /// Declares scalar-type conversion support using @p converter. Support for
634  /// a given pair of types `T, U` to convert from and to will be enabled only
635  /// if every contained subsystem supports that pair.
636  ///
637  /// See @ref system_scalar_conversion for detailed background and examples
638  /// related to scalar-type conversion support.
639  explicit Diagram(SystemScalarConverter converter)
640  : System<T>(std::move(converter)) {}
641 
642  /// For the subsystem associated with @p witness_func, gets its subcontext
643  /// from @p context, passes the subcontext to @p witness_func' Evaulate
644  /// method and returns the result. Aborts if the subsystem is not part of
645  /// this Diagram.
646  T DoCalcWitnessValue(const Context<T>& context,
647  const WitnessFunction<T>& witness_func) const final {
648  const System<T>& system = witness_func.get_system();
649  const Context<T>& subcontext = GetSubsystemContext(system, context);
650  return witness_func.CalcWitnessValue(subcontext);
651  }
652 
653  /// For the subsystem associated with `witness_func`, gets its mutable
654  /// sub composite event collection from `events`, and passes it to
655  /// `witness_func`'s AddEventToCollection method. This method also modifies
656  /// `event` by updating the pointers to "diagram" continuous state to point to
657  /// the ContinuousState pointers for the associated subsystem instead. Aborts
658  /// if the subsystem is not part of this Diagram.
660  Event<T>* event,
661  CompositeEventCollection<T>* events) const final {
662  DRAKE_DEMAND(events);
663  DRAKE_DEMAND(event);
664  DRAKE_DEMAND(event->get_event_data());
665 
666  // Get the event data- it will need to be modified.
667  auto data = dynamic_cast<WitnessTriggeredEventData<T>*>(
668  event->get_mutable_event_data());
669  DRAKE_DEMAND(data);
670 
671  // Get the vector of events corresponding to the subsystem.
672  const System<T>& subsystem = data->triggered_witness()->get_system();
673  CompositeEventCollection<T>& subevents =
674  GetMutableSubsystemCompositeEventCollection(subsystem, events);
675 
676  // Get the continuous states at both window endpoints.
677  auto diagram_xc0 = dynamic_cast<const DiagramContinuousState<T>*>(
678  data->xc0());
679  DRAKE_DEMAND(diagram_xc0 != nullptr);
680  auto diagram_xcf = dynamic_cast<const DiagramContinuousState<T>*>(
681  data->xcf());
682  DRAKE_DEMAND(diagram_xcf != nullptr);
683 
684  // Modify the pointer to the event data to point to the sub-system
685  // continuous states.
686  data->set_xc0(DoGetTargetSystemContinuousState(subsystem, diagram_xc0));
687  data->set_xcf(DoGetTargetSystemContinuousState(subsystem, diagram_xcf));
688 
689  // Add the event to the collection.
690  event->add_to_composite(&subevents);
691  }
692 
693  /// Provides witness functions of subsystems that are active at the beginning
694  /// of a continuous time interval. The vector of witness functions is not
695  /// ordered in a particular manner.
696  void DoGetWitnessFunctions(const Context<T>& context,
697  std::vector<const WitnessFunction<T>*>* witnesses) const final {
698  // A temporary vector is necessary since the vector of witnesses is
699  // declared to be empty on entry to DoGetWitnessFunctions().
700  std::vector<const WitnessFunction<T>*> temp_witnesses;
701 
702  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
703  DRAKE_DEMAND(diagram_context != nullptr);
704 
705  SubsystemIndex index(0);
706 
707  for (const auto& system : registered_systems_) {
708  DRAKE_ASSERT(index == GetSystemIndexOrAbort(system.get()));
709  temp_witnesses.clear();
710  system->GetWitnessFunctions(diagram_context->GetSubsystemContext(index),
711  &temp_witnesses);
712  witnesses->insert(witnesses->end(), temp_witnesses.begin(),
713  temp_witnesses.end());
714  ++index;
715  }
716  }
717 
718  /// Returns a pointer to mutable context if @p target_system is a sub system
719  /// of this, nullptr is returned otherwise.
721  const System<T>& target_system, Context<T>* context) const final {
722  if (&target_system == this)
723  return context;
724 
725  return GetSubsystemStuff<Context<T>, DiagramContext<T>>(
726  target_system, context,
729  }
730 
731  /// Returns a pointer to const context if @p target_system is a subsystem
732  /// of this, nullptr is returned otherwise.
734  const System<T>& target_system, const Context<T>* context) const final {
735  if (&target_system == this)
736  return context;
737 
738  return GetSubsystemStuff<const Context<T>, const DiagramContext<T>>(
739  target_system, context,
742  }
743 
744  /// Returns a pointer to mutable state if @p target_system is a subsystem
745  /// of this, nullptr is returned otherwise.
747  const System<T>& target_system, State<T>* state) const final {
748  if (&target_system == this)
749  return state;
750 
751  return GetSubsystemStuff<State<T>, DiagramState<T>>(
752  target_system, state,
755  }
756 
757  /// Returns a pointer to const state if @p target_system is a subsystem
758  /// of this, nullptr is returned otherwise.
760  const System<T>& target_system,
761  const ContinuousState<T>* xc) const final {
762  if (&target_system == this)
763  return xc;
764 
765  return GetSubsystemStuff<const ContinuousState<T>,
767  target_system, xc,
770  }
771 
772  /// Returns a pointer to const state if @p target_system is a subsystem
773  /// of this, nullptr is returned otherwise.
775  const System<T>& target_system, const State<T>* state) const final {
776  if (&target_system == this)
777  return state;
778 
779  return GetSubsystemStuff<const State<T>, const DiagramState<T>>(
780  target_system, state,
783  }
784 
785  /// Returns a pointer to mutable composite event collection if
786  /// @p target_system is a subsystem of this, nullptr is returned otherwise.
788  const System<T>& target_system,
789  CompositeEventCollection<T>* events) const final {
790  if (&target_system == this)
791  return events;
792 
793  return GetSubsystemStuff<CompositeEventCollection<T>,
795  target_system, events,
798  }
799 
800  /// Returns a pointer to const composite event collection if
801  /// @p target_system is a subsystem of this, nullptr is returned otherwise.
803  const System<T>& target_system,
804  const CompositeEventCollection<T>* events) const final {
805  if (&target_system == this)
806  return events;
807 
808  return GetSubsystemStuff<const CompositeEventCollection<T>,
810  target_system, events,
813  }
814 
815  /// The @p generalized_velocity vector must have the same size and ordering as
816  /// the generalized velocity in the ContinuousState that this Diagram reserves
817  /// in its context.
819  const Context<T>& context,
820  const Eigen::Ref<const VectorX<T>>& generalized_velocity,
821  VectorBase<T>* qdot) const override {
822  // Check that the dimensions of the continuous state in the context match
823  // the dimensions of the provided generalized velocity and configuration
824  // derivatives.
825  const ContinuousState<T>& xc = context.get_continuous_state();
826  const int nq = xc.get_generalized_position().size();
827  const int nv = xc.get_generalized_velocity().size();
828  DRAKE_DEMAND(nq == qdot->size());
829  DRAKE_DEMAND(nv == generalized_velocity.size());
830 
831  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
832  DRAKE_DEMAND(diagram_context != nullptr);
833 
834  // Iterate over the subsystems, asking each subsystem to map its subslice of
835  // velocity to configuration derivatives. This approach is valid because the
836  // DiagramContinuousState guarantees that the subsystem states are
837  // concatenated in order.
838  int v_index = 0; // The next index to read in generalized_velocity.
839  int q_index = 0; // The next index to write in qdot.
840  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
841  // Find the continuous state of subsystem i.
842  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
843  const ContinuousState<T>& sub_xc = subcontext.get_continuous_state();
844 
845  // Select the chunk of generalized_velocity belonging to subsystem i.
846  const int num_v = sub_xc.get_generalized_velocity().size();
847  const Eigen::Ref<const VectorX<T>>& v_slice =
848  generalized_velocity.segment(v_index, num_v);
849 
850  // Select the chunk of qdot belonging to subsystem i.
851  const int num_q = sub_xc.get_generalized_position().size();
852  Subvector<T> dq_slice(qdot, q_index, num_q);
853 
854  // Delegate the actual mapping to subsystem i itself.
855  registered_systems_[i]->MapVelocityToQDot(subcontext, v_slice, &dq_slice);
856 
857  // Advance the indices.
858  v_index += num_v;
859  q_index += num_q;
860  }
861  }
862 
863  /// The @p generalized_velocity vector must have the same size and ordering as
864  /// the generalized velocity in the ContinuousState that this Diagram reserves
865  /// in its context.
866  void DoMapQDotToVelocity(const Context<T>& context,
867  const Eigen::Ref<const VectorX<T>>& qdot,
868  VectorBase<T>* generalized_velocity) const override {
869  // Check that the dimensions of the continuous state in the context match
870  // the dimensions of the provided generalized velocity and configuration
871  // derivatives.
872  const ContinuousState<T>& xc = context.get_continuous_state();
873  const int nq = xc.get_generalized_position().size();
874  const int nv = xc.get_generalized_velocity().size();
875  DRAKE_DEMAND(nq == qdot.size());
876  DRAKE_DEMAND(nv == generalized_velocity->size());
877 
878  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
879  DRAKE_DEMAND(diagram_context != nullptr);
880 
881  // Iterate over the subsystems, asking each subsystem to map its subslice of
882  // configuration derivatives to velocity. This approach is valid because the
883  // DiagramContinuousState guarantees that the subsystem states are
884  // concatenated in order.
885  int q_index = 0; // The next index to read in qdot.
886  int v_index = 0; // The next index to write in generalized_velocity.
887  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
888  // Find the continuous state of subsystem i.
889  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
890  const ContinuousState<T>& sub_xc = subcontext.get_continuous_state();
891 
892  // Select the chunk of qdot belonging to subsystem i.
893  const int num_q = sub_xc.get_generalized_position().size();
894  const Eigen::Ref<const VectorX<T>>& dq_slice =
895  qdot.segment(q_index, num_q);
896 
897  // Select the chunk of generalized_velocity belonging to subsystem i.
898  const int num_v = sub_xc.get_generalized_velocity().size();
899  Subvector<T> v_slice(generalized_velocity, v_index, num_v);
900 
901  // Delegate the actual mapping to subsystem i itself.
902  registered_systems_[i]->MapQDotToVelocity(subcontext, dq_slice, &v_slice);
903 
904  // Advance the indices.
905  v_index += num_v;
906  q_index += num_q;
907  }
908  }
909 
910  /// Computes the next update time based on the configured actions, for scalar
911  /// types that are arithmetic, or aborts for scalar types that are not
912  /// arithmetic.
913  void DoCalcNextUpdateTime(const Context<T>& context,
914  CompositeEventCollection<T>* event_info,
915  T* time) const override {
916  DoCalcNextUpdateTimeImpl(context, event_info, time);
917  }
918 
920  const InputPortDescriptor<T>& descriptor) const override {
921  // Ask the subsystem to perform the allocation.
922  const InputPortLocator& id = input_port_ids_[descriptor.get_index()];
923  const System<T>* subsystem = id.first;
924  const InputPortIndex subindex = id.second;
925  return subsystem->AllocateInputVector(subsystem->get_input_port(subindex))
926  .release();
927  }
928 
930  const InputPortDescriptor<T>& descriptor) const override {
931  // Ask the subsystem to perform the allocation.
932  const InputPortLocator& id = input_port_ids_[descriptor.get_index()];
933  const System<T>* subsystem = id.first;
934  const InputPortIndex subindex = id.second;
935  return subsystem->AllocateInputAbstract(subsystem->get_input_port(subindex))
936  .release();
937  }
938 
939  private:
940  std::unique_ptr<ContextBase> DoMakeContext() const final {
941  const int num_systems = num_subsystems();
942  // Reserve inputs as specified during Diagram initialization.
943  auto context = std::make_unique<DiagramContext<T>>(num_systems);
944 
945  // Recursively construct each constituent system and its subsystems,
946  // then add to this diagram Context.
947  for (SubsystemIndex i(0); i < num_systems; ++i) {
948  const System<T>& sys = *registered_systems_[i];
949  std::unique_ptr<ContextBase> subcontext_base =
950  SystemBase::MakeContext(sys);
951  DRAKE_DEMAND(dynamic_cast<Context<T>*>(subcontext_base.get()) != nullptr);
952  std::unique_ptr<Context<T>> subcontext(
953  static_cast<Context<T>*>(subcontext_base.release()));
954  auto suboutput = sys.AllocateOutput(*subcontext);
955  context->AddSystem(i, std::move(subcontext), std::move(suboutput));
956  }
957 
958  // TODO(sherm1) Move to separate interconnection phase.
959  // Wire up the Diagram-internal inputs and outputs.
960  for (const auto& connection : connection_map_) {
961  const OutputPortLocator& src = connection.second;
962  const InputPortLocator& dest = connection.first;
963  context->Connect(ConvertToContextPortIdentifier(src),
964  ConvertToContextPortIdentifier(dest));
965  }
966 
967  // Diagram-external input ports are exported from child subsystems. Inform
968  // the new context so that it it can set up dependency tracking for the
969  // child subsystem's input port on its parent Diagram's input port.
970  for (InputPortIndex i(0); i < this->get_num_input_ports(); ++i) {
971  const InputPortLocator& id = input_port_ids_[i];
972  context->ExportInput(i, ConvertToContextPortIdentifier(id));
973  }
974 
975  // TODO(sherm1) Move to final resource allocation phase.
976  context->MakeState();
977  context->MakeParameters();
978 
979  return context;
980  }
981 
982  // Permits child Systems to take a look at the completed Context to see
983  // if they have any objections.
984  void DoValidateAllocatedContext(const ContextBase& context_base) const final {
985  auto& context = dynamic_cast<const DiagramContext<T>&>(context_base);
986 
987  // Depth-first validation of Context to make sure restrictions are met.
988  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
989  const System<T>& sys = *registered_systems_[i];
990  const Context<T>& subcontext = context.GetSubsystemContext(i);
991  SystemBase::ValidateAllocatedContext(sys, subcontext);
992  }
993  }
994 
995  // Evaluates the value of the specified subsystem input
996  // port in the given context. The port has already been determined _not_ to
997  // be a fixed port, so it must be connected either
998  // - to the output port of a peer subsystem, or
999  // - to an input port of this Diagram,
1000  // - or not connected at all in which case we return null.
1001  const AbstractValue* EvalConnectedSubsystemInputPort(
1002  const ContextBase& context,
1003  const InputPortBase& input_port_base) const final {
1004  auto& diagram_context =
1005  dynamic_cast<const DiagramContext<T>&>(context);
1006  auto& input_port =
1007  dynamic_cast<const InputPortDescriptor<T>&>(input_port_base);
1008  const InputPortLocator id{input_port.get_system(), input_port.get_index()};
1009 
1010  // Find if this input port is exported (connected to an input port of this
1011  // containing diagram).
1012  // TODO(sherm1) Fix this. Shouldn't have to search.
1013  const auto external_it =
1014  std::find(input_port_ids_.begin(), input_port_ids_.end(), id);
1015  const bool is_exported = (external_it != input_port_ids_.end());
1016 
1017  // Find if this input port is connected to an output port.
1018  // TODO(sherm1) Fix this. Shouldn't have to search.
1019  const auto upstream_it = connection_map_.find(id);
1020  const bool is_connected = (upstream_it != connection_map_.end());
1021 
1022  if (!(is_exported || is_connected))
1023  return nullptr;
1024 
1025  DRAKE_DEMAND(is_exported ^ is_connected);
1026 
1027  if (is_exported) {
1028  // The upstream source is an input to this whole Diagram; evaluate that
1029  // input port and use the result as the value for this one.
1030  const InputPortIndex i(external_it - input_port_ids_.begin());
1031  return this->EvalAbstractInput(diagram_context, i);
1032  }
1033 
1034  // The upstream source is an output port of one of this Diagram's child
1035  // subsystems; evaluate it.
1036  // TODO(david-german-tri): Add online algebraic loop detection here.
1037  DRAKE_ASSERT(is_connected);
1038  const OutputPortLocator& prerequisite = upstream_it->second;
1039  return &this->EvalSubsystemOutputPort(diagram_context, prerequisite);
1040  }
1041 
1042  std::string GetParentPathname() const final {
1043  return this->GetSystemPathname();
1044  }
1045 
1046  // Returns true if there might be direct feedthrough from the given
1047  // @p input_port of the Diagram to the given @p output_port of the Diagram.
1048  bool DoHasDirectFeedthrough(int input_port, int output_port) const {
1049  DRAKE_ASSERT(input_port >= 0);
1050  DRAKE_ASSERT(input_port < this->get_num_input_ports());
1051  DRAKE_ASSERT(output_port >= 0);
1052  DRAKE_ASSERT(output_port < this->get_num_output_ports());
1053 
1054  const InputPortLocator& target_input_id = input_port_ids_[input_port];
1055 
1056  // Search the graph for a direct-feedthrough connection from the output_port
1057  // back to the input_port. Maintain a set of the output port identifiers
1058  // that are known to have a direct-feedthrough path to the output_port.
1059  std::set<OutputPortLocator> active_set;
1060  active_set.insert(output_port_ids_[output_port]);
1061  while (!active_set.empty()) {
1062  const OutputPortLocator current_output_id = *active_set.begin();
1063  size_t removed_count = active_set.erase(current_output_id);
1064  DRAKE_ASSERT(removed_count == 1);
1065  const System<T>* sys = current_output_id.first;
1066  for (InputPortIndex i(0); i < sys->get_num_input_ports(); ++i) {
1067  if (sys->HasDirectFeedthrough(i, current_output_id.second)) {
1068  const InputPortLocator curr_input_id(sys, i);
1069  if (curr_input_id == target_input_id) {
1070  // We've found a direct-feedthrough path to the input_port.
1071  return true;
1072  } else {
1073  // We've found an intermediate input port has a direct-feedthrough
1074  // path to the output_port. Add the upstream output port (if there
1075  // is one) to the active set.
1076  auto it = connection_map_.find(curr_input_id);
1077  if (it != connection_map_.end()) {
1078  const OutputPortLocator& upstream_output = it->second;
1079  active_set.insert(upstream_output);
1080  }
1081  }
1082  }
1083  }
1084  }
1085  // If there are no intermediate output ports with a direct-feedthrough path
1086  // to the output_port, there is no direct feedthrough to it from the
1087  // input_port.
1088  return false;
1089  }
1090 
1091  template <typename EventType>
1092  std::unique_ptr<EventCollection<EventType>> AllocateForcedEventCollection(
1093  std::function<
1094  std::unique_ptr<EventCollection<EventType>>(const System<T>*)>
1095  allocater_func) const {
1096  const int num_systems = num_subsystems();
1097  auto ret = std::make_unique<DiagramEventCollection<EventType>>(num_systems);
1098  for (SubsystemIndex i(0); i < num_systems; ++i) {
1099  std::unique_ptr<EventCollection<EventType>> subevent_collection =
1100  allocater_func(registered_systems_[i].get());
1101  ret->set_and_own_subevent_collection(i, std::move(subevent_collection));
1102  }
1103  return std::move(ret);
1104  }
1105 
1106  // For each subsystem, if there is a publish event in its corresponding
1107  // subevent collection, calls its Publish method with the appropriate
1108  // subcontext and subevent collection.
1109  void DispatchPublishHandler(
1110  const Context<T>& context,
1111  const EventCollection<PublishEvent<T>>& event_info) const final {
1112  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1113  DRAKE_DEMAND(diagram_context);
1115  dynamic_cast<const DiagramEventCollection<PublishEvent<T>>&>(
1116  event_info);
1117 
1118  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1119  const EventCollection<PublishEvent<T>>& subinfo =
1120  info.get_subevent_collection(i);
1121 
1122  if (subinfo.HasEvents()) {
1123  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1124  registered_systems_[i]->Publish(subcontext, subinfo);
1125  }
1126  }
1127  }
1128 
1129  // For each subsystem, if there is a discrete update event in its
1130  // corresponding subevent collection, calls its CalcDiscreteVariableUpdates
1131  // method with the appropriate subcontext, subevent collection and
1132  // substate.
1133  void DispatchDiscreteVariableUpdateHandler(
1134  const Context<T>& context,
1135  const EventCollection<DiscreteUpdateEvent<T>>& event_info,
1136  DiscreteValues<T>* discrete_state) const final {
1137  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1138  DRAKE_DEMAND(diagram_context);
1139  auto diagram_discrete =
1140  dynamic_cast<DiagramDiscreteValues<T>*>(discrete_state);
1141  DRAKE_DEMAND(diagram_discrete);
1142 
1143  // As a baseline, initialize all the discrete variables to their
1144  // current values.
1145  diagram_discrete->CopyFrom(context.get_discrete_state());
1146 
1148  dynamic_cast<const DiagramEventCollection<DiscreteUpdateEvent<T>>&>(
1149  event_info);
1150 
1151  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1152  const EventCollection<DiscreteUpdateEvent<T>>& subinfo =
1153  info.get_subevent_collection(i);
1154 
1155  if (subinfo.HasEvents()) {
1156  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1157  DiscreteValues<T>& subdiscrete =
1158  diagram_discrete->get_mutable_subdiscrete(i);
1159 
1160  registered_systems_[i]->CalcDiscreteVariableUpdates(subcontext, subinfo,
1161  &subdiscrete);
1162  }
1163  }
1164  }
1165 
1166  // For each subsystem, if there is an unrestricted update event in its
1167  // corresponding subevent collection, calls its CalcUnrestrictedUpdate
1168  // method with the appropriate subcontext, subevent collection and substate.
1169  void DispatchUnrestrictedUpdateHandler(
1170  const Context<T>& context,
1171  const EventCollection<UnrestrictedUpdateEvent<T>>& event_info,
1172  State<T>* state) const final {
1173  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1174  DRAKE_DEMAND(diagram_context);
1175  auto diagram_state = dynamic_cast<DiagramState<T>*>(state);
1176  DRAKE_DEMAND(diagram_state != nullptr);
1177 
1178  // No need to set state to context's state, since it has already been done
1179  // in System::CalcUnrestrictedUpdate().
1180 
1183  event_info);
1184 
1185  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1187  info.get_subevent_collection(i);
1188 
1189  if (subinfo.HasEvents()) {
1190  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1191  State<T>& substate = diagram_state->get_mutable_substate(i);
1192 
1193  registered_systems_[i]->CalcUnrestrictedUpdate(subcontext, subinfo,
1194  &substate);
1195  }
1196  }
1197  }
1198 
1199  // Tries to recursively find @p target_system's BaseStuff
1200  // (context / state / etc). nullptr is returned if @p target_system is not
1201  // a subsystem of this diagram. This template function should only be used
1202  // to reduce code repetition for DoGetMutableTargetSystemContext(),
1203  // DoGetTargetSystemContext(), DoGetMutableTargetSystemState(), and
1204  // DoGetTargetSystemState().
1205  // @param target_system The subsystem of interest.
1206  // @param my_stuff BaseStuff that's associated with this diagram.
1207  // @param recursive_getter A member function of System that returns sub
1208  // context or state. Should be one of the four functions listed above.
1209  // @param get_child_stuff A member function of DiagramContext or DiagramState
1210  // that returns context or state given the index of the subsystem.
1211  //
1212  // @tparam BaseStuff Can be Context<T>, const Context<T>, State<T> and
1213  // const State<T>.
1214  // @tparam DerivedStuff Can be DiagramContext<T>,
1215  // const DiagramContext<T>, DiagramState<T> and const DiagramState<T>.
1216  //
1217  // @pre @p target_system cannot be `this`. The caller should check for this
1218  // edge case.
1219  template <typename BaseStuff, typename DerivedStuff>
1220  BaseStuff* GetSubsystemStuff(
1221  const System<T>& target_system, BaseStuff* my_stuff,
1222  std::function<BaseStuff*(const System<T>*, const System<T>&, BaseStuff*)>
1223  recursive_getter,
1224  std::function<BaseStuff&(DerivedStuff*, SubsystemIndex)> get_child_stuff)
1225  const {
1226  static_assert(
1227  std::is_same<BaseStuff,
1228  typename std::remove_pointer<BaseStuff>::type>::value,
1229  "BaseStuff cannot be a pointer");
1230  static_assert(
1231  std::is_same<DerivedStuff,
1232  typename std::remove_pointer<DerivedStuff>::type>::value,
1233  "DerivedStuff cannot be a pointer");
1234 
1235  DRAKE_DEMAND(my_stuff != nullptr);
1236  DRAKE_DEMAND(&target_system != this);
1237  DerivedStuff& my_stuff_as_derived = dynamic_cast<DerivedStuff&>(*my_stuff);
1238 
1239  SubsystemIndex index(0);
1240  for (const auto& child : registered_systems_) {
1241  BaseStuff& child_stuff = get_child_stuff(&my_stuff_as_derived, index);
1242 
1243  BaseStuff* const target_stuff =
1244  recursive_getter(child.get(), target_system, &child_stuff);
1245 
1246  if (target_stuff != nullptr) {
1247  return target_stuff;
1248  }
1249  ++index;
1250  }
1251 
1252  return nullptr;
1253  }
1254 
1255  // Uses this Diagram<T> to manufacture a Diagram<NewType>::Blueprint,
1256  // using system scalar conversion.
1257  //
1258  // @tparam NewType The scalar type to which to convert.
1259  template <typename NewType>
1260  std::unique_ptr<typename Diagram<NewType>::Blueprint> ConvertScalarType()
1261  const {
1262  std::vector<std::unique_ptr<System<NewType>>> new_systems;
1263  // Recursively convert all the subsystems.
1264  std::map<const System<T>*, const System<NewType>*> old_to_new_map;
1265  for (const auto& old_system : registered_systems_) {
1266  // Convert old_system to new_system using the old_system's converter.
1267  std::unique_ptr<System<NewType>> new_system =
1268  old_system->get_system_scalar_converter().
1269  template Convert<NewType>(*old_system);
1270  DRAKE_DEMAND(new_system != nullptr);
1271 
1272  // Update our mapping and take ownership.
1273  old_to_new_map[old_system.get()] = new_system.get();
1274  new_systems.push_back(std::move(new_system));
1275  }
1276 
1277  // Set up the blueprint.
1278  auto blueprint = std::make_unique<typename Diagram<NewType>::Blueprint>();
1279  // Make all the inputs and outputs.
1280  for (const InputPortLocator& id : input_port_ids_) {
1281  const System<NewType>* new_system = old_to_new_map[id.first];
1282  const InputPortIndex port = id.second;
1283  blueprint->input_port_ids.emplace_back(new_system, port);
1284  }
1285  for (const OutputPortLocator& id : output_port_ids_) {
1286  const System<NewType>* new_system = old_to_new_map[id.first];
1287  const OutputPortIndex port = id.second;
1288  blueprint->output_port_ids.emplace_back(new_system, port);
1289  }
1290  // Make all the connections.
1291  for (const auto& edge : connection_map_) {
1292  const InputPortLocator& old_dest = edge.first;
1293  const System<NewType>* const dest_system = old_to_new_map[old_dest.first];
1294  const InputPortIndex dest_port = old_dest.second;
1295  const typename Diagram<NewType>::InputPortLocator new_dest{dest_system,
1296  dest_port};
1297 
1298  const OutputPortLocator& old_src = edge.second;
1299  const System<NewType>* const src_system = old_to_new_map[old_src.first];
1300  const OutputPortIndex src_port = old_src.second;
1301  const typename Diagram<NewType>::OutputPortLocator new_src{src_system,
1302  src_port};
1303 
1304  blueprint->connection_map[new_dest] = new_src;
1305  }
1306  // Move the new systems into the blueprint.
1307  blueprint->systems = std::move(new_systems);
1308 
1309  return blueprint;
1310  }
1311 
1312  // Aborts for scalar types that are not numeric, since there is no reasonable
1313  // definition of "next update time" outside of the real line.
1314  //
1315  // @tparam T1 SFINAE boilerplate for the scalar type. Do not set.
1316  template <typename T1 = T>
1318  DoCalcNextUpdateTimeImpl(const Context<T1>&, CompositeEventCollection<T1>*,
1319  T1*) const {
1321  "The default implementation of Diagram<T>::DoCalcNextUpdateTime "
1322  "only works with types that are drake::is_numeric.");
1323  }
1324 
1325  // Computes the next update time across all the scheduled events, for
1326  // scalar types that are numeric.
1327  //
1328  // @tparam T1 SFINAE boilerplate for the scalar type. Do not set.
1329  template <typename T1 = T>
1330  typename std::enable_if<is_numeric<T1>::value>::type DoCalcNextUpdateTimeImpl(
1331  const Context<T1>& context, CompositeEventCollection<T1>* event_info,
1332  T1* time) const {
1333  auto diagram_context = dynamic_cast<const DiagramContext<T1>*>(&context);
1334  auto info = dynamic_cast<DiagramCompositeEventCollection<T1>*>(event_info);
1335  DRAKE_DEMAND(diagram_context != nullptr);
1336  DRAKE_DEMAND(info != nullptr);
1337 
1338  *time = std::numeric_limits<T1>::infinity();
1339 
1340  // Iterate over the subsystems, and harvest the most imminent updates.
1341  std::vector<T1> times(num_subsystems());
1342  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1343  const Context<T1>& subcontext = diagram_context->GetSubsystemContext(i);
1344  CompositeEventCollection<T1>& subinfo =
1345  info->get_mutable_subevent_collection(i);
1346  const T1 sub_time =
1347  registered_systems_[i]->CalcNextUpdateTime(subcontext, &subinfo);
1348  times[i] = sub_time;
1349 
1350  if (sub_time < *time) {
1351  *time = sub_time;
1352  }
1353  }
1354 
1355  // For all the subsystems whose next update time is bigger than *time,
1356  // clear their event collections.
1357  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1358  if (times[i] > *time)
1359  info->get_mutable_subevent_collection(i).Clear();
1360  }
1361  }
1362 
1363  std::map<PeriodicEventData, std::vector<const Event<T>*>,
1364  PeriodicEventDataComparator> DoGetPeriodicEvents() const override {
1365  std::map<PeriodicEventData,
1366  std::vector<const Event<T>*>,
1367  PeriodicEventDataComparator> periodic_events_map;
1368 
1369  for (int i = 0; i < num_subsystems(); ++i) {
1370  auto sub_map = registered_systems_[i]->GetPeriodicEvents();
1371  for (const auto& sub_attr_events : sub_map) {
1372  const auto& sub_vec = sub_attr_events.second;
1373  auto& vec = periodic_events_map[sub_attr_events.first];
1374  vec.insert(vec.end(), sub_vec.begin(), sub_vec.end());
1375  }
1376  }
1377 
1378  return periodic_events_map;
1379  }
1380 
1381  void DoGetPerStepEvents(
1382  const Context<T>& context,
1383  CompositeEventCollection<T>* event_info) const override {
1384  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1385  auto info = dynamic_cast<DiagramCompositeEventCollection<T>*>(event_info);
1386  DRAKE_DEMAND(diagram_context != nullptr);
1387  DRAKE_DEMAND(info != nullptr);
1388 
1389  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1390  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1391  CompositeEventCollection<T>& subinfo =
1392  info->get_mutable_subevent_collection(i);
1393 
1394  registered_systems_[i]->GetPerStepEvents(subcontext, &subinfo);
1395  }
1396  }
1397 
1398  void DoGetInitializationEvents(
1399  const Context<T>& context,
1400  CompositeEventCollection<T>* event_info) const override {
1401  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1402  auto info = dynamic_cast<DiagramCompositeEventCollection<T>*>(event_info);
1403  DRAKE_DEMAND(diagram_context != nullptr);
1404  DRAKE_DEMAND(info != nullptr);
1405 
1406  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1407  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1408  CompositeEventCollection<T>& subinfo =
1409  info->get_mutable_subevent_collection(i);
1410 
1411  registered_systems_[i]->GetInitializationEvents(subcontext, &subinfo);
1412  }
1413  }
1414 
1415  // A structural outline of a Diagram, produced by DiagramBuilder.
1416  struct Blueprint {
1417  // The ordered subsystem ports that are inputs to the entire diagram.
1418  std::vector<InputPortLocator> input_port_ids;
1419  // The ordered subsystem ports that are outputs of the entire diagram.
1420  std::vector<OutputPortLocator> output_port_ids;
1421  // A map from the input ports of constituent systems to the output ports
1422  // on which they depend. This graph is possibly cyclic, but must not
1423  // contain an algebraic loop.
1424  std::map<InputPortLocator, OutputPortLocator> connection_map;
1425  // All of the systems to be included in the diagram.
1426  std::vector<std::unique_ptr<System<T>>> systems;
1427  };
1428 
1429  // Constructs a Diagram from the Blueprint that a DiagramBuilder produces.
1430  // This constructor is private because only DiagramBuilder calls it. The
1431  // constructor takes the systems from the blueprint.
1432  explicit Diagram(std::unique_ptr<Blueprint> blueprint) : Diagram() {
1433  Initialize(std::move(blueprint));
1434  }
1435 
1436  // Validates the given @p blueprint and sets up the Diagram accordingly.
1437  void Initialize(std::unique_ptr<Blueprint> blueprint) {
1438  // We must be given something to own.
1439  DRAKE_DEMAND(!blueprint->systems.empty());
1440  // We must not already own any subsystems.
1441  DRAKE_DEMAND(registered_systems_.empty());
1442 
1443  // Move the data from the blueprint into private member variables.
1444  connection_map_ = std::move(blueprint->connection_map);
1445  input_port_ids_ = std::move(blueprint->input_port_ids);
1446  output_port_ids_ = std::move(blueprint->output_port_ids);
1447  registered_systems_ = std::move(blueprint->systems);
1448 
1449  // Generate a map from the System pointer to its index in the registered
1450  // order.
1451  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1452  system_index_map_[registered_systems_[i].get()] = i;
1453  SystemBase::set_parent_service(registered_systems_[i].get(), this);
1454  }
1455 
1456  // Generate constraints for the diagram from the constraints on the
1457  // subsystems.
1458  for (SubsystemIndex i(0); i < num_subsystems(); ++i) {
1459  const auto sys = registered_systems_[i].get();
1460  for (SystemConstraintIndex j(0); j < sys->get_num_constraints(); ++j) {
1461  const auto c = &(sys->get_constraint(j));
1462  typename SystemConstraint<T>::CalcCallback diagram_calc =
1463  [this, sys, c](const Context<T>& context, VectorX<T>* value) {
1464  c->Calc(this->GetSubsystemContext(*sys, context), value);
1465  };
1466  this->AddConstraint(std::make_unique<SystemConstraint<T>>(
1467  diagram_calc, c->size(), c->type(),
1468  sys->get_name() + ":" + c->description()));
1469  }
1470  }
1471 
1472  // Every system must appear exactly once.
1473  DRAKE_DEMAND(registered_systems_.size() == system_index_map_.size());
1474  // Every port named in the connection_map_ must actually exist.
1475  DRAKE_ASSERT(PortsAreValid());
1476  // Every subsystem must have a unique name.
1477  DRAKE_THROW_UNLESS(NamesAreUniqueAndNonEmpty());
1478 
1479  // Add the inputs to the Diagram topology, and check their invariants.
1480  for (const InputPortLocator& id : input_port_ids_) {
1481  ExportInput(id);
1482  }
1483  for (const OutputPortLocator& id : output_port_ids_) {
1484  ExportOutput(id);
1485  }
1486 
1487  // Identify the intersection of the subsystems' scalar conversion support.
1488  // Remove all conversions that at least one subsystem did not support.
1489  SystemScalarConverter& this_scalar_converter =
1490  SystemImpl::get_mutable_system_scalar_converter(this);
1491  for (const auto& system : registered_systems_) {
1492  this_scalar_converter.RemoveUnlessAlsoSupportedBy(
1493  system->get_system_scalar_converter());
1494  }
1495 
1496  this->set_forced_publish_events(
1497  AllocateForcedEventCollection<PublishEvent<T>>(
1499  this->set_forced_discrete_update_events(
1500  AllocateForcedEventCollection<DiscreteUpdateEvent<T>>(
1502  this->set_forced_unrestricted_update_events(
1503  AllocateForcedEventCollection<UnrestrictedUpdateEvent<T>>(
1505  }
1506 
1507  // Exposes the given port as an input of the Diagram.
1508  void ExportInput(const InputPortLocator& port) {
1509  const System<T>* const sys = port.first;
1510  const int port_index = port.second;
1511  // Fail quickly if this system is not part of the diagram.
1512  GetSystemIndexOrAbort(sys);
1513 
1514  // Add this port to our externally visible topology.
1515  const auto& subsystem_descriptor = sys->get_input_port(port_index);
1516  this->DeclareInputPort(subsystem_descriptor.get_data_type(),
1517  subsystem_descriptor.size(),
1518  subsystem_descriptor.get_random_type());
1519  }
1520 
1521  // Exposes the given subsystem output port as an output of the Diagram.
1522  void ExportOutput(const OutputPortLocator& port) {
1523  const System<T>* const sys = port.first;
1524  const int port_index = port.second;
1525  const auto& source_output_port = sys->get_output_port(port_index);
1526  auto diagram_port = std::make_unique<internal::DiagramOutputPort<T>>(
1527  *this, &source_output_port);
1528  this->CreateOutputPort(std::move(diagram_port));
1529  }
1530 
1531  // Returns a reference to the value in the given context, of the specified
1532  // output port of one of this Diagram's immediate subsystems. Calculates the
1533  // result first before returning it.
1534  // TODO(sherm1) Replace with cached version to avoid recalculation.
1535  const AbstractValue& EvalSubsystemOutputPort(
1536  const DiagramContext<T>& context, const OutputPortLocator& id) const {
1537  const System<T>* const system = id.first;
1538  const OutputPortIndex port_index(id.second);
1539  const OutputPort<T>& port = system->get_output_port(port_index);
1540  const SubsystemIndex i = GetSystemIndexOrAbort(system);
1541  SPDLOG_TRACE(log(), "Evaluating output for subsystem {}, port {}",
1542  system->GetSystemPathname(), port_index);
1543  const Context<T>& subsystem_context = context.GetSubsystemContext(i);
1544  SystemOutput<T>* subsystem_output = context.GetSubsystemOutput(i);
1545  AbstractValue* port_output = subsystem_output->GetMutableData(port_index);
1546  port.Calc(subsystem_context, port_output);
1547  return *port_output;
1548  }
1549 
1550  // Converts an InputPortLocator to a DiagramContext::InputPortIdentifier.
1551  // The DiagramContext::InputPortIdentifier contains the index of the System in
1552  // the diagram, instead of an actual pointer to the System.
1553  // TODO(sherm1) Should just use the (SystemIndex,PortIndex) form everywhere.
1555  ConvertToContextPortIdentifier(const InputPortLocator& locator) const {
1556  typename DiagramContext<T>::InputPortIdentifier identifier;
1557  identifier.first = GetSystemIndexOrAbort(locator.first);
1558  identifier.second = locator.second;
1559  return identifier;
1560  }
1561 
1562  // Converts an OutputPortLocator to a DiagramContext::OutputPortIdentifier.
1563  // The DiagramContext::OutputPortIdentifier contains the index of the System
1564  // in the diagram, instead of an actual pointer to the System.
1566  ConvertToContextPortIdentifier(const OutputPortLocator& locator) const {
1567  typename DiagramContext<T>::OutputPortIdentifier identifier;
1568  identifier.first = GetSystemIndexOrAbort(locator.first);
1569  identifier.second = locator.second;
1570  return identifier;
1571  }
1572 
1573  // Sets up the OutputPortValue pointers in @p output to point to the subsystem
1574  // output values, found in @p context, that are the outputs of this Diagram.
1575  void ExposeSubsystemOutputs(const DiagramContext<T>& context,
1576  internal::DiagramOutput<T>* output) const {
1577  // The number of output ports of this diagram must equal the number of
1578  // ports in the provided DiagramOutput.
1579  const int num_ports = static_cast<int>(output_port_ids_.size());
1580  DRAKE_DEMAND(output->get_num_ports() == num_ports);
1581 
1582  for (OutputPortIndex i(0); i < num_ports; ++i) {
1583  const OutputPortLocator& id = output_port_ids_[i];
1584  // For each configured output port ID, obtain from the DiagramContext the
1585  // actual OutputPortValue that supplies its value.
1586  const SubsystemIndex sys_index = GetSystemIndexOrAbort(id.first);
1587  const OutputPortIndex port_index = id.second;
1588  SystemOutput<T>* subsystem_output = context.GetSubsystemOutput(sys_index);
1589  OutputPortValue* output_port_value =
1590  subsystem_output->get_mutable_port_value(port_index);
1591 
1592  // Then, put a pointer to that OutputPortValue in the DiagramOutput.
1593  (*output->get_mutable_port_values())[i] = output_port_value;
1594  }
1595  }
1596 
1597  // Returns true if every port mentioned in the connection map exists.
1598  bool PortsAreValid() const {
1599  for (const auto& entry : connection_map_) {
1600  const InputPortLocator& dest = entry.first;
1601  const OutputPortLocator& src = entry.second;
1602  if (dest.second < 0 || dest.second >= dest.first->get_num_input_ports()) {
1603  return false;
1604  }
1605  if (src.second < 0 || src.second >= src.first->get_num_output_ports()) {
1606  return false;
1607  }
1608  }
1609  return true;
1610  }
1611 
1612  // Returns true if every subsystem has a unique, non-empty name.
1613  // O(N * log(N)) in the number of subsystems.
1614  bool NamesAreUniqueAndNonEmpty() const {
1615  std::set<std::string> names;
1616  for (const auto& system : registered_systems_) {
1617  const std::string& name = system->get_name();
1618  if (name.empty()) {
1619  // This can only happen if someone blanks out the name *after* adding
1620  // it to DiagramBuilder; if an empty name is given to DiagramBuilder,
1621  // a default non-empty name is automatically assigned.
1622  log()->error("Subsystem of type {} has no name",
1623  NiceTypeName::Get(*system));
1624  // We skip names.insert here, so that the return value will be false.
1625  continue;
1626  }
1627  if (names.find(name) != names.end()) {
1628  log()->error("Non-unique name \"{}\" for subsystem of type {}", name,
1629  NiceTypeName::Get(*system));
1630  }
1631  names.insert(name);
1632  }
1633  return names.size() == registered_systems_.size();
1634  }
1635 
1636  int num_subsystems() const {
1637  return static_cast<int>(registered_systems_.size());
1638  }
1639 
1640  // A map from the input ports of constituent systems, to the output ports of
1641  // the systems from which they get their values.
1642  std::map<InputPortLocator, OutputPortLocator> connection_map_;
1643 
1644  // The Systems in this Diagram, which are owned by this Diagram, in the order
1645  // they were registered. Index by SubsystemIndex.
1646  std::vector<std::unique_ptr<System<T>>> registered_systems_;
1647 
1648  // Map to quickly satisify "What is the subsytem index of the child system?"
1649  std::map<const System<T>*, SubsystemIndex> system_index_map_;
1650 
1651  // The ordered inputs and outputs of this Diagram. Index by InputPortIndex
1652  // and OutputPortIndex.
1653  std::vector<InputPortLocator> input_port_ids_;
1654  std::vector<OutputPortLocator> output_port_ids_;
1655 
1656  // For all T, Diagram<T> considers DiagramBuilder<T> a friend, so that the
1657  // builder can set the internal state correctly.
1658  friend class DiagramBuilder<T>;
1659 
1660  // For any T1 & T2, Diagram<T1> considers Diagram<T2> a friend, so that
1661  // Diagram can provide transmogrification methods across scalar types.
1662  // See Diagram<T>::ConvertScalarType.
1663  template <typename> friend class Diagram;
1664 };
1665 
1666 } // namespace systems
1667 } // 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:759
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:720
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:866
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:422
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:184
const SystemScalarConverter & get_system_scalar_converter() const
(Advanced) Returns the SystemScalarConverter for this object.
Definition: system.h:1308
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:182
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:604
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:211
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:646
OutputPortValue contains the value of a single System output port.
Definition: output_port_value.h:22
const Context< T > & GetSubsystemContext(SubsystemIndex index) const
Returns the context structure for a given constituent system index.
Definition: diagram_context.h:256
std::pair< const System< double > *, OutputPortIndex > OutputPortLocator
Definition: diagram.h:165
Definition: automotive_demo.cc:90
std::vector< OutputPortValue * > * get_mutable_port_values()
Definition: diagram.h:130
std::unique_ptr< SystemOutput< T > > AllocateOutput(const Context< T > &context) const override
Returns a container that can hold the values of all of this System&#39;s output ports.
Definition: diagram.h:340
DiagramOutput< T > * DoClone() const override
The NVI implementation of Clone().
Definition: diagram.h:135
std::mt19937 RandomGenerator
Defines the implementation of the stdc++ concept UniformRandomBitGenerator to be used by the Systems ...
Definition: system.h:71
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:913
Diagram()
Constructs an uninitialized Diagram.
Definition: diagram.h:625
void set_numeric_parameters(std::unique_ptr< DiscreteValues< T >> numeric_params)
Definition: parameters.h:90
SystemOutput< T > * GetSubsystemOutput(SubsystemIndex index) const
Returns the output structure for a given constituent system at index.
Definition: diagram_context.h:246
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:1125
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:172
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:696
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:31
std::unique_ptr< ContinuousState< T > > AllocateTimeDerivatives() const override
Aggregates the time derivatives from each subsystem into a DiagramContinuousState.
Definition: diagram.h:377
Abstract base class that represents an event.
Definition: event.h:166
#define DRAKE_ABORT_MSG(message)
Aborts the program (via ::abort) with a message showing at least the given message (macro argument)...
Definition: drake_assert.h:51
VectorBase is an abstract base class that real-valued signals between Systems and real-valued System ...
Definition: vector_base.h:27
const InputPortDescriptor< T > & get_input_port(int port_index) const
Returns the typed input port at index port_index.
Definition: system.h:972
std::unique_ptr< DiscreteValues< T > > AllocateDiscreteVariables() const override
Aggregates the discrete update variables from each subsystem into a DiagramDiscreteVariables.
Definition: diagram.h:388
#define SPDLOG_TRACE(logger,...)
Definition: text_logging.h:124
Context< T > & GetMutableSubsystemContext(const System< T > &subsystem, Context< T > *context) const
Returns the subcontext that corresponds to the system subsystem.
Definition: diagram.h:448
const OutputPort< T > & get_source_output_port() const
Obtain a reference to the subsystem output port that was exported to create this diagram port...
Definition: diagram.h:68
Holds information about the subsystem output port that has been exported to become one of this Diagra...
Definition: diagram.h:47
#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:36
logging::logger * log()
Retrieve an instance of a logger to use for logging; for example: drake::log()->info("potato!") ...
Definition: text_logging.cc:38
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:211
std::vector< double > vector
Definition: translator_test.cc:20
CompositeEventCollection for a Diagram.
Definition: event_collection.h:628
This extends InputPortBase with some scalar type-dependent methods.
Definition: input_port_descriptor.h:19
BasicVector< T > * DoAllocateInputVector(const InputPortDescriptor< T > &descriptor) const override
Allocates an input vector of the leaf type that the System requires on the port specified by descript...
Definition: diagram.h:919
DiagramBuilder is a factory class for Diagram.
Definition: diagram.h:35
DiagramOutput is an implementation of SystemOutput that holds unowned OutputPortValue pointers...
Definition: diagram.h:112
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:71
std::unique_ptr< AbstractValue > AllocateInputAbstract(const InputPortDescriptor< T > &descriptor) const
Given a port descriptor, allocates the abstract storage.
Definition: system.h:125
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:469
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:128
Provides non-templatized 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:503
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:175
#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:26
void SetRandomParameters(const Context< T > &context, Parameters< T > *params, RandomGenerator *generator) const override
Assigns random values to all parameters.
Definition: diagram.h:292
AbstractValue * GetMutableData(int index)
Returns a pointer to the data inside the port at index, and updates the version so that Contexts depe...
Definition: output_port_value.h:123
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:398
void RemoveUnlessAlsoSupportedBy(const SystemScalarConverter &other)
Removes from this converter all pairs where other.IsConvertible<T, U> is false.
Definition: system_scalar_converter.cc:45
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:484
std::unique_ptr< BasicVector< T > > AllocateInputVector(const InputPortDescriptor< T > &descriptor) const
Given a port descriptor, allocates the vector storage.
Definition: system.h:112
#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:29
The DiagramContext is a container for all of the data necessary to uniquely determine the computation...
Definition: diagram_context.h:117
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:276
State is a container for all the data comprising the complete state of a particular System at a parti...
Definition: state.h:27
const OutputPortValue & get_port_value(int index) const override
Definition: diagram.h:125
void SetDefaultParameters(const Context< T > &context, Parameters< T > *params) const override
Assigns default values to all parameters.
Definition: diagram.h:227
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:33
Base class for all System functionality that is dependent on the templatized scalar type T for input...
Definition: input_port_descriptor.h:12
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:733
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:293
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
OutputPortValue * get_mutable_port_value(int index) override
Definition: diagram.h:120
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:746
void GetGraphvizFragment(std::stringstream *dot) const override
Returns a Graphviz fragment describing this Diagram.
Definition: diagram.h:516
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:659
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:774
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
virtual void GetGraphvizInputPortToken(const InputPortDescriptor< T > &port, std::stringstream *dot) const
Appends a fragment to the dot stream identifying the graphviz node representing port.
Definition: system.h:1118
static void ValidateAllocatedContext(const SystemBase &system, const ContextBase &context)
Allows Diagram to use its private ValidateAllocatedContext() to invoke the same method on its childre...
Definition: system_base.h:598
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:199
static void set_parent_service(SystemBase *child, const internal::SystemParentServiceInterface *parent_service)
Declares that parent_service is the service interface of the Diagram that owns this subsystem...
Definition: system_base.h:583
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:802
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:818
Parameters is a container for variables that parameterize a System so that it can represent a family ...
Definition: parameters.h:26
PortDataType get_data_type() const
Gets the port data type specified at port construction.
Definition: output_port.h:132
An abstract base class template for the values of the output ports of a System.
Definition: output_port_value.h:88
void error(const char *, const Args &...)
Definition: text_logging.h:110
virtual std::unique_ptr< SystemOutput< T > > AllocateOutput(const Context< T > &context) const =0
Returns a container that can hold the values of all of this System&#39;s output ports.
AbstractValue & get_mutable_abstract_parameter(int index)
Returns the abstract-valued parameter at index.
Definition: parameters.h:104
std::pair< const System< double > *, InputPortIndex > InputPortLocator
Definition: diagram.h:164
virtual OutputPortValue * get_mutable_port_value(int index)=0
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:787
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:163
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:437
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
AbstractValue * DoAllocateInputAbstract(const InputPortDescriptor< T > &descriptor) const override
Allocates an abstract input of the leaf type that the System requires on the port specified by descri...
Definition: diagram.h:929
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
const System< T > * get_system() const
Returns a reference to the System that owns this input port.
Definition: input_port_descriptor.h:45
std::pair< SubsystemIndex, OutputPortIndex > OutputPortIdentifier
Identifies a child subsystem&#39;s output port.
Definition: diagram_context.h:130
The argument to Convert need not be the exact type S that was used to populate the SystemScalarConver...
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:614
void Calc(const Context< T > &context, AbstractValue *value) const
Unconditionally computes the value of this output port with respect to the given context, into an already-allocated AbstractValue object whose concrete type must be exactly the same as the type returned by this port&#39;s allocator.
Definition: output_port.h:101
virtual State< T > & get_mutable_state()=0
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:639
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:152
const OutputPort< T > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:979
int get_num_ports() const override
Definition: diagram.h:118
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:26
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:122
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:135
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
int size() const
Returns the fixed size expected for a vector-valued output port.
Definition: output_port.h:136
int size() const
Definition: system_constraint.h:120
void GetGraphvizInputPortToken(const InputPortDescriptor< T > &port, std::stringstream *dot) const override
Appends a fragment to the dot stream identifying the graphviz node representing port.
Definition: diagram.h:598
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:459
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:493
OutputPortIndex get_index() const
Returns the index of this output port within the owning System.
Definition: output_port.h:127
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
An InputPort is a System resource that describes the kind of input a System accepts, on a given port.
Definition: input_port_base.h:20
int data
Definition: value_test.cc:19