Drake
diagram_builder.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <map>
4 #include <memory>
5 #include <set>
6 #include <stdexcept>
7 #include <unordered_set>
8 #include <utility>
9 #include <vector>
10 
16 
17 namespace drake {
18 namespace systems {
19 
26 template <typename T>
27 class DiagramBuilder {
28  public:
29  // DiagramBuilder objects are neither copyable nor moveable.
31 
33  virtual ~DiagramBuilder() {}
34 
48  template<class S>
49  S* AddSystem(std::unique_ptr<S> system) {
50  if (system->get_name().empty()) {
51  system->set_name(system->GetMemoryObjectName());
52  }
53  S* raw_sys_ptr = system.get();
54  systems_.insert(raw_sys_ptr);
55  registered_systems_.push_back(std::move(system));
56  return raw_sys_ptr;
57  }
58 
80  template<class S, typename... Args>
81  S* AddSystem(Args&&... args) {
82  return AddSystem(std::make_unique<S>(std::forward<Args>(args)...));
83  }
84 
107  template<template<typename Scalar> class S, typename... Args>
108  S<T>* AddSystem(Args&&... args) {
109  return AddSystem(std::make_unique<S<T>>(std::forward<Args>(args)...));
110  }
111 
113  bool empty() const { return registered_systems_.empty(); }
114 
116  std::vector<systems::System<T>*> GetMutableSystems() {
117  std::vector<systems::System<T>*> result;
118  result.reserve(registered_systems_.size());
119  for (const auto& system : registered_systems_) {
120  result.push_back(system.get());
121  }
122  return result;
123  }
124 
126  void Connect(const OutputPort<T>& src,
127  const InputPortDescriptor<T>& dest) {
128  DRAKE_DEMAND(src.size() == dest.size());
129  PortIdentifier dest_id{dest.get_system(), dest.get_index()};
130  PortIdentifier src_id{&src.get_system(), src.get_index()};
131  ThrowIfInputAlreadyWired(dest_id);
132  ThrowIfSystemNotRegistered(&src.get_system());
133  ThrowIfSystemNotRegistered(dest.get_system());
134  dependency_graph_[dest_id] = src_id;
135  }
136 
142  void Connect(const System<T>& src, const System<T>& dest) {
145  Connect(src.get_output_port(0), dest.get_input_port(0));
146  }
147 
153  void Cascade(const System<T>& src, const System<T>& dest) {
154  Connect(src, dest);
155  }
156 
161  PortIdentifier id{input.get_system(), input.get_index()};
162  ThrowIfInputAlreadyWired(id);
163  ThrowIfSystemNotRegistered(input.get_system());
164  int return_id = static_cast<int>(input_port_ids_.size());
165  input_port_ids_.push_back(id);
166  diagram_input_set_.insert(id);
167  return return_id;
168  }
169 
173  int ExportOutput(const OutputPort<T>& output) {
174  ThrowIfSystemNotRegistered(&output.get_system());
175  int return_id = static_cast<int>(output_port_ids_.size());
176  output_port_ids_.push_back(
177  PortIdentifier{&output.get_system(), output.get_index()});
178  return return_id;
179  }
180 
184  std::unique_ptr<Diagram<T>> Build() {
185  std::unique_ptr<Diagram<T>> diagram(new Diagram<T>(Compile()));
186  return std::move(diagram);
187  }
188 
195  void BuildInto(Diagram<T>* target) {
196  target->Initialize(Compile());
197  }
198 
199  private:
200  typedef typename Diagram<T>::PortIdentifier PortIdentifier;
201 
202  void ThrowIfInputAlreadyWired(const PortIdentifier& id) const {
203  if (dependency_graph_.find(id) != dependency_graph_.end() ||
204  diagram_input_set_.find(id) != diagram_input_set_.end()) {
205  throw std::logic_error("Input port is already wired.");
206  }
207  }
208 
209  void ThrowIfSystemNotRegistered(const System<T>* system) const {
210  DRAKE_THROW_UNLESS(systems_.find(system) != systems_.end());
211  }
212 
213  // Helper method to do the algebraic loop test. It recursively performs the
214  // depth-first search on the graph to find cycles. The "stack" is really just
215  // a set because we need to do relatively "cheap" lookups for membership in
216  // the stack. The stack-like property of the set is maintained by this method.
217  static bool HasCycleRecurse(
218  const PortIdentifier& n,
219  const std::map<PortIdentifier, std::set<PortIdentifier>>& edges,
220  std::set<PortIdentifier>* visited, std::set<PortIdentifier>* stack) {
221  DRAKE_ASSERT(visited->count(n) == 0);
222  visited->insert(n);
223 
224  auto edge_iter = edges.find(n);
225  if (edge_iter != edges.end()) {
226  DRAKE_ASSERT(stack->count(n) == 0);
227  stack->insert(n); // Push onto the stack.
228  for (const auto& target : edge_iter->second) {
229  if (visited->count(target) == 0 &&
230  HasCycleRecurse(target, edges, visited, stack)) {
231  return true;
232  } else if (stack->count(target) > 0) {
233  return true;
234  }
235  }
236  stack->erase(n); // Pop from the stack.
237  }
238  return false;
239  }
240 
241  // Evaluates the graph of port dependencies -- including *connections* between
242  // output ports and input ports and direct feedthrough connections between
243  // input ports and output ports. If an algebraic loop is detected, throws
244  // a std::logic_error.
245  void ThrowIfAlgebraicLoopsExist() const {
246  // Each port in the diagram is a node in a graph.
247  // An edge exists from node u to node v if:
248  // 1. output u is connected to input v (via Connect(u, v) method), or
249  // 2. a direct feedthrough from input u to output v is reported.
250  // A depth-first search of the graph should produce a forest of valid trees
251  // if there are no algebraic loops. Otherwise, at least one link moving
252  // *up* the tree will exist.
253 
254  // Build the graph.
255  // Generally, the nodes of the graph would be the set of all defined ports
256  // (input and output) of each subsystem. However, we only need to
257  // consider the input/output ports that have a diagram level output-to-input
258  // connection (ports that are not connected in this manner cannot contribute
259  // to an algebraic loop).
260 
261  // Track *all* of the nodes involved in a diagram-level connection as
262  // described above.
263  std::set<PortIdentifier> nodes;
264  // A map from node u, to the set of edges { (u, v_i) }. In normal cases,
265  // not every node in `nodes` will serve as a key in `edges` (as that is a
266  // necessary condition for there to be no algebraic loop).
267  std::map<PortIdentifier, std::set<PortIdentifier>> edges;
268 
269  // In order to store PortIdentifiers for both input and output ports in the
270  // same set, I need to encode the ports. The identifier for the first input
271  // port and output port look identical (same system pointer, same port
272  // id 0). So, to distinguish them, I'll modify the output ports to use the
273  // negative half of the int space. The function below provides a utility for
274  // encoding an output port id.
275  auto output_to_key = [](int port_id) { return -(port_id + 1); };
276 
277  // Populate the node set from the connections (and define the edges implied
278  // by those connections).
279  for (const auto& connection : dependency_graph_) {
280  // Dependency graph is a mapping from the destination of the connection
281  // to what it *depends on* (the source).
282  const PortIdentifier& src = connection.second;
283  const PortIdentifier& dest = connection.first;
284  PortIdentifier encoded_src{src.first, output_to_key(src.second)};
285  nodes.insert(encoded_src);
286  nodes.insert(dest);
287  edges[encoded_src].insert(dest);
288  }
289 
290  // Populate more edges based on direct feedthrough.
291  for (const auto& system : registered_systems_) {
292  for (const auto& pair : system->GetDirectFeedthroughs()) {
293  PortIdentifier src_port{system.get(), pair.first};
294  PortIdentifier dest_port{system.get(), output_to_key(pair.second)};
295  if (nodes.count(src_port) > 0 && nodes.count(dest_port) > 0) {
296  // Track direct feedthrough only on port pairs where *both* ports are
297  // connected to other ports at the diagram level.
298  edges[src_port].insert(dest_port);
299  }
300  }
301  }
302 
303  // Evaluate the graph for cycles.
304  std::set<PortIdentifier> visited;
305  std::set<PortIdentifier> stack;
306  for (const auto& node : nodes) {
307  if (visited.count(node) == 0) {
308  if (HasCycleRecurse(node, edges, &visited, &stack)) {
309  throw std::logic_error("Algebraic loop detected in DiagramBuilder.");
310  }
311  }
312  }
313  }
314 
315  // TODO(russt): Implement AddRandomSources method to wire up all dangling
316  // random input ports with a compatible RandomSource system.
317 
318  // Produces the Blueprint that has been described by the calls to
319  // Connect, ExportInput, and ExportOutput. Throws std::logic_error if the
320  // graph is empty or contains algebraic loops.
321  // The DiagramBuilder passes ownership of the registered systems to the
322  // blueprint.
323  std::unique_ptr<typename Diagram<T>::Blueprint> Compile() {
324  if (registered_systems_.size() == 0) {
325  throw std::logic_error("Cannot Compile an empty DiagramBuilder.");
326  }
327  ThrowIfAlgebraicLoopsExist();
328 
329  auto blueprint = std::make_unique<typename Diagram<T>::Blueprint>();
330  blueprint->input_port_ids = input_port_ids_;
331  blueprint->output_port_ids = output_port_ids_;
332  blueprint->dependency_graph = dependency_graph_;
333  blueprint->systems = std::move(registered_systems_);
334 
335  return blueprint;
336  }
337 
338  // The ordered inputs and outputs of the Diagram to be built.
339  std::vector<PortIdentifier> input_port_ids_;
340  std::vector<PortIdentifier> output_port_ids_;
341 
342  // For fast membership queries: has this input port already been declared?
343  std::set<PortIdentifier> diagram_input_set_;
344 
345  // A map from the input ports of constituent systems, to the output ports of
346  // the systems on which they depend.
347  std::map<PortIdentifier, PortIdentifier> dependency_graph_;
348 
349  // A mirror on the systems in the diagram. Should have the same values as
350  // registered_systems_. Used for fast membership queries.
351  std::unordered_set<const System<T>*> systems_;
352  // The Systems in this DiagramBuilder, in the order they were registered.
353  std::vector<std::unique_ptr<System<T>>> registered_systems_;
354 
356 };
357 
358 } // namespace systems
359 } // namespace drake
DiagramBuilder()
Definition: diagram_builder.h:32
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:142
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:108
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:126
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:173
virtual std::multimap< int, int > GetDirectFeedthroughs() const =0
Reports all direct feedthroughs from input ports to output ports.
Provides a convenient wrapper to throw an exception when a condition is unmet.
Definition: automotive_demo.cc:88
void Cascade(const System< T > &src, const System< T > &dest)
Cascades src and dest.
Definition: diagram_builder.h:153
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:184
const InputPortDescriptor< T > & get_input_port(int port_index) const
Returns the descriptor of the input port at index port_index.
Definition: system.h:918
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:33
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:81
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
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
std::vector< systems::System< T > * > GetMutableSystems()
Returns the list of contained Systems.
Definition: diagram_builder.h:116
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:160
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:195
bool empty() const
Returns whether any Systems have been added yet.
Definition: diagram_builder.h:113
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:913
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:929
S * AddSystem(std::unique_ptr< S > system)
Takes ownership of system and adds it to the builder.
Definition: diagram_builder.h:49
#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:908
OutputPortIndex get_index() const
Returns the index of this output port within the owning System.
Definition: output_port.h:106
Provides careful macros to selectively enable or disable the special member functions for copy-constr...