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"
25 
26 namespace drake {
27 namespace systems {
28 
29 template <typename T>
30 class Diagram;
31 template <typename T>
33 
34 namespace internal {
35 
38 template <typename U>
39 std::vector<U*> Unpack(const std::vector<std::unique_ptr<U>>& in) {
40  std::vector<U*> out(in.size());
41  std::transform(in.begin(), in.end(), out.begin(),
42  [](const std::unique_ptr<U>& p) { return p.get(); });
43  return out;
44 }
45 
46 //==============================================================================
47 // DIAGRAM OUTPUT PORT
48 //==============================================================================
53 template <typename T>
54 class DiagramOutputPort : public OutputPort<T> {
55  public:
57 
58 
59  DiagramOutputPort(const Diagram<T>& diagram,
62  const OutputPort<T>* source_output_port)
63  : OutputPort<T>(diagram, source_output_port->get_data_type(),
64  source_output_port->size()),
65  source_output_port_(source_output_port),
66  subsystem_index_(
67  diagram.GetSystemIndexOrAbort(&source_output_port->get_system())) {}
68 
69  ~DiagramOutputPort() final = default;
70 
75  return *source_output_port_;
76  }
77 
78  private:
79  // These forward to the source system output port, passing in just the source
80  // System's Context, not the whole Diagram context we're given.
81  std::unique_ptr<AbstractValue> DoAllocate(
82  const Context<T>& context) const final {
83  const Context<T>& subcontext = get_subcontext(context);
84  return source_output_port_->Allocate(subcontext);
85  }
86 
87  void DoCalc(const Context<T>& context, AbstractValue* value) const final {
88  const Context<T>& subcontext = get_subcontext(context);
89  return source_output_port_->Calc(subcontext, value);
90  }
91 
92  const AbstractValue& DoEval(const Context<T>& context) const final {
93  const Context<T>& subcontext = get_subcontext(context);
94  return source_output_port_->Eval(subcontext);
95  }
96 
97  // Dig out the right subcontext for delegation.
98  const Context<T>& get_subcontext(const Context<T>& context) const {
99  const DiagramContext<T>* diagram_context =
100  dynamic_cast<const DiagramContext<T>*>(&context);
101  DRAKE_DEMAND(diagram_context != nullptr);
102  return diagram_context->GetSubsystemContext(subsystem_index_);
103  }
104 
105  const OutputPort<T>* const source_output_port_;
106  const int subsystem_index_;
107 };
108 
109 //==============================================================================
110 // DIAGRAM OUTPUT
111 //==============================================================================
117 template <typename T>
118 class DiagramOutput : public SystemOutput<T> {
119  public:
121 
122  DiagramOutput() = default;
123 
124  int get_num_ports() const override { return static_cast<int>(ports_.size()); }
125 
127  DRAKE_DEMAND(index >= 0 && index < get_num_ports());
128  return ports_[index];
129  }
130 
131  const OutputPortValue& get_port_value(int index) const override {
132  DRAKE_DEMAND(index >= 0 && index < get_num_ports());
133  return *ports_[index];
134  }
135 
136  std::vector<OutputPortValue*>* get_mutable_port_values() { return &ports_; }
137 
138  protected:
139  // Returns a clone that has the same number of output ports, with values
140  // set to nullptr.
141  DiagramOutput<T>* DoClone() const override {
142  DiagramOutput<T>* clone = new DiagramOutput<T>();
143  clone->ports_.resize(get_num_ports());
144  return clone;
145  }
146 
147  private:
148  std::vector<OutputPortValue*> ports_;
149 };
150 
151 //==============================================================================
152 // DIAGRAM TIME DERIVATIVES
153 //==============================================================================
157 template <typename T>
159  public:
161 
163  std::vector<std::unique_ptr<ContinuousState<T>>>&& substates)
164  : DiagramContinuousState<T>(Unpack(substates)),
165  substates_(std::move(substates)) {}
166 
168 
169  private:
170  std::vector<std::unique_ptr<ContinuousState<T>>> substates_;
171 };
172 
173 //==============================================================================
174 // DIAGRAM DISCRETE VARIABLES
175 //==============================================================================
179 template <typename T>
181  public:
183 
185  std::vector<std::unique_ptr<DiscreteValues<T>>>&& subdiscretes)
186  : DiscreteValues<T>(Flatten(Unpack(subdiscretes))),
187  subdiscretes_(std::move(subdiscretes)) {}
188 
190 
191  int num_subdiscretes() const {
192  return static_cast<int>(subdiscretes_.size());
193  }
194 
196  DRAKE_DEMAND(index >= 0 && index < num_subdiscretes());
197  return subdiscretes_[index].get();
198  }
199 
200  private:
201  std::vector<BasicVector<T>*> Flatten(
202  const std::vector<DiscreteValues<T>*>& in) const {
203  std::vector<BasicVector<T>*> out;
204  for (const DiscreteValues<T>* xd : in) {
205  const std::vector<BasicVector<T>*>& xd_data = xd->get_data();
206  out.insert(out.end(), xd_data.begin(), xd_data.end());
207  }
208  return out;
209  }
210 
211  std::vector<std::unique_ptr<DiscreteValues<T>>> subdiscretes_;
212 };
213 
214 } // namespace internal
215 
216 //==============================================================================
217 // DIAGRAM
218 //==============================================================================
227 template <typename T>
228 class Diagram : public System<T>,
230  public:
231  // Diagram objects are neither copyable nor moveable.
233 
234  typedef typename std::pair<const System<T>*, int> PortIdentifier;
235 
236  ~Diagram() override {}
237 
239  std::vector<const systems::System<T>*> GetSystems() const {
240  std::vector<const systems::System<T>*> result;
241  result.reserve(registered_systems_.size());
242  for (const auto& system : registered_systems_) {
243  result.push_back(system.get());
244  }
245  return result;
246  }
247 
248  std::multimap<int, int> GetDirectFeedthroughs() const final {
249  std::multimap<int, int> pairs;
250  for (int u = 0; u < this->get_num_input_ports(); ++u) {
251  for (int v = 0; v < this->get_num_output_ports(); ++v) {
252  if (DoHasDirectFeedthrough(u, v)) {
253  pairs.emplace(u, v);
254  }
255  }
256  }
257  return pairs;
258  };
259 
262  std::unique_ptr<CompositeEventCollection<T>>
264  const int num_systems = num_subsystems();
265  std::vector<std::unique_ptr<CompositeEventCollection<T>>> subevents(
266  num_systems);
267  for (int i = 0; i < num_systems; ++i) {
268  subevents[i] = registered_systems_[i]->AllocateCompositeEventCollection();
269  }
270 
271  return std::make_unique<DiagramCompositeEventCollection<T>>(
272  std::move(subevents));
273  }
274 
275  std::unique_ptr<Context<T>> AllocateContext() const override {
276  const int num_systems = num_subsystems();
277  // Reserve inputs as specified during Diagram initialization.
278  auto context = std::make_unique<DiagramContext<T>>(num_systems);
279 
280  // Add each constituent system to the Context.
281  for (int i = 0; i < num_systems; ++i) {
282  const System<T>* const sys = registered_systems_[i].get();
283  auto subcontext = sys->AllocateContext();
284  auto suboutput = sys->AllocateOutput(*subcontext);
285  context->AddSystem(i, std::move(subcontext), std::move(suboutput));
286  }
287 
288  // Wire up the Diagram-internal inputs and outputs.
289  for (const auto& connection : dependency_graph_) {
290  const PortIdentifier& src = connection.second;
291  const PortIdentifier& dest = connection.first;
292  context->Connect(ConvertToContextPortIdentifier(src),
293  ConvertToContextPortIdentifier(dest));
294  }
295 
296  // Declare the Diagram-external inputs.
297  for (const PortIdentifier& id : input_port_ids_) {
298  context->ExportInput(ConvertToContextPortIdentifier(id));
299  }
300 
301  context->MakeState();
302  context->MakeParameters();
303  return std::move(context);
304  }
305 
306  void SetDefaultState(const Context<T>& context,
307  State<T>* state) const override {
308  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
309  DRAKE_DEMAND(diagram_context != nullptr);
310 
311  auto diagram_state = dynamic_cast<DiagramState<T>*>(state);
312  DRAKE_DEMAND(diagram_state != nullptr);
313 
314  // Set default state of each constituent system.
315  for (int i = 0; i < num_subsystems(); ++i) {
316  auto& subcontext = diagram_context->GetSubsystemContext(i);
317  auto& substate = diagram_state->get_mutable_substate(i);
318  registered_systems_[i]->SetDefaultState(subcontext, &substate);
319  }
320  }
321 
322  void SetDefaults(Context<T>* context) const final {
323  auto diagram_context = dynamic_cast<DiagramContext<T>*>(context);
324  DRAKE_DEMAND(diagram_context != nullptr);
325 
326  // Set defaults of each constituent system.
327  for (int i = 0; i < num_subsystems(); ++i) {
328  auto& subcontext = diagram_context->GetMutableSubsystemContext(i);
329  registered_systems_[i]->SetDefaults(&subcontext);
330  }
331  }
332 
333  std::unique_ptr<SystemOutput<T>> AllocateOutput(
334  const Context<T>& context) const override {
335  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
336  DRAKE_DEMAND(diagram_context != nullptr);
337 
338  // The output ports of this Diagram are output ports of its constituent
339  // systems. Create a DiagramOutput with that many ports.
340  auto output = std::make_unique<internal::DiagramOutput<T>>();
341  output->get_mutable_port_values()->resize(output_port_ids_.size());
342  ExposeSubsystemOutputs(*diagram_context, output.get());
343  return std::move(output);
344  }
345 
347  // The three methods below are hidden from doxygen, as described in
348  // documentation for their corresponding methods in System.
349  std::unique_ptr<EventCollection<PublishEvent<T>>>
350  AllocateForcedPublishEventCollection() const final {
351  return AllocateForcedEventCollection<PublishEvent<T>>(
353  }
354 
355  std::unique_ptr<EventCollection<DiscreteUpdateEvent<T>>>
356  AllocateForcedDiscreteUpdateEventCollection() const final {
357  return AllocateForcedEventCollection<DiscreteUpdateEvent<T>>(
359  }
360 
361  std::unique_ptr<EventCollection<UnrestrictedUpdateEvent<T>>>
362  AllocateForcedUnrestrictedUpdateEventCollection() const final {
363  return AllocateForcedEventCollection<UnrestrictedUpdateEvent<T>>(
365  }
367 
370  std::unique_ptr<ContinuousState<T>> AllocateTimeDerivatives() const override {
371  std::vector<std::unique_ptr<ContinuousState<T>>> sub_derivatives;
372  for (const auto& system : registered_systems_) {
373  sub_derivatives.push_back(system->AllocateTimeDerivatives());
374  }
375  return std::unique_ptr<ContinuousState<T>>(
376  new internal::DiagramTimeDerivatives<T>(std::move(sub_derivatives)));
377  }
378 
381  std::unique_ptr<DiscreteValues<T>> AllocateDiscreteVariables()
382  const override {
383  std::vector<std::unique_ptr<DiscreteValues<T>>> sub_discretes;
384  for (const auto& system : registered_systems_) {
385  sub_discretes.push_back(system->AllocateDiscreteVariables());
386  }
387  return std::unique_ptr<DiscreteValues<T>>(
388  new internal::DiagramDiscreteVariables<T>(std::move(sub_discretes)));
389  }
390 
391  void DoCalcTimeDerivatives(const Context<T>& context,
392  ContinuousState<T>* derivatives) const override {
393  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
394  DRAKE_DEMAND(diagram_context != nullptr);
395 
396  auto diagram_derivatives =
397  dynamic_cast<DiagramContinuousState<T>*>(derivatives);
398  DRAKE_DEMAND(diagram_derivatives != nullptr);
399  const int n = diagram_derivatives->get_num_substates();
400  DRAKE_DEMAND(num_subsystems() == n);
401 
402  // Evaluate the derivatives of each constituent system.
403  for (int i = 0; i < n; ++i) {
404  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
405  ContinuousState<T>* subderivatives =
406  diagram_derivatives->get_mutable_substate(i);
407  registered_systems_[i]->CalcTimeDerivatives(subcontext, subderivatives);
408  }
409  }
410 
416  const ContinuousState<T>& derivatives, const System<T>* subsystem) const {
417  DRAKE_DEMAND(subsystem != nullptr);
418  auto diagram_derivatives =
419  dynamic_cast<const DiagramContinuousState<T>*>(&derivatives);
420  DRAKE_DEMAND(diagram_derivatives != nullptr);
421  const int i = GetSystemIndexOrAbort(subsystem);
422  return diagram_derivatives->get_substate(i);
423  }
424 
430  const Context<T>& GetSubsystemContext(const System<T>& subsystem,
431  const Context<T>& context) const {
432  auto ret = DoGetTargetSystemContext(subsystem, &context);
433  DRAKE_DEMAND(ret != nullptr);
434  return *ret;
435  }
436 
442  Context<T>* context) const {
443  auto ret = DoGetMutableTargetSystemContext(subsystem, context);
444  DRAKE_DEMAND(ret != nullptr);
445  return *ret;
446  }
447 
453  const CompositeEventCollection<T>& events) const {
454  auto ret = DoGetTargetSystemCompositeEventCollection(subsystem, &events);
455  DRAKE_DEMAND(ret != nullptr);
456  return *ret;
457  }
458 
463  const System<T>& subsystem, CompositeEventCollection<T>* events) const {
464  auto ret = DoGetMutableTargetSystemCompositeEventCollection(
465  subsystem, events);
466  DRAKE_DEMAND(ret != nullptr);
467  return *ret;
468  }
469 
478  Context<T>* context) const {
479  Context<T>& subcontext = GetMutableSubsystemContext(subsystem, context);
480  return *subcontext.get_mutable_state();
481  }
482 
487  State<T>* state) const {
488  auto ret = DoGetMutableTargetSystemState(subsystem, state);
489  DRAKE_DEMAND(ret != nullptr);
490  return *ret;
491  }
492 
496  const State<T>& GetSubsystemState(const System<T>& subsystem,
497  const State<T>& state) const {
498  auto ret = DoGetTargetSystemState(subsystem, &state);
499  DRAKE_DEMAND(ret != nullptr);
500  return *ret;
501  }
502 
506  void GetPath(std::stringstream* output) const override {
507  return System<T>::GetPath(output);
508  }
509 
510  //----------------------------------------------------------------------------
512 
513 
516  void GetGraphvizFragment(std::stringstream* dot) const override {
517  // Open the Diagram.
518  const int64_t id = this->GetGraphvizId();
519  *dot << "subgraph cluster" << id << "diagram" " {" << std::endl;
520  *dot << "color=black" << std::endl;
521  *dot << "concentrate=true" << std::endl;
522  std::string name = this->get_name();
523  if (name.empty()) name = std::to_string(id);
524  *dot << "label=\"" << name << "\";" << std::endl;
525 
526  // Add a cluster for the input port nodes.
527  *dot << "subgraph cluster" << id << "inputports" << " {" << std::endl;
528  *dot << "rank=same" << std::endl;
529  *dot << "color=lightgrey" << std::endl;
530  *dot << "style=filled" << std::endl;
531  *dot << "label=\"input ports\"" << std::endl;
532  for (int i = 0; i < this->get_num_input_ports(); ++i) {
533  this->GetGraphvizInputPortToken(this->get_input_port(i), dot);
534  *dot << "[color=blue, label=\"u" << i << "\"];" << std::endl;
535  }
536  *dot << "}" << std::endl;
537 
538  // Add a cluster for the output port nodes.
539  *dot << "subgraph cluster" << id << "outputports" << " {" << std::endl;
540  *dot << "rank=same" << std::endl;
541  *dot << "color=lightgrey" << std::endl;
542  *dot << "style=filled" << std::endl;
543  *dot << "label=\"output ports\"" << std::endl;
544  for (int i = 0; i < this->get_num_output_ports(); ++i) {
545  this->GetGraphvizOutputPortToken(this->get_output_port(i), dot);
546  *dot << "[color=green, label=\"y" << i << "\"];" << std::endl;
547  }
548  *dot << "}" << std::endl;
549 
550  // Add a cluster for the subsystems.
551  *dot << "subgraph cluster" << id << "subsystems" << " {" << std::endl;
552  *dot << "color=white" << std::endl;
553  *dot << "label=\"\"" << std::endl;
554  // -- Add the subsystems themselves.
555  for (const auto& subsystem : registered_systems_) {
556  subsystem->GetGraphvizFragment(dot);
557  }
558  // -- Add the connections as edges.
559  for (const auto& edge : dependency_graph_) {
560  const PortIdentifier& src = edge.second;
561  const System<T>* src_sys = src.first;
562  const PortIdentifier& dest = edge.first;
563  const System<T>* dest_sys = dest.first;
564  src_sys->GetGraphvizOutputPortToken(src_sys->get_output_port(src.second),
565  dot);
566  *dot << " -> ";
567  dest_sys->GetGraphvizInputPortToken(dest_sys->get_input_port(dest.second),
568  dot);
569  *dot << ";" << std::endl;
570  }
571 
572  // -- Add edges from the input and output port nodes to the subsystems that
573  // actually service that port. These edges are higlighted in blue
574  // (input) and green (output), matching the port nodes.
575  for (int i = 0; i < this->get_num_input_ports(); ++i) {
576  const auto& port_id = input_port_ids_[i];
577  this->GetGraphvizInputPortToken(this->get_input_port(i), dot);
578  *dot << " -> ";
579  port_id.first->GetGraphvizInputPortToken(
580  port_id.first->get_input_port(port_id.second), dot);
581  *dot << " [color=blue];" << std::endl;
582  }
583 
584  for (int i = 0; i < this->get_num_output_ports(); ++i) {
585  const auto& port_id = output_port_ids_[i];
586  port_id.first->GetGraphvizOutputPortToken(
587  port_id.first->get_output_port(port_id.second), dot);
588  *dot << " -> ";
589  this->GetGraphvizOutputPortToken(this->get_output_port(i), dot);
590  *dot << " [color=green];" << std::endl;
591  }
592  *dot << "}" << std::endl;
593 
594  // Close the diagram.
595  *dot << "}" << std::endl;
596  }
597 
599  std::stringstream* dot) const override {
600  DRAKE_DEMAND(port.get_system() == this);
601  *dot << "_" << this->GetGraphvizId() << "_u" << port.get_index();
602  }
603 
605  std::stringstream* dot) const override {
606  DRAKE_DEMAND(&port.get_system() == this);
607  *dot << "_" << this->GetGraphvizId() << "_y" << port.get_index();
608  }
609 
611 
618  const Context<T>* context,
619  const InputPortDescriptor<T>& descriptor) const override {
620  DRAKE_DEMAND(context != nullptr);
621  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(context);
622  DRAKE_DEMAND(diagram_context != nullptr);
623  const PortIdentifier id{descriptor.get_system(), descriptor.get_index()};
624 
625  // Find if this input port is exported.
626  const auto external_it =
627  std::find(input_port_ids_.begin(), input_port_ids_.end(), id);
628  const bool is_exported = (external_it != input_port_ids_.end());
629 
630  // Find if this input port is connected to an output port.
631  const auto upstream_it = dependency_graph_.find(id);
632  const bool is_connected = (upstream_it != dependency_graph_.end());
633 
634  DRAKE_DEMAND(is_exported ^ is_connected);
635 
636  if (is_exported) {
637  // The upstream output port is an input of this whole Diagram; ask our
638  // parent to evaluate it.
639  const int i = external_it - input_port_ids_.begin();
640  this->EvalInputPort(*diagram_context, i);
641  } else {
642  // The upstream output port exists in this Diagram; evaluate it.
643  // TODO(david-german-tri): Add online algebraic loop detection here.
644  DRAKE_ASSERT(is_connected);
645  const PortIdentifier& prerequisite = upstream_it->second;
646  this->EvaluateOutputPort(*diagram_context, prerequisite);
647  }
648  }
649 
652  int GetSystemIndexOrAbort(const System<T>* sys) const {
653  auto it = system_index_map_.find(sys);
654  DRAKE_DEMAND(it != system_index_map_.end());
655  return it->second;
656  }
657 
658  protected:
661  Diagram() {}
662 
667  T DoEvaluateWitness(const Context<T>& context,
668  const WitnessFunction<T>& witness_func) const final {
669  const System<T>& system = witness_func.get_system();
670  const Context<T>& subcontext = GetSubsystemContext(system, context);
671  return witness_func.Evaluate(subcontext);
672  }
673 
679  const WitnessFunction<T>& witness_func,
680  CompositeEventCollection<T>* events) const final {
681  DRAKE_DEMAND(events);
682  const System<T>& subsystem = witness_func.get_system();
683  CompositeEventCollection<T>& subevents =
684  GetMutableSubsystemCompositeEventCollection(subsystem, events);
685  witness_func.AddEvent(&subevents);
686  }
687 
691  void DoGetWitnessFunctions(const Context<T>& context,
692  std::vector<const WitnessFunction<T>*>* witnesses) const final {
693  // A temporary vector is necessary since the vector of witnesses is
694  // declared to be empty on entry to DoGetWitnessFunctions().
695  std::vector<const WitnessFunction<T>*> temp_witnesses;
696 
697  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
698  DRAKE_DEMAND(diagram_context != nullptr);
699 
700  int index = 0; // The subsystem index.
701 
702  for (const auto& system : registered_systems_) {
703  DRAKE_ASSERT(index == GetSystemIndexOrAbort(system.get()));
704  temp_witnesses.clear();
705  system->GetWitnessFunctions(diagram_context->GetSubsystemContext(index),
706  &temp_witnesses);
707  witnesses->insert(witnesses->end(), temp_witnesses.begin(),
708  temp_witnesses.end());
709  ++index;
710  }
711  }
712 
716  const System<T>& target_system, Context<T>* context) const final {
717  if (&target_system == this)
718  return context;
719 
720  return GetSubsystemStuff<Context<T>, DiagramContext<T>>(
721  target_system, context,
724  }
725 
729  const System<T>& target_system, const Context<T>* context) const final {
730  if (&target_system == this)
731  return context;
732 
733  return GetSubsystemStuff<const Context<T>, const DiagramContext<T>>(
734  target_system, context,
737  }
738 
742  const System<T>& target_system, State<T>* state) const final {
743  if (&target_system == this)
744  return state;
745 
746  return GetSubsystemStuff<State<T>, DiagramState<T>>(
747  target_system, state,
750  }
751 
755  const System<T>& target_system, const State<T>* state) const final {
756  if (&target_system == this)
757  return state;
758 
759  return GetSubsystemStuff<const State<T>, const DiagramState<T>>(
760  target_system, state,
763  }
764 
768  const System<T>& target_system,
769  CompositeEventCollection<T>* events) const final {
770  if (&target_system == this)
771  return events;
772 
773  return GetSubsystemStuff<CompositeEventCollection<T>,
775  target_system, events,
778  }
779 
783  const System<T>& target_system,
784  const CompositeEventCollection<T>* events) const final {
785  if (&target_system == this)
786  return events;
787 
788  return GetSubsystemStuff<const CompositeEventCollection<T>,
790  target_system, events,
793  }
794 
799  const Context<T>& context,
800  const Eigen::Ref<const VectorX<T>>& generalized_velocity,
801  VectorBase<T>* qdot) const override {
802  // Check that the dimensions of the continuous state in the context match
803  // the dimensions of the provided generalized velocity and configuration
804  // derivatives.
805  const ContinuousState<T>* xc = context.get_continuous_state();
806  DRAKE_DEMAND(xc != nullptr);
807  const int nq = xc->get_generalized_position().size();
808  const int nv = xc->get_generalized_velocity().size();
809  DRAKE_DEMAND(nq == qdot->size());
810  DRAKE_DEMAND(nv == generalized_velocity.size());
811 
812  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
813  DRAKE_DEMAND(diagram_context != nullptr);
814 
815  // Iterate over the subsystems, asking each subsystem to map its subslice of
816  // velocity to configuration derivatives. This approach is valid because the
817  // DiagramContinuousState guarantees that the subsystem states are
818  // concatenated in order.
819  int v_index = 0; // The next index to read in generalized_velocity.
820  int q_index = 0; // The next index to write in qdot.
821  for (int i = 0; i < num_subsystems(); ++i) {
822  // Find the continuous state of subsystem i.
823  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
824  const ContinuousState<T>* sub_xc = subcontext.get_continuous_state();
825  // If subsystem i is stateless, skip it.
826  if (sub_xc == nullptr) continue;
827 
828  // Select the chunk of generalized_velocity belonging to subsystem i.
829  const int num_v = sub_xc->get_generalized_velocity().size();
830  const Eigen::Ref<const VectorX<T>>& v_slice =
831  generalized_velocity.segment(v_index, num_v);
832 
833  // Select the chunk of qdot belonging to subsystem i.
834  const int num_q = sub_xc->get_generalized_position().size();
835  Subvector<T> dq_slice(qdot, q_index, num_q);
836 
837  // Delegate the actual mapping to subsystem i itself.
838  registered_systems_[i]->MapVelocityToQDot(subcontext, v_slice, &dq_slice);
839 
840  // Advance the indices.
841  v_index += num_v;
842  q_index += num_q;
843  }
844  }
845 
849  void DoMapQDotToVelocity(const Context<T>& context,
850  const Eigen::Ref<const VectorX<T>>& qdot,
851  VectorBase<T>* generalized_velocity) const override {
852  // Check that the dimensions of the continuous state in the context match
853  // the dimensions of the provided generalized velocity and configuration
854  // derivatives.
855  const ContinuousState<T>* xc = context.get_continuous_state();
856  DRAKE_DEMAND(xc != nullptr);
857  const int nq = xc->get_generalized_position().size();
858  const int nv = xc->get_generalized_velocity().size();
859  DRAKE_DEMAND(nq == qdot.size());
860  DRAKE_DEMAND(nv == generalized_velocity->size());
861 
862  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
863  DRAKE_DEMAND(diagram_context != nullptr);
864 
865  // Iterate over the subsystems, asking each subsystem to map its subslice of
866  // configuration derivatives to velocity. This approach is valid because the
867  // DiagramContinuousState guarantees that the subsystem states are
868  // concatenated in order.
869  int q_index = 0; // The next index to read in qdot.
870  int v_index = 0; // The next index to write in generalized_velocity.
871  for (int i = 0; i < num_subsystems(); ++i) {
872  // Find the continuous state of subsystem i.
873  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
874  const ContinuousState<T>* sub_xc = subcontext.get_continuous_state();
875  // If subsystem i is stateless, skip it.
876  if (sub_xc == nullptr) continue;
877 
878  // Select the chunk of qdot belonging to subsystem i.
879  const int num_q = sub_xc->get_generalized_position().size();
880  const Eigen::Ref<const VectorX<T>>& dq_slice =
881  qdot.segment(q_index, num_q);
882 
883  // Select the chunk of generalized_velocity belonging to subsystem i.
884  const int num_v = sub_xc->get_generalized_velocity().size();
885  Subvector<T> v_slice(generalized_velocity, v_index, num_v);
886 
887  // Delegate the actual mapping to subsystem i itself.
888  registered_systems_[i]->MapQDotToVelocity(subcontext, dq_slice, &v_slice);
889 
890  // Advance the indices.
891  v_index += num_v;
892  q_index += num_q;
893  }
894  }
895 
899  void DoCalcNextUpdateTime(const Context<T>& context,
900  CompositeEventCollection<T>* event_info,
901  T* time) const override {
902  DoCalcNextUpdateTimeImpl(context, event_info, time);
903  }
904 
911  using FromType = System<double>;
912  using ToType = std::unique_ptr<System<AutoDiffXd>>;
913  std::function<ToType(const FromType&)> subsystem_converter{
914  [](const FromType& subsystem) { return subsystem.ToAutoDiffXd(); }};
915  return ConvertScalarType<AutoDiffXd>(subsystem_converter).release();
916  }
917 
924  using FromType = System<double>;
925  using ToType = std::unique_ptr<System<symbolic::Expression>>;
926  std::function<ToType(const FromType&)> subsystem_converter{
927  [](const FromType& subsystem) { return subsystem.ToSymbolic(); }};
928  return ConvertScalarType<symbolic::Expression>(subsystem_converter)
929  .release();
930  }
931 
933  const InputPortDescriptor<T>& descriptor) const override {
934  // Ask the subsystem to perform the allocation.
935  const PortIdentifier& id = input_port_ids_[descriptor.get_index()];
936  const System<T>* subsystem = id.first;
937  const int subindex = id.second;
938  return subsystem->AllocateInputVector(subsystem->get_input_port(subindex))
939  .release();
940  }
941 
943  const InputPortDescriptor<T>& descriptor) const override {
944  // Ask the subsystem to perform the allocation.
945  const PortIdentifier& id = input_port_ids_[descriptor.get_index()];
946  const System<T>* subsystem = id.first;
947  const int subindex = id.second;
948  return subsystem->AllocateInputAbstract(subsystem->get_input_port(subindex))
949  .release();
950  }
951 
952  private:
953  // Returns true if there might be direct feedthrough from the given
954  // @p input_port of the Diagram to the given @p output_port of the Diagram.
955  bool DoHasDirectFeedthrough(int input_port, int output_port) const {
956  DRAKE_ASSERT(input_port >= 0);
957  DRAKE_ASSERT(input_port < this->get_num_input_ports());
958  DRAKE_ASSERT(output_port >= 0);
959  DRAKE_ASSERT(output_port < this->get_num_output_ports());
960 
961  const PortIdentifier& target_input_id = input_port_ids_[input_port];
962 
963  // Search the graph for a direct-feedthrough connection from the output_port
964  // back to the input_port. Maintain a set of the output port identifiers
965  // that are known to have a direct-feedthrough path to the output_port.
966  std::set<PortIdentifier> active_set;
967  active_set.insert(output_port_ids_[output_port]);
968  while (!active_set.empty()) {
969  const PortIdentifier current_output_id = *active_set.begin();
970  size_t removed_count = active_set.erase(current_output_id);
971  DRAKE_ASSERT(removed_count == 1);
972  const System<T>* sys = current_output_id.first;
973  for (int i = 0; i < sys->get_num_input_ports(); ++i) {
974  if (sys->HasDirectFeedthrough(i, current_output_id.second)) {
975  const PortIdentifier curr_input_id(sys, i);
976  if (curr_input_id == target_input_id) {
977  // We've found a direct-feedthrough path to the input_port.
978  return true;
979  } else {
980  // We've found an intermediate input port has a direct-feedthrough
981  // path to the output_port. Add the upstream output port (if there
982  // is one) to the active set.
983  auto it = dependency_graph_.find(curr_input_id);
984  if (it != dependency_graph_.end()) {
985  const PortIdentifier& upstream_output = it->second;
986  active_set.insert(upstream_output);
987  }
988  }
989  }
990  }
991  }
992  // If there are no intermediate output ports with a direct-feedthrough path
993  // to the output_port, there is no direct feedthrough to it from the
994  // input_port.
995  return false;
996  }
997 
998  template <typename EventType>
999  std::unique_ptr<EventCollection<EventType>> AllocateForcedEventCollection(
1000  std::function<
1001  std::unique_ptr<EventCollection<EventType>>(const System<T>*)>
1002  allocater_func) const {
1003  const int num_systems = num_subsystems();
1004  auto ret = std::make_unique<DiagramEventCollection<EventType>>(num_systems);
1005  for (int i = 0; i < num_systems; ++i) {
1006  std::unique_ptr<EventCollection<EventType>> subevent_collection =
1007  allocater_func(registered_systems_[i].get());
1008  ret->set_and_own_subevent_collection(i, std::move(subevent_collection));
1009  }
1010  return std::move(ret);
1011  }
1012 
1013  // For each subsystem, if there is a publish event in its corresponding
1014  // subevent collection, calls its Publish method with the appropriate
1015  // subcontext and subevent collection.
1016  void DispatchPublishHandler(
1017  const Context<T>& context,
1018  const EventCollection<PublishEvent<T>>& event_info) const final {
1019  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1020  DRAKE_DEMAND(diagram_context);
1022  dynamic_cast<const DiagramEventCollection<PublishEvent<T>>&>(
1023  event_info);
1024 
1025  for (int i = 0; i < num_subsystems(); ++i) {
1026  const EventCollection<PublishEvent<T>>& subinfo =
1027  info.get_subevent_collection(i);
1028 
1029  if (subinfo.HasEvents()) {
1030  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1031  registered_systems_[i]->Publish(subcontext, subinfo);
1032  }
1033  }
1034  }
1035 
1036  // For each subsystem, if there is a discrete update event in its
1037  // corresponding subevent collection, calls its CalcDiscreteVariableUpdates
1038  // method with the appropriate subcontext, subevent collection and
1039  // substate.
1040  void DispatchDiscreteVariableUpdateHandler(
1041  const Context<T>& context,
1042  const EventCollection<DiscreteUpdateEvent<T>>& event_info,
1043  DiscreteValues<T>* discrete_state) const final {
1044  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1045  DRAKE_DEMAND(diagram_context);
1046  auto diagram_discrete =
1047  dynamic_cast<internal::DiagramDiscreteVariables<T>*>(discrete_state);
1048  DRAKE_DEMAND(diagram_discrete);
1049 
1050  // As a baseline, initialize all the discrete variables to their
1051  // current values.
1052  // TODO(siyuan): should have a API level CopyFrom for DiscreteValues.
1053  for (int i = 0; i < diagram_discrete->num_groups(); ++i) {
1054  diagram_discrete->get_mutable_vector(i)->set_value(
1055  context.get_discrete_state(i)->get_value());
1056  }
1057 
1059  dynamic_cast<const DiagramEventCollection<DiscreteUpdateEvent<T>>&>(
1060  event_info);
1061 
1062  for (int i = 0; i < num_subsystems(); ++i) {
1063  const EventCollection<DiscreteUpdateEvent<T>>& subinfo =
1064  info.get_subevent_collection(i);
1065 
1066  if (subinfo.HasEvents()) {
1067  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1068  DiscreteValues<T>* subdiscrete =
1069  diagram_discrete->get_mutable_subdiscrete(i);
1070  DRAKE_DEMAND(subdiscrete != nullptr);
1071 
1072  registered_systems_[i]->CalcDiscreteVariableUpdates(subcontext, subinfo,
1073  subdiscrete);
1074  }
1075  }
1076  }
1077 
1078  // For each subsystem, if there is an unrestricted update event in its
1079  // corresponding subevent collection, calls its CalcUnrestrictedUpdate
1080  // method with the appropriate subcontext, subevent collection and substate.
1081  void DispatchUnrestrictedUpdateHandler(
1082  const Context<T>& context,
1083  const EventCollection<UnrestrictedUpdateEvent<T>>& event_info,
1084  State<T>* state) const final {
1085  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1086  DRAKE_DEMAND(diagram_context);
1087  auto diagram_state = dynamic_cast<DiagramState<T>*>(state);
1088  DRAKE_DEMAND(diagram_state != nullptr);
1089 
1090  // No need to set state to context's state, since it has already been done
1091  // in System::CalcUnrestrictedUpdate().
1092 
1095  event_info);
1096 
1097  for (int i = 0; i < num_subsystems(); ++i) {
1099  info.get_subevent_collection(i);
1100 
1101  if (subinfo.HasEvents()) {
1102  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1103  State<T>& substate = diagram_state->get_mutable_substate(i);
1104 
1105  registered_systems_[i]->CalcUnrestrictedUpdate(subcontext, subinfo,
1106  &substate);
1107  }
1108  }
1109  }
1110 
1131  template <typename BaseStuff, typename DerivedStuff>
1132  BaseStuff* GetSubsystemStuff(
1133  const System<T>& target_system, BaseStuff* my_stuff,
1134  std::function<BaseStuff* (const System<T>*, const System<T>&, BaseStuff*)>
1135  recursive_getter,
1136  std::function<BaseStuff& (DerivedStuff*, int)> get_child_stuff) const {
1137  static_assert(
1138  std::is_same<BaseStuff,
1139  typename std::remove_pointer<BaseStuff>::type>::value,
1140  "BaseStuff cannot be a pointer");
1141  static_assert(
1142  std::is_same<DerivedStuff,
1143  typename std::remove_pointer<DerivedStuff>::type>::value,
1144  "DerivedStuff cannot be a pointer");
1145 
1146  DRAKE_DEMAND(my_stuff != nullptr);
1147  DRAKE_DEMAND(&target_system != this);
1148  DerivedStuff& my_stuff_as_derived = dynamic_cast<DerivedStuff&>(*my_stuff);
1149 
1150  int index = 0;
1151  for (const auto& child : registered_systems_) {
1152  BaseStuff& child_stuff =
1153  get_child_stuff(&my_stuff_as_derived, index);
1154 
1155  BaseStuff* const target_stuff =
1156  recursive_getter(child.get(), target_system, &child_stuff);
1157 
1158  if (target_stuff != nullptr) {
1159  return target_stuff;
1160  }
1161  index++;
1162  }
1163 
1164  return nullptr;
1165  }
1166 
1173  template <typename NewType, typename T1 = T>
1174  std::unique_ptr<Diagram<NewType>> ConvertScalarType(
1175  std::function<std::unique_ptr<System<NewType>>(
1176  const System<
1177  std::enable_if_t<std::is_same<T1, double>::value, double>>&)>
1178  converter) const {
1179  std::vector<std::unique_ptr<System<NewType>>> new_systems;
1180  // Recursively convert all the subsystems.
1181  std::map<const System<T1>*, const System<NewType>*> old_to_new_map;
1182  for (const auto& old_system : registered_systems_) {
1183  new_systems.push_back(converter(*old_system));
1184  old_to_new_map[old_system.get()] = new_systems.back().get();
1185  }
1186 
1187  // Set up the blueprint.
1188  auto blueprint = std::make_unique<typename Diagram<NewType>::Blueprint>();
1189  // Make all the inputs and outputs.
1190  for (const PortIdentifier& id : input_port_ids_) {
1191  const System<NewType>* new_system = old_to_new_map[id.first];
1192  const int port = id.second;
1193  blueprint->input_port_ids.emplace_back(new_system, port);
1194  }
1195  for (const PortIdentifier& id : output_port_ids_) {
1196  const System<NewType>* new_system = old_to_new_map[id.first];
1197  const int port = id.second;
1198  blueprint->output_port_ids.emplace_back(new_system, port);
1199  }
1200  // Make all the connections.
1201  for (const auto& edge : dependency_graph_) {
1202  const PortIdentifier& old_dest = edge.first;
1203  const System<NewType>* const dest_system = old_to_new_map[old_dest.first];
1204  const int dest_port = old_dest.second;
1205  const typename Diagram<NewType>::PortIdentifier new_dest{dest_system,
1206  dest_port};
1207 
1208  const PortIdentifier& old_src = edge.second;
1209  const System<NewType>* const src_system = old_to_new_map[old_src.first];
1210  const int src_port = old_src.second;
1211  const typename Diagram<NewType>::PortIdentifier new_src{src_system,
1212  src_port};
1213 
1214  blueprint->dependency_graph[new_dest] = new_src;
1215  }
1216  // Move the new systems into the blueprint.
1217  blueprint->systems = std::move(new_systems);
1218 
1219  // Construct a new Diagram of type NewType from the blueprint.
1220  std::unique_ptr<Diagram<NewType>> new_diagram(
1221  new Diagram<NewType>(std::move(blueprint)));
1222  return std::move(new_diagram);
1223  }
1224 
1230  template <typename NewType, typename T1 = T>
1231  std::unique_ptr<Diagram<NewType>> ConvertScalarType(
1232  std::function<std::unique_ptr<System<NewType>>(
1233  const System<std::enable_if_t<!std::is_same<T1, double>::value,
1234  double>>&)>) const {
1236  "Scalar type conversion is only supported from Diagram<double>.");
1237  }
1238 
1239  // Aborts for scalar types that are not numeric, since there is no reasonable
1240  // definition of "next update time" outside of the real line.
1241  //
1242  // @tparam T1 SFINAE boilerplate for the scalar type. Do not set.
1243  template <typename T1 = T>
1245  DoCalcNextUpdateTimeImpl(const Context<T1>&, CompositeEventCollection<T1>*,
1246  T1*) const {
1248  "The default implementation of Diagram<T>::DoCalcNextUpdateTime "
1249  "only works with types that are drake::is_numeric.");
1250  }
1251 
1252  // Computes the next update time across all the scheduled events, for
1253  // scalar types that are numeric.
1254  //
1255  // @tparam T1 SFINAE boilerplate for the scalar type. Do not set.
1256  template <typename T1 = T>
1257  typename std::enable_if<is_numeric<T1>::value>::type DoCalcNextUpdateTimeImpl(
1258  const Context<T1>& context, CompositeEventCollection<T1>* event_info,
1259  T1* time) const {
1260  auto diagram_context = dynamic_cast<const DiagramContext<T1>*>(&context);
1261  auto info = dynamic_cast<DiagramCompositeEventCollection<T1>*>(event_info);
1262  DRAKE_DEMAND(diagram_context != nullptr);
1263  DRAKE_DEMAND(info != nullptr);
1264 
1265  *time = std::numeric_limits<T1>::infinity();
1266 
1267  // Iterate over the subsystems, and harvest the most imminent updates.
1268  std::vector<T1> times(num_subsystems());
1269  for (int i = 0; i < num_subsystems(); ++i) {
1270  const Context<T1>& subcontext = diagram_context->GetSubsystemContext(i);
1271  CompositeEventCollection<T1>& subinfo =
1272  info->get_mutable_subevent_collection(i);
1273  const T1 sub_time =
1274  registered_systems_[i]->CalcNextUpdateTime(subcontext, &subinfo);
1275  times[i] = sub_time;
1276 
1277  if (sub_time < *time) {
1278  *time = sub_time;
1279  }
1280  }
1281 
1282  // For all the subsystems whose next update time is bigger than *time,
1283  // clear their event collections.
1284  for (int i = 0; i < num_subsystems(); ++i) {
1285  if (times[i] > *time)
1286  info->get_mutable_subevent_collection(i).Clear();
1287  }
1288  }
1289 
1290  void DoGetPerStepEvents(
1291  const Context<T>& context,
1292  CompositeEventCollection<T>* event_info) const override {
1293  auto diagram_context = dynamic_cast<const DiagramContext<T>*>(&context);
1294  auto info = dynamic_cast<DiagramCompositeEventCollection<T>*>(event_info);
1295  DRAKE_DEMAND(diagram_context != nullptr);
1296  DRAKE_DEMAND(info != nullptr);
1297 
1298  for (int i = 0; i < num_subsystems(); ++i) {
1299  const Context<T>& subcontext = diagram_context->GetSubsystemContext(i);
1300  CompositeEventCollection<T>& subinfo =
1301  info->get_mutable_subevent_collection(i);
1302 
1303  registered_systems_[i]->GetPerStepEvents(subcontext, &subinfo);
1304  }
1305  }
1306 
1307  // A structural outline of a Diagram, produced by DiagramBuilder.
1308  struct Blueprint {
1309  // The ordered subsystem ports that are inputs to the entire diagram.
1310  std::vector<PortIdentifier> input_port_ids;
1311  // The ordered subsystem ports that are outputs of the entire diagram.
1312  std::vector<PortIdentifier> output_port_ids;
1313  // A map from the input ports of constituent systems to the output ports
1314  // on which they depend. This graph is possibly cyclic, but must not
1315  // contain an algebraic loop.
1316  std::map<PortIdentifier, PortIdentifier> dependency_graph;
1317  // All of the systems to be included in the diagram.
1318  std::vector<std::unique_ptr<System<T>>> systems;
1319  };
1320 
1321  // Constructs a Diagram from the Blueprint that a DiagramBuilder produces.
1322  // This constructor is private because only DiagramBuilder calls it. The
1323  // constructor takes the systems from the blueprint.
1324  explicit Diagram(std::unique_ptr<Blueprint> blueprint) {
1325  Initialize(std::move(blueprint));
1326  }
1327 
1328  // Validates the given @p blueprint and sets up the Diagram accordingly.
1329  void Initialize(std::unique_ptr<Blueprint> blueprint) {
1330  // We must be given something to own.
1331  DRAKE_DEMAND(!blueprint->systems.empty());
1332  // We must not already own any subsystems.
1333  DRAKE_DEMAND(registered_systems_.empty());
1334 
1335  // Copy the data from the blueprint into private member variables.
1336  dependency_graph_ = std::move(blueprint->dependency_graph);
1337  input_port_ids_ = std::move(blueprint->input_port_ids);
1338  output_port_ids_ = std::move(blueprint->output_port_ids);
1339  registered_systems_ = std::move(blueprint->systems);
1340 
1341  // Generate a map from the System pointer to its index in the registered
1342  // order.
1343  for (int i = 0; i < num_subsystems(); ++i) {
1344  system_index_map_[registered_systems_[i].get()] = i;
1345  registered_systems_[i]->set_parent(this);
1346  }
1347 
1348  // Every system must appear exactly once.
1349  DRAKE_DEMAND(registered_systems_.size() == system_index_map_.size());
1350  // Every port named in the dependency_graph_ must actually exist.
1351  DRAKE_ASSERT(PortsAreValid());
1352  // Every subsystem must have a unique name.
1353  DRAKE_THROW_UNLESS(NamesAreUniqueAndNonEmpty());
1354 
1355  // Add the inputs to the Diagram topology, and check their invariants.
1356  for (const PortIdentifier& id : input_port_ids_) {
1357  ExportInput(id);
1358  }
1359  for (const PortIdentifier& id : output_port_ids_) {
1360  ExportOutput(id);
1361  }
1362 
1363  this->set_forced_publish_events(
1364  AllocateForcedEventCollection<PublishEvent<T>>(
1366  this->set_forced_discrete_update_events(
1367  AllocateForcedEventCollection<DiscreteUpdateEvent<T>>(
1369  this->set_forced_unrestricted_update_events(
1370  AllocateForcedEventCollection<UnrestrictedUpdateEvent<T>>(
1372  }
1373 
1374  // Exposes the given port as an input of the Diagram.
1375  void ExportInput(const PortIdentifier& port) {
1376  const System<T>* const sys = port.first;
1377  const int port_index = port.second;
1378  // Fail quickly if this system is not part of the diagram.
1379  GetSystemIndexOrAbort(sys);
1380 
1381  // Add this port to our externally visible topology.
1382  const auto& subsystem_descriptor = sys->get_input_port(port_index);
1383  this->DeclareInputPort(subsystem_descriptor.get_data_type(),
1384  subsystem_descriptor.size());
1385  }
1386 
1387  // Exposes the given subsystem output port as an output of the Diagram.
1388  void ExportOutput(const PortIdentifier& port) {
1389  const System<T>* const sys = port.first;
1390  const int port_index = port.second;
1391  const auto& source_output_port = sys->get_output_port(port_index);
1392  auto diagram_port = std::make_unique<internal::DiagramOutputPort<T>>(
1393  *this, &source_output_port);
1394  this->CreateOutputPort(std::move(diagram_port));
1395  }
1396 
1397  // Evaluates the value of the output port with the given @p id in the given
1398  // @p context.
1399  //
1400  // TODO(david-german-tri): Add Diagram-level cache entries to keep track of
1401  // whether a given output port has already been evaluated. Right now, we
1402  // are recomputing every intermediate output to satisfy every system that
1403  // depends on it, recursively. This is O(N^2 * M), where M is the number of
1404  // output ports the Diagram exposes, and N is the number of intermediate
1405  // output ports the Diagram contains.
1406  void EvaluateOutputPort(const DiagramContext<T>& context,
1407  const PortIdentifier& id) const {
1408  const System<T>* const system = id.first;
1409  const OutputPortIndex port_index(id.second);
1410  const OutputPort<T>& port = system->get_output_port(port_index);
1411  const int i = GetSystemIndexOrAbort(system);
1412  SPDLOG_TRACE(log(), "Evaluating output for subsystem {}, port {}",
1413  system->GetPath(), port_index);
1414  const Context<T>& subsystem_context = context.GetSubsystemContext(i);
1415  SystemOutput<T>* subsystem_output = context.GetSubsystemOutput(i);
1416  AbstractValue* port_output = subsystem_output->GetMutableData(port_index);
1417  port.Calc(subsystem_context, port_output);
1418  }
1419 
1420  // Converts a PortIdentifier to a DiagramContext::PortIdentifier.
1421  // The DiagramContext::PortIdentifier contains the index of the System in the
1422  // the diagram, instead of an actual pointer to the System.
1423  typename DiagramContext<T>::PortIdentifier ConvertToContextPortIdentifier(
1424  const PortIdentifier& id) const {
1425  typename DiagramContext<T>::PortIdentifier output;
1426  output.first = GetSystemIndexOrAbort(id.first);
1427  output.second = id.second;
1428  return output;
1429  }
1430 
1431  // Sets up the OutputPortValue pointers in @p output to point to the subsystem
1432  // output values, found in @p context, that are the outputs of this Diagram.
1433  void ExposeSubsystemOutputs(const DiagramContext<T>& context,
1434  internal::DiagramOutput<T>* output) const {
1435  // The number of output ports of this diagram must equal the number of
1436  // ports in the provided DiagramOutput.
1437  const int num_ports = static_cast<int>(output_port_ids_.size());
1438  DRAKE_DEMAND(output->get_num_ports() == num_ports);
1439 
1440  for (int i = 0; i < num_ports; ++i) {
1441  const PortIdentifier& id = output_port_ids_[i];
1442  // For each configured output port ID, obtain from the DiagramContext the
1443  // actual OutputPortValue that supplies its value.
1444  const int sys_index = GetSystemIndexOrAbort(id.first);
1445  const int port_index = id.second;
1446  SystemOutput<T>* subsystem_output = context.GetSubsystemOutput(sys_index);
1447  OutputPortValue* output_port_value =
1448  subsystem_output->get_mutable_port_value(port_index);
1449 
1450  // Then, put a pointer to that OutputPortValue in the DiagramOutput.
1451  (*output->get_mutable_port_values())[i] = output_port_value;
1452  }
1453  }
1454 
1455  // Returns true if every port mentioned in the dependency_graph_ exists.
1456  bool PortsAreValid() const {
1457  for (const auto& entry : dependency_graph_) {
1458  const PortIdentifier& dest = entry.first;
1459  const PortIdentifier& src = entry.second;
1460  if (dest.second < 0 || dest.second >= dest.first->get_num_input_ports()) {
1461  return false;
1462  }
1463  if (src.second < 0 || src.second >= src.first->get_num_output_ports()) {
1464  return false;
1465  }
1466  }
1467  return true;
1468  }
1469 
1470  // Returns true if every subsystem has a unique, non-empty name.
1471  // O(N * log(N)) in the number of subsystems.
1472  bool NamesAreUniqueAndNonEmpty() const {
1473  std::set<std::string> names;
1474  for (const auto& system : registered_systems_) {
1475  const std::string& name = system->get_name();
1476  if (name.empty()) {
1477  // This can only happen if someone blanks out the name *after* adding
1478  // it to DiagramBuilder; if an empty name is given to DiagramBuilder,
1479  // a default non-empty name is automatically assigned.
1480  log()->error("Subsystem of type {} has no name",
1481  NiceTypeName::Get(*system));
1482  // We skip names.insert here, so that the return value will be false.
1483  continue;
1484  }
1485  if (names.find(name) != names.end()) {
1486  log()->error("Non-unique name \"{}\" for subsystem of type {}", name,
1487  NiceTypeName::Get(*system));
1488  }
1489  names.insert(name);
1490  }
1491  return names.size() == registered_systems_.size();
1492  }
1493 
1494  int num_subsystems() const {
1495  return static_cast<int>(registered_systems_.size());
1496  }
1497 
1498  // A map from the input ports of constituent systems, to the output ports of
1499  // the systems on which they depend.
1500  std::map<PortIdentifier, PortIdentifier> dependency_graph_;
1501 
1502  // The Systems in this Diagram, which are owned by this Diagram, in the order
1503  // they were registered.
1504  std::vector<std::unique_ptr<System<T>>> registered_systems_;
1505 
1506  // Map to quickly satisify "What is the subsytem index of the child system?"
1507  std::map<const System<T>*, int> system_index_map_;
1508 
1509  // The ordered inputs and outputs of this Diagram.
1510  std::vector<PortIdentifier> input_port_ids_;
1511  std::vector<PortIdentifier> output_port_ids_;
1512 
1513  // For all T, Diagram<T> considers DiagramBuilder<T> a friend, so that the
1514  // builder can set the internal state correctly.
1515  friend class DiagramBuilder<T>;
1516 
1517  // For all T, Diagram<T> considers Diagram<double> a friend, so that
1518  // Diagram<double> can provide transmogrification methods to more flavorful
1519  // scalar types. See Diagram<T>::ConvertScalarType.
1520  friend class Diagram<double>;
1521 };
1522 
1523 } // namespace systems
1524 } // namespace drake
const EventCollection< EventType > & get_subevent_collection(int index) const
Returns a const pointer to subsystem&#39;s EventCollection at index.
Definition: event_collection.h:221
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:715
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:849
virtual int size() const =0
Returns the number of elements in the vector.
The DiscreteValues is a container for numerical but non-continuous state and parameters.
Definition: discrete_values.h:28
std::multimap< int, int > GetDirectFeedthroughs() const final
Reports all direct feedthroughs from input ports to output ports.
Definition: diagram.h:248
virtual std::unique_ptr< Context< T > > AllocateContext() const =0
Allocates a context, initialized with the correct numbers of concrete input ports and state variables...
void GetGraphvizOutputPortToken(const OutputPort< T > &port, std::stringstream *dot) const override
Appends a fragment to the dot stream identifying the graphviz node representing port.
Definition: diagram.h:604
void SetDefaultState(const Context< T > &context, State< T > *state) const override
Assigns default values to all elements of the state.
Definition: diagram.h:306
int num_subdiscretes() const
Definition: diagram.h:191
OutputPortValue contains the value of a single System output port.
Definition: output_port_value.h:23
NOTE: The contents of this class are for the most part direct ports of drake/systems/plants//inverseK...
Definition: automotive_demo.cc:88
std::vector< OutputPortValue * > * get_mutable_port_values()
Definition: diagram.h:136
This file contains traits for number (scalar) types.
const DiscreteValues< T > * get_discrete_state() const
Definition: context.h:133
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:333
const ContinuousState< T > * get_continuous_state() const
Returns a const pointer to the continuous component of the state, which may be of size zero...
Definition: context.h:118
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:141
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:899
Diagram()
Constructs an uninitialized Diagram.
Definition: diagram.h:661
Diagram< AutoDiffXd > * DoToAutoDiffXd() const override
Creates a deep copy of this Diagram<double>, converting the scalar type to AutoDiffXd, and preserving all internal structure.
Definition: diagram.h:910
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:942
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:678
void GetPath(std::stringstream *output) const override
Returns the full path of this Diagram in the tree of Diagrams.
Definition: diagram.h:506
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
int u
Definition: rgbd_camera_test.cc:160
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:691
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
A column vector of any size, templated on scalar type.
Definition: eigen_types.h:43
This class represents an unrestricted update event.
Definition: event.h:326
DiagramContinuousState is a ContinuousState consisting of Supervectors over a set of constituent Cont...
Definition: diagram_continuous_state.h:17
std::unique_ptr< ContinuousState< T > > AllocateTimeDerivatives() const override
Aggregates the time derivatives from each subsystem into a DiagramTimeDerivatives.
Definition: diagram.h:370
#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:53
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:2217
const InputPortDescriptor< T > & get_input_port(int port_index) const
Returns the descriptor of the input port at index port_index.
Definition: system.h:812
std::unique_ptr< DiscreteValues< T > > AllocateDiscreteVariables() const override
Aggregates the discrete update variables from each subsystem into a DiagramDiscreteVariables.
Definition: diagram.h:381
#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:441
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:74
void SetDefaults(Context< T > *context) const final
Definition: diagram.h:322
Holds information about the subsystem output port that has been exported to become one of this Diagra...
Definition: diagram.h:54
std::unique_ptr< System< AutoDiffXd > > ToAutoDiffXd() const
Creates a deep copy of this System, transmogrified to use the autodiff scalar type, with a dynamic-sized vector of partial derivatives.
Definition: system.h:968
#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
DiagramTimeDerivatives is a version of DiagramContinuousState that owns the constituent continuous st...
Definition: diagram.h:158
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:202
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:19
Diagram< symbolic::Expression > * DoToSymbolic() const override
Creates a deep copy of this Diagram<double>, converting the scalar type to symbolic::Expression, and preserving all internal structure.
Definition: diagram.h:923
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:932
DiagramBuilder is a factory class for Diagram.
Definition: diagram.h:32
DiagramOutput is an implementation of SystemOutput that holds unowned OutputPortValue pointers...
Definition: diagram.h:118
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:93
DiagramDiscreteVariables is a version of DiscreteState that owns the constituent discrete states...
Definition: diagram.h:180
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:462
std::vector< Number > result
Definition: ipopt_solver.cc:153
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:496
DiscreteValues< T > * get_mutable_subdiscrete(int index)
Definition: diagram.h:195
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:39
std::vector< const systems::System< T > * > GetSystems() const
Returns the list of contained Systems.
Definition: diagram.h:239
int v
Definition: rgbd_camera_test.cc:161
#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:39
DiagramState is a State, annotated with pointers to all the mutable substates that it spans...
Definition: diagram_context.h:27
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:234
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:391
Provides Drake&#39;s assertion implementation.
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:477
std::unique_ptr< BasicVector< T > > AllocateInputVector(const InputPortDescriptor< T > &descriptor) const
Given a port descriptor, allocates the vector storage.
Definition: system.h:80
#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:47
The DiagramContext is a container for all of the data necessary to uniquely determine the computation...
Definition: diagram_context.h:118
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:652
~DiagramTimeDerivatives() override
Definition: diagram.h:167
const OutputPortValue & get_port_value(int index) const override
Definition: diagram.h:131
const VectorBase< T > & get_generalized_position() const
Returns the subset of the state vector that is generalized position q.
Definition: continuous_state.h:112
Diagram is a System composed of one or more constituent Systems, arranged in a directed graph where t...
Definition: diagram.h:30
A superclass template for systems that receive input, maintain state, and produce output of a given m...
Definition: input_port_descriptor.h:10
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:728
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:179
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
virtual State< T > * get_mutable_state()=0
OutputPortValue * get_mutable_port_value(int index) override
Definition: diagram.h:126
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:741
void GetGraphvizFragment(std::stringstream *dot) const override
Returns a Graphviz fragment describing this Diagram.
Definition: diagram.h:516
DiagramOutputPort(const DiagramOutputPort &)=delete
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:754
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:415
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:63
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:935
std::unique_ptr< CompositeEventCollection< T > > AllocateCompositeEventCollection() const final
Allocates a DiagramEventCollection for this Diagram.
Definition: diagram.h:263
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:782
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:798
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:617
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.
virtual OutputPortValue * get_mutable_port_value(int index)=0
~DiagramDiscreteVariables() override
Definition: diagram.h:189
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:767
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:430
const VectorBase< T > & get_generalized_velocity() const
Returns the subset of the continuous state vector that is generalized velocity v. ...
Definition: continuous_state.h:124
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:667
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:942
This class represents a publish event.
Definition: event.h:194
const System< T > * get_system() const
Definition: input_port_descriptor.h:56
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:30
Provides public header files of Drake&#39;s symbolic library.
InputPortEvaluatorInterface is implemented by classes that are able to evaluate the OutputPortValue c...
Definition: input_port_evaluator_interface.h:27
std::unique_ptr< System< symbolic::Expression > > ToSymbolic() const
Creates a deep copy of this System, transmogrified to use the symbolic scalar type.
Definition: system.h:1018
void GetPath(std::stringstream *output) const
Writes the full path of this System in the tree of Systems to output.
Definition: system.h:785
ContinuousState is a container for all the continuous state variables xc.
Definition: continuous_state.h:44
int get_index() const
Definition: input_port_descriptor.h:57
const OutputPort< T > & get_output_port(int port_index) const
Returns the output port at index port_index.
Definition: system.h:823
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:275
int get_num_ports() const override
Definition: diagram.h:124
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:35
const System< T > & get_system() const
Returns a reference to the System that owns this output port.
Definition: output_port.h:101
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
void GetGraphvizInputPortToken(const InputPortDescriptor< T > &port, std::stringstream *dot) const override
Appends a fragment to the dot stream identifying the graphviz node representing port.
Definition: diagram.h:598
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:452
std::string GetPath() const
Definition: system.h:795
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:802
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:486
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:257
Provides careful macros to selectively enable or disable the special member functions for copy-constr...