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  PortIdentifier dest_id{dest.get_system(), dest.get_index()};
132  PortIdentifier src_id{&src.get_system(), src.get_index()};
133  ThrowIfInputAlreadyWired(dest_id);
134  ThrowIfSystemNotRegistered(&src.get_system());
135  ThrowIfSystemNotRegistered(dest.get_system());
136  dependency_graph_[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  PortIdentifier id{input.get_system(), input.get_index()};
164  ThrowIfInputAlreadyWired(id);
165  ThrowIfSystemNotRegistered(input.get_system());
166  int return_id = static_cast<int>(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.
175  int ExportOutput(const OutputPort<T>& output) {
176  ThrowIfSystemNotRegistered(&output.get_system());
177  int return_id = static_cast<int>(output_port_ids_.size());
178  output_port_ids_.push_back(
179  PortIdentifier{&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  typedef typename Diagram<T>::PortIdentifier PortIdentifier;
203 
204  void ThrowIfInputAlreadyWired(const PortIdentifier& id) const {
205  if (dependency_graph_.find(id) != dependency_graph_.end() ||
206  diagram_input_set_.find(id) != diagram_input_set_.end()) {
207  throw std::logic_error("Input port is already wired.");
208  }
209  }
210 
211  void ThrowIfSystemNotRegistered(const System<T>* system) const {
212  DRAKE_THROW_UNLESS(systems_.find(system) != systems_.end());
213  }
214 
215  // Helper method to do the algebraic loop test. It recursively performs the
216  // depth-first search on the graph to find cycles.
217  static bool HasCycleRecurse(
218  const PortIdentifier& n,
219  const std::map<PortIdentifier, std::set<PortIdentifier>>& edges,
220  std::set<PortIdentifier>* visited,
221  std::vector<PortIdentifier>* stack) {
222  DRAKE_ASSERT(visited->count(n) == 0);
223  visited->insert(n);
224 
225  auto edge_iter = edges.find(n);
226  if (edge_iter != edges.end()) {
227  DRAKE_ASSERT(std::find(stack->begin(), stack->end(), n) == stack->end());
228  stack->push_back(n);
229  for (const auto& target : edge_iter->second) {
230  if (visited->count(target) == 0 &&
231  HasCycleRecurse(target, edges, visited, stack)) {
232  return true;
233  } else if (std::find(stack->begin(), stack->end(), target) !=
234  stack->end()) {
235  return true;
236  }
237  }
238  stack->pop_back();
239  }
240  return false;
241  }
242 
243  // Evaluates the graph of port dependencies -- including *connections* between
244  // output ports and input ports and direct feedthrough connections between
245  // input ports and output ports. If an algebraic loop is detected, throws
246  // a std::logic_error.
247  void ThrowIfAlgebraicLoopsExist() const {
248  // Each port in the diagram is a node in a graph.
249  // An edge exists from node u to node v if:
250  // 1. output u is connected to input v (via Connect(u, v) method), or
251  // 2. a direct feedthrough from input u to output v is reported.
252  // A depth-first search of the graph should produce a forest of valid trees
253  // if there are no algebraic loops. Otherwise, at least one link moving
254  // *up* the tree will exist.
255 
256  // Build the graph.
257  // Generally, the nodes of the graph would be the set of all defined ports
258  // (input and output) of each subsystem. However, we only need to
259  // consider the input/output ports that have a diagram level output-to-input
260  // connection (ports that are not connected in this manner cannot contribute
261  // to an algebraic loop).
262 
263  // Track *all* of the nodes involved in a diagram-level connection as
264  // described above.
265  std::set<PortIdentifier> nodes;
266  // A map from node u, to the set of edges { (u, v_i) }. In normal cases,
267  // not every node in `nodes` will serve as a key in `edges` (as that is a
268  // necessary condition for there to be no algebraic loop).
269  std::map<PortIdentifier, std::set<PortIdentifier>> edges;
270 
271  // In order to store PortIdentifiers for both input and output ports in the
272  // same set, I need to encode the ports. The identifier for the first input
273  // port and output port look identical (same system pointer, same port
274  // id 0). So, to distinguish them, I'll modify the output ports to use the
275  // negative half of the int space. The function below provides a utility for
276  // encoding an output port id.
277  auto output_to_key = [](int port_id) { return -(port_id + 1); };
278 
279  // Populate the node set from the connections (and define the edges implied
280  // by those connections).
281  for (const auto& connection : dependency_graph_) {
282  // Dependency graph is a mapping from the destination of the connection
283  // to what it *depends on* (the source).
284  const PortIdentifier& src = connection.second;
285  const PortIdentifier& dest = connection.first;
286  PortIdentifier encoded_src{src.first, output_to_key(src.second)};
287  nodes.insert(encoded_src);
288  nodes.insert(dest);
289  edges[encoded_src].insert(dest);
290  }
291 
292  // Populate more edges based on direct feedthrough.
293  for (const auto& system : registered_systems_) {
294  for (const auto& pair : system->GetDirectFeedthroughs()) {
295  PortIdentifier src_port{system.get(), pair.first};
296  PortIdentifier dest_port{system.get(), output_to_key(pair.second)};
297  if (nodes.count(src_port) > 0 && nodes.count(dest_port) > 0) {
298  // Track direct feedthrough only on port pairs where *both* ports are
299  // connected to other ports at the diagram level.
300  edges[src_port].insert(dest_port);
301  }
302  }
303  }
304 
305  // Evaluate the graph for cycles.
306  std::set<PortIdentifier> visited;
307  std::vector<PortIdentifier> stack;
308  for (const auto& node : nodes) {
309  if (visited.count(node) == 0) {
310  if (HasCycleRecurse(node, edges, &visited, &stack)) {
311  std::stringstream ss;
312 
313  auto port_to_stream = [&ss](const auto& id) {
314  ss << " " << id.first->get_name() << ":";
315  if (id.second < 0)
316  ss << "Out(";
317  else
318  ss << "In(";
319  ss << (id.second >= 0 ? id.second : -id.second - 1) << ")";
320  };
321 
322  ss << "Algebraic loop detected in DiagramBuilder:\n";
323  for (size_t i = 0; i < stack.size() - 1; ++i) {
324  port_to_stream(stack[i]);
325  ss << " depends on\n";
326  }
327  port_to_stream(stack.back());
328 
329  throw std::runtime_error(ss.str());
330  }
331  }
332  }
333  }
334 
335  // TODO(russt): Implement AddRandomSources method to wire up all dangling
336  // random input ports with a compatible RandomSource system.
337 
338  // Produces the Blueprint that has been described by the calls to
339  // Connect, ExportInput, and ExportOutput. Throws std::logic_error if the
340  // graph is empty or contains algebraic loops.
341  // The DiagramBuilder passes ownership of the registered systems to the
342  // blueprint.
343  std::unique_ptr<typename Diagram<T>::Blueprint> Compile() {
344  if (registered_systems_.size() == 0) {
345  throw std::logic_error("Cannot Compile an empty DiagramBuilder.");
346  }
347  ThrowIfAlgebraicLoopsExist();
348 
349  auto blueprint = std::make_unique<typename Diagram<T>::Blueprint>();
350  blueprint->input_port_ids = input_port_ids_;
351  blueprint->output_port_ids = output_port_ids_;
352  blueprint->dependency_graph = dependency_graph_;
353  blueprint->systems = std::move(registered_systems_);
354 
355  return blueprint;
356  }
357 
358  // The ordered inputs and outputs of the Diagram to be built.
359  std::vector<PortIdentifier> input_port_ids_;
360  std::vector<PortIdentifier> output_port_ids_;
361 
362  // For fast membership queries: has this input port already been declared?
363  std::set<PortIdentifier> diagram_input_set_;
364 
365  // A map from the input ports of constituent systems, to the output ports of
366  // the systems on which they depend.
367  std::map<PortIdentifier, PortIdentifier> dependency_graph_;
368 
369  // A mirror on the systems in the diagram. Should have the same values as
370  // registered_systems_. Used for fast membership queries.
371  std::unordered_set<const System<T>*> systems_;
372  // The Systems in this DiagramBuilder, in the order they were registered.
373  std::vector<std::unique_ptr<System<T>>> registered_systems_;
374 
376 };
377 
378 } // namespace systems
379 } // 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
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
int 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
virtual std::multimap< int, int > GetDirectFeedthroughs() const =0
Reports all direct feedthroughs from input ports to output ports.
Definition: automotive_demo.cc:89
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 descriptor of the 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
InputPortDescriptor is a notation for specifying the kind of input a System accepts, on a given port.
Definition: input_port_descriptor.h:21
DiagramBuilder is a factory class for Diagram.
Definition: diagram.h:33
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:67
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
std::pair< const System< T > *, int > PortIdentifier
Definition: diagram.h:235
#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
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:31
int 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
A superclass template for systems that receive input, maintain state, and produce output of a given m...
Definition: input_port_descriptor.h:12
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
bool empty() const
Returns whether any Systems have been added yet.
Definition: diagram_builder.h:115
const System< T > * get_system() const
Definition: input_port_descriptor.h:69
int get_num_output_ports() const
Returns the number of output ports of the system.
Definition: system.h:967
int size() const
Definition: input_port_descriptor.h:72
int get_index() const
Definition: input_port_descriptor.h:70
const OutputPort< T > & get_output_port(int port_index) const
Returns the output port at index port_index.
Definition: system.h:983
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:101
int size() const
Returns the fixed size expected for a vector-valued output port.
Definition: output_port.h:115
int get_num_input_ports() const
Returns the number of input ports of the system.
Definition: system.h:962
OutputPortIndex get_index() const
Returns the index of this output port within the owning System.
Definition: output_port.h:106