Implements ImplicitGcsFromExplicit

Resolves RobotLocomotion#22307.

geometry/optimization/graph_of_convex_sets.cc

@@ -483,6 +483,53 @@ Vertex* GraphOfConvexSets::AddVertex(const ConvexSet& set, std::string name) {
   return iter->second.get();
 }
 
+namespace {
+
+template <typename C>
+std::vector<std::pair<solvers::Binding<C>, std::unordered_set<Transcription>>>
+ReplaceVariables(
+    const std::vector<std::pair<Binding<C>, std::unordered_set<Transcription>>>&
+        bindings,
+    const std::unordered_map<Variable, Variable>& subs) {
+  std::vector<std::pair<Binding<C>, std::unordered_set<Transcription>>>
+      new_bindings;
+  for (const auto& pair : bindings) {
+    const Binding<C>& binding = pair.first;
+    const std::unordered_set<Transcription>& transcriptions = pair.second;
+    VectorX<Variable> new_vars(ssize(binding.variables()));
+    for (int i = 0; i < ssize(binding.variables()); ++i) {
+      auto it = subs.find(binding.variables()[i]);
+      if (it == subs.end()) {
+        throw std::runtime_error(fmt::format(
+            "ReplaceVariables: variable {} was not found in the substitution "
+            "map. Probably the Drake implementation of AddVertexFromTemplate "
+            "or AddEdgeFromTemplate needs to be updated.",
+            binding.variables()[i].get_name()));
+      }
+      new_vars[i] = it->second;
+    }
+    new_bindings.push_back(std::make_pair(
+        Binding<C>(binding.evaluator(), new_vars), transcriptions));
+  }
+  return new_bindings;
+}
+
+}  // namespace
+
+Vertex* GraphOfConvexSets::AddVertexFromTemplate(const Vertex& v) {
+  Vertex* v_new = AddVertex(v.set(), v.name());
+  std::unordered_map<Variable, Variable> subs;
+  for (int i = 0; i < ssize(v.ell_); ++i) {
+    subs.emplace(v.ell_[i], v_new->ell_[i]);
+  }
+  for (int i = 0; i < ssize(v.placeholder_x_); ++i) {
+    subs.emplace(v.placeholder_x_[i], v_new->placeholder_x_[i]);
+  }
+  v_new->costs_ = ReplaceVariables(v.costs_, subs);
+  v_new->constraints_ = ReplaceVariables(v.constraints_, subs);
+  return v_new;
+}
+
 Edge* GraphOfConvexSets::AddEdge(Vertex* u, Vertex* v, std::string name) {
   DRAKE_DEMAND(u != nullptr);
   DRAKE_DEMAND(v != nullptr);
@@ -498,6 +545,29 @@ Edge* GraphOfConvexSets::AddEdge(Vertex* u, Vertex* v, std::string name) {
   return e;
 }
 
+Edge* GraphOfConvexSets::AddEdgeFromTemplate(Vertex* u, Vertex* v,
+                                             const Edge& e) {
+  Edge* e_new = AddEdge(u, v, e.name());
+  std::unordered_map<Variable, Variable> subs;
+  subs.emplace(e.phi_, e_new->phi_);
+  for (int i = 0; i < ssize(e.y_); ++i) {
+    subs.emplace(e.y_[i], e_new->y_[i]);
+  }
+  for (int i = 0; i < ssize(e.z_); ++i) {
+    subs.emplace(e.z_[i], e_new->z_[i]);
+  }
+  for (int i = 0; i < ssize(e.ell_); ++i) {
+    subs.emplace(e.ell_[i], e_new->ell_[i]);
+  }
+  for (int i = 0; i < ssize(e.slacks_); ++i) {
+    subs.emplace(e.slacks_[i], e_new->slacks_[i]);
+  }
+  e_new->costs_ = ReplaceVariables(e.costs_, subs);
+  e_new->constraints_ = ReplaceVariables(e.constraints_, subs);
+  e_new->phi_value_ = e.phi_value_;
+  return e_new;
+}
+
 void GraphOfConvexSets::RemoveVertex(Vertex* vertex) {
   DRAKE_THROW_UNLESS(vertex != nullptr);
   VertexId vertex_id = vertex->id();

geometry/optimization/graph_of_convex_sets.h

@@ -411,7 +411,7 @@ class GraphOfConvexSets {
     const VertexId id_{};
     const std::unique_ptr<const ConvexSet> set_;
     const std::string name_{};
-    const VectorX<symbolic::Variable> placeholder_x_{};
+    VectorX<symbolic::Variable> placeholder_x_{};
     // Note: ell_[i] is associated with costs_[i].
     solvers::VectorXDecisionVariable ell_{};
     std::vector<std::pair<solvers::Binding<solvers::Cost>,
@@ -438,6 +438,9 @@ class GraphOfConvexSets {
 
     ~Edge();
 
+    /** Returns a deep copy of this edge. */
+    std::unique_ptr<Edge> CloneWithoutVertices() const;
+
     /** Returns the unique identifier associated with this Edge. */
     EdgeId id() const { return id_; }
 
@@ -675,15 +678,30 @@ class GraphOfConvexSets {
   the graph. If @p name is empty then a default name will be provided. */
   Vertex* AddVertex(const ConvexSet& set, std::string name = "");
 
+  /** Adds a new vertex to the graph by taking the name, costs, and constraints
+  (but not any edges) from `v`. `v` does not need to be registered with this GCS
+  instance; this method can be used to effectively copy a Vertex from another
+  GCS instance into `this`. */
+  Vertex* AddVertexFromTemplate(const Vertex& v);
+
   // TODO(russt): Provide helper methods to add multiple vertices which share
   // the same ConvexSet.
 
   /** Adds an edge to the graph from Vertex @p u to Vertex @p v.  The
   vertex references must refer to valid vertices in this graph. If @p name is
   empty then a default name will be provided.
-  */
+  @throws std::exception if `u` or `v` are not valid vertices in this graph. */
   Edge* AddEdge(Vertex* u, Vertex* v, std::string name = "");
 
+  /** Adds an edge to the graph from Vertex @p u to Vertex @p v, by taking the
+  name, costs, and constraints from `template_edge`. `template_edge` does not
+  need to be registered with this GCS instance; this method can be used to
+  effectively copy an Edge from another GCS instance into `this`.
+  @throws std::exception if `u` or `v` are not valid vertices in this graph.
+  @throws std::exception if `u` or `v` do not match the sizes of the
+  `template_edge.u()` and `template_edge.v()` vertices. */
+  Edge* AddEdgeFromTemplate(Vertex* u, Vertex* v, const Edge& template_edge);
+
   /** Removes vertex @p vertex from the graph as well as any edges from or to
   the vertex. Runtime is O(nₑ) where nₑ is the number of edges in the graph.
   @pre The vertex must be part of the graph.

geometry/optimization/implicit_graph_of_convex_sets.cc

@@ -52,6 +52,58 @@ void ImplicitGraphOfConvexSets::ExpandRecursively(Vertex* start,
   }
 }
 
+ImplicitGraphOfConvexSetsFromExplicit::ImplicitGraphOfConvexSetsFromExplicit(
+    const GraphOfConvexSets& gcs)
+    : explicit_gcs_(gcs) {}
+
+ImplicitGraphOfConvexSetsFromExplicit::
+    ~ImplicitGraphOfConvexSetsFromExplicit() = default;
+
+Vertex* ImplicitGraphOfConvexSetsFromExplicit::ImplicitVertexFromExplicit(
+    const GraphOfConvexSets::Vertex& v_explicit) {
+  auto it = explicit_to_implicit_map_.find(&v_explicit);
+  if (it == explicit_to_implicit_map_.end()) {
+    Vertex* v_implicit = mutable_gcs().AddVertexFromTemplate(v_explicit);
+    explicit_to_implicit_map_[&v_explicit] = v_implicit;
+    return v_implicit;
+  }
+  return it->second;
+}
+
+void ImplicitGraphOfConvexSetsFromExplicit::Expand(Vertex* u_implicit) {
+  DRAKE_THROW_UNLESS(u_implicit != nullptr);
+  DRAKE_THROW_UNLESS(mutable_gcs().IsValid(*u_implicit));
+  // Expand is only called once per vertex, so the outgoing edges should be
+  // empty.
+  DRAKE_DEMAND(u_implicit->outgoing_edges().empty());
+
+  // Find the corresponding explicit vertex. The Successors() method should
+  // only call this with vertices that have been added to the implicit GCS.
+  Vertex* u_explicit = explicit_to_implicit_map_.at(u_implicit);
+  DRAKE_DEMAND(u_explicit != nullptr);
+
+  // For each outgoing edge from the explicit vertex, add a corresponding edge
+  // the to the implicit GCS.
+  for (Edge* e_explicit : u_explicit->outgoing_edges()) {
+    DRAKE_DEMAND(e_explicit != nullptr);
+
+    // Add the successor vertex to the implicit GCS if it hasn't been added yet.
+    Vertex* v_explicit{nullptr};
+    auto it = explicit_to_implicit_map_.find(&e_explicit->v());
+    if (it == explicit_to_implicit_map_.end()) {
+      v_explicit = mutable_gcs().AddVertexFromTemplate(e_explicit->v());
+      explicit_to_implicit_map_[&e_explicit->v()] = v_explicit;
+    } else {
+      // Then the successor vertex has already been added to the implicit GCS,
+      // but the edge has not been added yet (since Expand is only called once
+      // per vertex)
+      v_explicit = it->second;
+    }
+    DRAKE_DEMAND(v_explicit != nullptr);
+    mutable_gcs().AddEdgeFromTemplate(u_implicit, v_explicit, *e_explicit);
+  }
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake

geometry/optimization/implicit_graph_of_convex_sets.h

@@ -2,6 +2,7 @@
 
 #include <set>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "drake/geometry/optimization/graph_of_convex_sets.h"
@@ -43,7 +44,7 @@ class ImplicitGraphOfConvexSets {
   /** Makes repeated recursive calls to Successors() until no new vertices will
   be added to the graph, or `max_successor_calls` has been reached.
 
-  Note: `v` is mutable because exanding a vertex requires changes to the
+  Note: `v` is mutable because expanding a vertex requires changes to the
   underlying vertex object.
 
   @throws std::exception if `v` is not already registered with the graph.
@@ -78,11 +79,33 @@ class ImplicitGraphOfConvexSets {
   std::set<GraphOfConvexSets::VertexId> expanded_vertices_{};
 };
 
-// TODO(russt): Add an implementation the provides an interface for an implicit
-// GCS given an explicit GCS. Note that this will require something to copy
-// Vertex and Edges; probably something like GCS::CopyVertex(Vertex* v) that
-// copies the sets, costs, and constraints, but not the internal Edge pointers,
-// etc.
+/** Provides an implicit GCS interface given an explicit GCS. Vertices and
+edges are cloned into the implicit GCS as they are expanded. */
+class ImplicitGraphOfConvexSetsFromExplicit final
+    : public ImplicitGraphOfConvexSets {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ImplicitGraphOfConvexSetsFromExplicit);
+
+  /** Constructs an implicit GCS from an explicit GCS. `gcs` must remain
+  valid for the lifetime of this object. */
+  explicit ImplicitGraphOfConvexSetsFromExplicit(const GraphOfConvexSets& gcs);
+
+  virtual ~ImplicitGraphOfConvexSetsFromExplicit();
+
+  /** Looks up the implicit vertex corresponding to `v`. If `v` is not
+  already in the implicit GCS, it is added. */
+  GraphOfConvexSets::Vertex* ImplicitVertexFromExplicit(
+      const GraphOfConvexSets::Vertex& v);
+
+ protected:
+  void Expand(GraphOfConvexSets::Vertex* v) override;
+
+ private:
+  const GraphOfConvexSets& explicit_gcs_{};
+  std::unordered_map<const GraphOfConvexSets::Vertex*,
+                     GraphOfConvexSets::Vertex*>
+      explicit_to_implicit_map_{};
+};
 
 }  // namespace optimization
 }  // namespace geometry

geometry/optimization/test/implicit_graph_of_convex_sets_test.cc

@@ -223,6 +223,33 @@ GTEST_TEST(ImplicitGraphOfConvexSetsTest, GridGcs) {
   EXPECT_EQ(dut.gcs().num_edges(), 12);
 }
 
+GTEST_TEST(ImplicitGraphOfConvexSetsTest, FromExplicit) {
+  GraphOfConvexSets gcs;
+  Vertex* source = gcs.AddVertex(Point(Vector2d(3., 5.)), "source");
+  Vertex* target = gcs.AddVertex(Point(Vector2d(-2., 4.)), "target");
+  Vertex* sink = gcs.AddVertex(Point(Vector2d(5., -2.3)), "sink");
+  Edge* e_on = gcs.AddEdge(source, target, "e_on");
+  Edge* e_off = gcs.AddEdge(source, sink, "e_off");
+
+  unused(e_on, e_off);
+  ImplicitGraphOfConvexSetsFromExplicit dut(gcs);
+  EXPECT_EQ(dut.gcs().num_vertices(), 0);
+  EXPECT_EQ(dut.gcs().num_edges(), 0);
+
+  Vertex* source_implicit = dut.ImplicitVertexFromExplicit(*source);
+  EXPECT_EQ(dut.gcs().num_vertices(), 1);
+  EXPECT_EQ(dut.gcs().num_edges(), 0);
+
+  // Looking up the same vertex again should not add a new vertex.
+  source_implicit = dut.ImplicitVertexFromExplicit(*source);
+  EXPECT_EQ(dut.gcs().num_vertices(), 1);
+  EXPECT_EQ(dut.gcs().num_edges(), 0);
+
+  dut.ExpandRecursively(source_implicit);
+  EXPECT_EQ(dut.gcs().num_vertices(), 3);
+  EXPECT_EQ(dut.gcs().num_edges(), 2);
+}
+
 }  // namespace optimization
 }  // namespace geometry
 }  // namespace drake