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