Drake
diagram_builder.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <algorithm>
4 #include <map>
5 #include <memory>
6 #include <set>
7 #include <sstream>
8 #include <stdexcept>
9 #include <unordered_set>
10 #include <utility>
11 #include <vector>
12 
13 #include "drake/common/drake_assert.h"
14 #include "drake/common/drake_copyable.h"
15 #include "drake/common/drake_throw.h"
16 #include "drake/systems/framework/diagram.h"
17 #include "drake/systems/framework/system.h"
18 
19 namespace drake {
20 namespace systems {
21 
22 /// DiagramBuilder is a factory class for Diagram. It is single
23 /// use: after calling Build or BuildInto, DiagramBuilder gives up ownership
24 /// of the constituent systems, and should therefore be discarded.
25 ///
26 /// A system must be added to the DiagramBuilder with AddSystem before it can
27 /// be wired up in any way. Every system must have a unique, non-empty name.
28 template <typename T>
29 class DiagramBuilder {
30  public:
31  // DiagramBuilder objects are neither copyable nor moveable.
33 
35  virtual ~DiagramBuilder() {}
36 
37  /// Takes ownership of @p system and adds it to the builder. Returns a bare
38  /// pointer to the System, which will remain valid for the lifetime of the
39  /// Diagram built by this builder.
40  ///
41  /// If the system's name is unset, sets it to System::GetMemoryObjectName()
42  /// as a default in order to have unique names within the diagram.
43  ///
44  /// @code
45  /// DiagramBuilder<T> builder;
46  /// auto foo = builder.AddSystem(std::make_unique<Foo<T>>());
47  /// @endcode
48  ///
49  /// @tparam S The type of system to add.
50  template<class S>
51  S* AddSystem(std::unique_ptr<S> system) {
52  if (system->get_name().empty()) {
53  system->set_name(system->GetMemoryObjectName());
54  }
55  S* raw_sys_ptr = system.get();
56  systems_.insert(raw_sys_ptr);
57  registered_systems_.push_back(std::move(system));
58  return raw_sys_ptr;
59  }
60 
61  /// Constructs a new system with the given @p args, and adds it to the
62  /// builder, which retains ownership. Returns a bare pointer to the System,
63  /// which will remain valid for the lifetime of the Diagram built by this
64  /// builder.
65  ///
66  /// @code
67  /// DiagramBuilder<double> builder;
68  /// auto foo = builder.AddSystem<Foo<double>>("name", 3.14);
69  /// @endcode
70  ///
71  /// Note that for dependent names you must use the template keyword:
72  ///
73  /// @code
74  /// DiagramBuilder<T> builder;
75  /// auto foo = builder.template AddSystem<Foo<T>>("name", 3.14);
76  /// @endcode
77  ///
78  /// You may prefer the `unique_ptr` variant instead.
79  ///
80  ///
81  /// @tparam S The type of System to construct. Must subclass System<T>.
82  template<class S, typename... Args>
83  S* AddSystem(Args&&... args) {
84  return AddSystem(std::make_unique<S>(std::forward<Args>(args)...));
85  }
86 
87  /// Constructs a new system with the given @p args, and adds it to the
88  /// builder, which retains ownership. Returns a bare pointer to the System,
89  /// which will remain valid for the lifetime of the Diagram built by this
90  /// builder.
91  ///
92  /// @code
93  /// DiagramBuilder<double> builder;
94  /// // Foo must be a template.
95  /// auto foo = builder.AddSystem<Foo>("name", 3.14);
96  /// @endcode
97  ///
98  /// Note that for dependent names you must use the template keyword:
99  ///
100  /// @code
101  /// DiagramBuilder<T> builder;
102  /// auto foo = builder.template AddSystem<Foo>("name", 3.14);
103  /// @endcode
104  ///
105  /// You may prefer the `unique_ptr` variant instead.
106  ///
107  /// @tparam S A template for the type of System to construct. The template
108  /// will be specialized on the scalar type T of this builder.
109  template<template<typename Scalar> class S, typename... Args>
110  S<T>* AddSystem(Args&&... args) {
111  return AddSystem(std::make_unique<S<T>>(std::forward<Args>(args)...));
112  }
113 
114  /// Returns whether any Systems have been added yet.
115  bool empty() const { return registered_systems_.empty(); }
116 
117  /// Returns the list of contained Systems.
118  std::vector<systems::System<T>*> GetMutableSystems() {
119  std::vector<systems::System<T>*> result;
120  result.reserve(registered_systems_.size());
121  for (const auto& system : registered_systems_) {
122  result.push_back(system.get());
123  }
124  return result;
125  }
126 
127  /// Declares that input port @p dest is connected to output port @p src.
128  void Connect(const OutputPort<T>& src,
129  const InputPortDescriptor<T>& dest) {
130  DRAKE_DEMAND(src.size() == dest.size());
131  InputPortLocator dest_id{dest.get_system(), dest.get_index()};
132  OutputPortLocator src_id{&src.get_system(), src.get_index()};
133  ThrowIfInputAlreadyWired(dest_id);
134  ThrowIfSystemNotRegistered(&src.get_system());
135  ThrowIfSystemNotRegistered(dest.get_system());
136  connection_map_[dest_id] = src_id;
137  }
138 
139  /// Declares that sole input port on the @p dest system is connected to sole
140  /// output port on the @p src system. Throws an exception if the sole-port
141  /// precondition is not met (i.e., if @p dest has no input ports, or @p dest
142  /// has more than one input port, or @p src has no output ports, or @p src
143  /// has more than one output port).
144  void Connect(const System<T>& src, const System<T>& dest) {
147  Connect(src.get_output_port(0), dest.get_input_port(0));
148  }
149 
150  /// Cascades @p src and @p dest. The sole input port on the @p dest system
151  /// is connected to sole output port on the @p src system. Throws an
152  /// exception if the sole-port precondition is not met (i.e., if @p dest has
153  /// no input ports, or @p dest has more than one input port, or @p src has no
154  /// output ports, or @p src has more than one output port).
155  void Cascade(const System<T>& src, const System<T>& dest) {
156  Connect(src, dest);
157  }
158 
159  /// Declares that the given @p input port of a constituent system is an input
160  /// to the entire Diagram.
161  /// @return The index of the exported input port of the entire diagram.
163  InputPortLocator id{input.get_system(), input.get_index()};
164  ThrowIfInputAlreadyWired(id);
165  ThrowIfSystemNotRegistered(input.get_system());
166  InputPortIndex return_id(input_port_ids_.size());
167  input_port_ids_.push_back(id);
168  diagram_input_set_.insert(id);
169  return return_id;
170  }
171 
172  /// Declares that the given @p output port of a constituent system is an
173  /// output of the entire diagram.
174  /// @return The index of the exported output port of the entire diagram.
176  ThrowIfSystemNotRegistered(&output.get_system());
177  OutputPortIndex return_id(output_port_ids_.size());
178  output_port_ids_.push_back(
179  OutputPortLocator{&output.get_system(), output.get_index()});
180  return return_id;
181  }
182 
183  /// Builds the Diagram that has been described by the calls to Connect,
184  /// ExportInput, and ExportOutput. Throws std::logic_error if the graph is
185  /// not buildable.
186  std::unique_ptr<Diagram<T>> Build() {
187  std::unique_ptr<Diagram<T>> diagram(new Diagram<T>(Compile()));
188  return std::move(diagram);
189  }
190 
191  /// Configures @p target to have the topology that has been described by
192  /// the calls to Connect, ExportInput, and ExportOutput. Throws
193  /// std::logic_error if the graph is not buildable.
194  ///
195  /// Only Diagram subclasses should call this method. The target must not
196  /// already be initialized.
197  void BuildInto(Diagram<T>* target) {
198  target->Initialize(Compile());
199  }
200 
201  private:
202  using InputPortLocator = typename Diagram<T>::InputPortLocator;
203  using OutputPortLocator = typename Diagram<T>::OutputPortLocator;
204 
205  // This generic port identifier is used only for cycle detection below
206  // because the algorithm treats both input & output ports as nodes.
207  using PortIdentifier = std::pair<const System<T>*, int>;
208 
209  // Throws if the given input port (belonging to a child subsystem) has
210  // already been connected to an output port, or exported to be an input
211  // port of the whole diagram.
212  void ThrowIfInputAlreadyWired(const InputPortLocator& id) const {
213  if (connection_map_.find(id) != connection_map_.end() ||
214  diagram_input_set_.find(id) != diagram_input_set_.end()) {
215  throw std::logic_error("Input port is already wired.");
216  }
217  }
218 
219  void ThrowIfSystemNotRegistered(const System<T>* system) const {
220  DRAKE_THROW_UNLESS(systems_.find(system) != systems_.end());
221  }
222 
223  // Helper method to do the algebraic loop test. It recursively performs the
224  // depth-first search on the graph to find cycles.
225  static bool HasCycleRecurse(
226  const PortIdentifier& n,
227  const std::map<PortIdentifier, std::set<PortIdentifier>>& edges,
228  std::set<PortIdentifier>* visited,
229  std::vector<PortIdentifier>* stack) {
230  DRAKE_ASSERT(visited->count(n) == 0);
231  visited->insert(n);
232 
233  auto edge_iter = edges.find(n);
234  if (edge_iter != edges.end()) {
235  DRAKE_ASSERT(std::find(stack->begin(), stack->end(), n) == stack->end());
236  stack->push_back(n);
237  for (const auto& target : edge_iter->second) {
238  if (visited->count(target) == 0 &&
239  HasCycleRecurse(target, edges, visited, stack)) {
240  return true;
241  } else if (std::find(stack->begin(), stack->end(), target) !=
242  stack->end()) {
243  return true;
244  }
245  }
246  stack->pop_back();
247  }
248  return false;
249  }
250 
251  // Evaluates the graph of port dependencies -- including *connections* between
252  // output ports and input ports and direct feedthrough connections between
253  // input ports and output ports. If an algebraic loop is detected, throws
254  // a std::logic_error.
255  void ThrowIfAlgebraicLoopsExist() const {
256  // Each port in the diagram is a node in a graph.
257  // An edge exists from node u to node v if:
258  // 1. output u is connected to input v (via Connect(u, v) method), or
259  // 2. a direct feedthrough from input u to output v is reported.
260  // A depth-first search of the graph should produce a forest of valid trees
261  // if there are no algebraic loops. Otherwise, at least one link moving
262  // *up* the tree will exist.
263 
264  // Build the graph.
265  // Generally, the nodes of the graph would be the set of all defined ports
266  // (input and output) of each subsystem. However, we only need to
267  // consider the input/output ports that have a diagram level output-to-input
268  // connection (ports that are not connected in this manner cannot contribute
269  // to an algebraic loop).
270 
271  // Track *all* of the nodes involved in a diagram-level connection as
272  // described above.
273  std::set<PortIdentifier> nodes;
274  // A map from node u, to the set of edges { (u, v_i) }. In normal cases,
275  // not every node in `nodes` will serve as a key in `edges` (as that is a
276  // necessary condition for there to be no algebraic loop).
277  std::map<PortIdentifier, std::set<PortIdentifier>> edges;
278 
279  // In order to store PortIdentifiers for both input and output ports in the
280  // same set, I need to encode the ports. The identifier for the first input
281  // port and output port look identical (same system pointer, same port
282  // id 0). So, to distinguish them, I'll modify the output ports to use the
283  // negative half of the int space. The function below provides a utility for
284  // encoding an output port id.
285  auto output_to_key = [](int port_id) { return -(port_id + 1); };
286 
287  // Populate the node set from the connections (and define the edges implied
288  // by those connections).
289  for (const auto& connection : connection_map_) {
290  // Dependency graph is a mapping from the destination of the connection
291  // to what it *depends on* (the source).
292  const PortIdentifier& src = connection.second;
293  const PortIdentifier& dest = connection.first;
294  PortIdentifier encoded_src{src.first, output_to_key(src.second)};
295  nodes.insert(encoded_src);
296  nodes.insert(dest);
297  edges[encoded_src].insert(dest);
298  }
299 
300  // Populate more edges based on direct feedthrough.
301  for (const auto& system : registered_systems_) {
302  for (const auto& pair : system->GetDirectFeedthroughs()) {
303  PortIdentifier src_port{system.get(), pair.first};
304  PortIdentifier dest_port{system.get(), output_to_key(pair.second)};
305  if (nodes.count(src_port) > 0 && nodes.count(dest_port) > 0) {
306  // Track direct feedthrough only on port pairs where *both* ports are
307  // connected to other ports at the diagram level.
308  edges[src_port].insert(dest_port);
309  }
310  }
311  }
312 
313  // Evaluate the graph for cycles.
314  std::set<PortIdentifier> visited;
315  std::vector<PortIdentifier> stack;
316  for (const auto& node : nodes) {
317  if (visited.count(node) == 0) {
318  if (HasCycleRecurse(node, edges, &visited, &stack)) {
319  std::stringstream ss;
320 
321  auto port_to_stream = [&ss](const auto& id) {
322  ss << " " << id.first->get_name() << ":";
323  if (id.second < 0)
324  ss << "Out(";
325  else
326  ss << "In(";
327  ss << (id.second >= 0 ? id.second : -id.second - 1) << ")";
328  };
329 
330  ss << "Algebraic loop detected in DiagramBuilder:\n";
331  for (size_t i = 0; i < stack.size() - 1; ++i) {
332  port_to_stream(stack[i]);
333  ss << " depends on\n";
334  }
335  port_to_stream(stack.back());
336 
337  throw std::runtime_error(ss.str());
338  }
339  }
340  }
341  }
342 
343  // TODO(russt): Implement AddRandomSources method to wire up all dangling
344  // random input ports with a compatible RandomSource system.
345 
346  // Produces the Blueprint that has been described by the calls to
347  // Connect, ExportInput, and ExportOutput. Throws std::logic_error if the
348  // graph is empty or contains algebraic loops.
349  // The DiagramBuilder passes ownership of the registered systems to the
350  // blueprint.
351  std::unique_ptr<typename Diagram<T>::Blueprint> Compile() {
352  if (registered_systems_.size() == 0) {
353  throw std::logic_error("Cannot Compile an empty DiagramBuilder.");
354  }
355  ThrowIfAlgebraicLoopsExist();
356 
357  auto blueprint = std::make_unique<typename Diagram<T>::Blueprint>();
358  blueprint->input_port_ids = input_port_ids_;
359  blueprint->output_port_ids = output_port_ids_;
360  blueprint->connection_map = connection_map_;
361  blueprint->systems = std::move(registered_systems_);
362 
363  return blueprint;
364  }
365 
366  // The ordered inputs and outputs of the Diagram to be built.
367  std::vector<InputPortLocator> input_port_ids_;
368  std::vector<OutputPortLocator> output_port_ids_;
369 
370  // For fast membership queries: has this input port already been declared?
371  std::set<InputPortLocator> diagram_input_set_;
372 
373  // A map from the input ports of constituent systems, to the output ports of
374  // the systems from which they get their values.
375  std::map<InputPortLocator, OutputPortLocator> connection_map_;
376 
377  // A mirror on the systems in the diagram. Should have the same values as
378  // registered_systems_. Used for fast membership queries.
379  std::unordered_set<const System<T>*> systems_;
380  // The Systems in this DiagramBuilder, in the order they were registered.
381  std::vector<std::unique_ptr<System<T>>> registered_systems_;
382 
384 };
385 
386 } // namespace systems
387 } // namespace drake
DiagramBuilder()
Definition: diagram_builder.h:34
void Connect(const System< T > &src, const System< T > &dest)
Declares that sole input port on the dest system is connected to sole output port on the src system...
Definition: diagram_builder.h:144
int i
Definition: reset_after_move_test.cc:51
S< T > * AddSystem(Args &&...args)
Constructs a new system with the given args, and adds it to the builder, which retains ownership...
Definition: diagram_builder.h:110
void Connect(const OutputPort< T > &src, const InputPortDescriptor< T > &dest)
Declares that input port dest is connected to output port src.
Definition: diagram_builder.h:128
virtual std::multimap< int, int > GetDirectFeedthroughs() const =0
Reports all direct feedthroughs from input ports to output ports.
int size() const
Returns the fixed size expected for a vector-valued input port.
Definition: input_port_base.h:47
std::pair< const System< T > *, OutputPortIndex > OutputPortLocator
Definition: diagram.h:165
Definition: automotive_demo.cc:90
void Cascade(const System< T > &src, const System< T > &dest)
Cascades src and dest.
Definition: diagram_builder.h:155
std::unique_ptr< Diagram< T > > Build()
Builds the Diagram that has been described by the calls to Connect, ExportInput, and ExportOutput...
Definition: diagram_builder.h:186
const InputPortDescriptor< T > & get_input_port(int port_index) const
Returns the typed input port at index port_index.
Definition: system.h:972
friend int AddRandomInputs(double, systems::DiagramBuilder< double > *)
For each subsystem input port in builder that is (a) not yet connected and (b) labeled as random in t...
Definition: random_source.cc:20
#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
virtual ~DiagramBuilder()
Definition: diagram_builder.h:35
This extends InputPortBase with some scalar type-dependent methods.
Definition: input_port_descriptor.h:19
DiagramBuilder is a factory class for Diagram.
Definition: diagram.h:35
S * AddSystem(Args &&...args)
Constructs a new system with the given args, and adds it to the builder, which retains ownership...
Definition: diagram_builder.h:83
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:71
std::vector< Number > result
Definition: ipopt_solver.cc:151
#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
#define DRAKE_DEMAND(condition)
Evaluates condition and iff the value is false will trigger an assertion failure with a message showi...
Definition: drake_assert.h:45
InputPortIndex get_index() const
Returns the index of this input port within the owning System.
Definition: input_port_base.h:29
std::vector< systems::System< T > * > GetMutableSystems()
Returns the list of contained Systems.
Definition: diagram_builder.h:118
Diagram is a System composed of one or more constituent Systems, arranged in a directed graph where t...
Definition: diagram.h:33
Base class for all System functionality that is dependent on the templatized scalar type T for input...
Definition: input_port_descriptor.h:12
OutputPortIndex ExportOutput(const OutputPort< T > &output)
Declares that the given output port of a constituent system is an output of the entire diagram...
Definition: diagram_builder.h:175
void BuildInto(Diagram< T > *target)
Configures target to have the topology that has been described by the calls to Connect, ExportInput, and ExportOutput.
Definition: diagram_builder.h:197
InputPortIndex ExportInput(const InputPortDescriptor< T > &input)
Declares that the given input port of a constituent system is an input to the entire Diagram...
Definition: diagram_builder.h:162
bool empty() const
Returns whether any Systems have been added yet.
Definition: diagram_builder.h:115
std::pair< const System< T > *, InputPortIndex > InputPortLocator
Definition: diagram.h:164
int get_num_input_ports() const
Returns the number of input ports currently allocated in this System.
Definition: system_base.h:163
const System< T > * get_system() const
Returns a reference to the System that owns this input port.
Definition: input_port_descriptor.h:45
int get_num_output_ports() const
Returns the number of output ports of the system.
Definition: system.h:966
const OutputPort< T > & get_output_port(int port_index) const
Returns the typed output port at index port_index.
Definition: system.h:979
S * AddSystem(std::unique_ptr< S > system)
Takes ownership of system and adds it to the builder.
Definition: diagram_builder.h:51
#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
const System< T > & get_system() const
Returns a reference to the System that owns this output port.
Definition: output_port.h:122
int size() const
Returns the fixed size expected for a vector-valued output port.
Definition: output_port.h:136
A type-safe non-negative index class.
Definition: type_safe_index.h:155
OutputPortIndex get_index() const
Returns the index of this output port within the owning System.
Definition: output_port.h:127