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