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