Makes GCS clonable and implements ImplicitGcsFromExplicit

Resolves RobotLocomotion#22307.

bindings/pydrake/geometry/geometry_py_optimization.cc

@@ -978,12 +978,30 @@ void DefineGraphOfConvexSetsAndRelated(py::module m) {
         .def("AddVertex", &GraphOfConvexSets::AddVertex, py::arg("set"),
             py::arg("name") = "", py_rvp::reference_internal,
             cls_doc.AddVertex.doc)
+        .def("AddVertexFromTemplate", &GraphOfConvexSets::AddVertexFromTemplate,
+            py::arg("template_vertex"), py_rvp::reference_internal,
+            cls_doc.AddVertexFromTemplate.doc)
         .def("AddEdge",
             py::overload_cast<GraphOfConvexSets::Vertex*,
                 GraphOfConvexSets::Vertex*, std::string>(
                 &GraphOfConvexSets::AddEdge),
             py::arg("u"), py::arg("v"), py::arg("name") = "",
             py_rvp::reference_internal, cls_doc.AddEdge.doc)
+        .def("AddEdgeFromTemplate", &GraphOfConvexSets::AddEdgeFromTemplate,
+            py::arg("u"), py::arg("v"), py::arg("template_edge"),
+            py_rvp::reference_internal, cls_doc.AddEdgeFromTemplate.doc)
+        .def("GetVertexByName", &GraphOfConvexSets::GetVertexByName,
+            py::arg("name"), py_rvp::reference_internal,
+            cls_doc.GetVertexByName.doc)
+        .def("GetMutableVertexByName",
+            &GraphOfConvexSets::GetMutableVertexByName, py::arg("name"),
+            py_rvp::reference_internal, cls_doc.GetMutableVertexByName.doc)
+        .def("GetEdgeByName", &GraphOfConvexSets::GetEdgeByName,
+            py::arg("name"), py_rvp::reference_internal,
+            cls_doc.GetEdgeByName.doc)
+        .def("GetMutableEdgeByName", &GraphOfConvexSets::GetMutableEdgeByName,
+            py::arg("name"), py_rvp::reference_internal,
+            cls_doc.GetMutableEdgeByName.doc)
         .def("RemoveVertex",
             py::overload_cast<GraphOfConvexSets::Vertex*>(
                 &GraphOfConvexSets::RemoveVertex),
@@ -1091,6 +1109,7 @@ void DefineGraphOfConvexSetsAndRelated(py::module m) {
             py::arg("options") = GraphOfConvexSetsOptions(),
             py::arg("initial_guess") = nullptr,
             cls_doc.SolveConvexRestriction.doc);
+    DefClone(&graph_of_convex_sets);
   }
 
   // Trampoline class to support deriving from ImplicitGraphOfConvexSets in
@@ -1127,6 +1146,22 @@ void DefineGraphOfConvexSetsAndRelated(py::module m) {
       .def("mutable_gcs", &PyImplicitGraphOfConvexSets::mutable_gcs,
           py_rvp::reference_internal,
           doc.ImplicitGraphOfConvexSets.mutable_gcs.doc);
+
+  // ImplicitGraphOfConvexSetsFromExplicit
+  {
+    const auto& cls_doc = doc.ImplicitGraphOfConvexSetsFromExplicit;
+    py::class_<ImplicitGraphOfConvexSetsFromExplicit,
+        ImplicitGraphOfConvexSets>(
+        m, "ImplicitGraphOfConvexSetsFromExplicit", cls_doc.doc)
+        .def(py::init<const GraphOfConvexSets&>(), py::arg("gcs"),
+            // Keep alive, reference: `self` keeps `gcs` alive.
+            py::keep_alive<1, 2>(),  // BR
+            cls_doc.ctor.doc)
+        .def("ImplicitVertexFromExplicit",
+            &ImplicitGraphOfConvexSetsFromExplicit::ImplicitVertexFromExplicit,
+            py::arg("v"), py_rvp::reference_internal,
+            cls_doc.ImplicitVertexFromExplicit.doc);
+  }
 }
 
 // Definitions for c_iris_collision_geometry.h.

bindings/pydrake/geometry/test/optimization_test.py

@@ -811,8 +811,12 @@ def test_graph_of_convex_sets(self):
         spp = mut.GraphOfConvexSets()
         source = spp.AddVertex(set=mut.Point([0.1]), name="source")
         source_cost = source.AddCost(1.23)
+        self.assertEqual(spp.GetVertexByName(name="source"), source)
+        self.assertEqual(spp.GetMutableVertexByName(name="source"), source)
         target = spp.AddVertex(set=mut.Point([0.2]), name="target")
         edge0 = spp.AddEdge(u=source, v=target, name="edge0")
+        self.assertEqual(spp.GetEdgeByName(name="edge0"), edge0)
+        self.assertEqual(spp.GetMutableEdgeByName(name="edge0"), edge0)
         edge0_cost = edge0.AddCost(2.34)
         edge1 = spp.AddEdge(u=source, v=target, name="edge1")
         edge1.AddCost(3.45)
@@ -1043,19 +1047,30 @@ def test_graph_of_convex_sets(self):
         edge1.AddPhiConstraint(phi_value=True)
         spp.ClearAllPhiConstraints()
 
+        # AddFromTemplate
+        second_target = spp.AddVertexFromTemplate(template_vertex=target)
+        self.assertEqual(second_target.name(), "target")
+        second_edge0 = spp.AddEdgeFromTemplate(
+            u=source, v=second_target, template_edge=edge0)
+        self.assertEqual(second_edge0.name(), "edge0")
+
+        clone = spp.Clone()
+        self.assertEqual(clone.num_vertices(), 3)
+        self.assertEqual(clone.num_edges(), 3)
+
         # Remove Edges
-        self.assertEqual(len(spp.Edges()), 2)
+        self.assertEqual(len(spp.Edges()), 3)
         spp.RemoveEdge(edge0)
-        self.assertEqual(len(spp.Edges()), 1)
+        self.assertEqual(len(spp.Edges()), 2)
         spp.RemoveEdge(edge1)
-        self.assertEqual(len(spp.Edges()), 0)
+        self.assertEqual(len(spp.Edges()), 1)
 
         # Remove Vertices
-        self.assertEqual(len(spp.Vertices()), 2)
+        self.assertEqual(len(spp.Vertices()), 3)
         spp.RemoveVertex(source)
-        self.assertEqual(len(spp.Vertices()), 1)
+        self.assertEqual(len(spp.Vertices()), 2)
         spp.RemoveVertex(target)
-        self.assertEqual(len(spp.Vertices()), 0)
+        self.assertEqual(len(spp.Vertices()), 1)
 
     def test_implicit_graph_of_convex_sets(self):
         # A simple loop graph, a -> b -> c -> a, where the vertices are
@@ -1089,6 +1104,17 @@ def Expand(self, v):
         self.assertEqual(dut.gcs().num_vertices(), 3)
         self.assertEqual(dut.gcs().num_edges(), 3)
 
+    def test_implicit_graph_of_convex_sets_from_explicit(self):
+        gcs = mut.GraphOfConvexSets()
+        source = gcs.AddVertex(set=mut.Point([0.0]), name="source")
+        target = gcs.AddVertex(set=mut.Point([1.0]), name="target")
+        gcs.AddEdge(u=source, v=target, name="edge")
+        dut = mut.ImplicitGraphOfConvexSetsFromExplicit(gcs=gcs)
+        implicit_source = dut.ImplicitVertexFromExplicit(v=source)
+        dut.ExpandRecursively(start=implicit_source, max_successor_calls=10)
+        self.assertEqual(dut.gcs().num_vertices(), 2)
+        self.assertEqual(dut.gcs().num_edges(), 1)
+
 
 class TestCspaceFreePolytope(unittest.TestCase):
     def setUp(self):

geometry/optimization/graph_of_convex_sets.cc

@@ -473,6 +473,21 @@ std::optional<Eigen::VectorXd> Edge::GetSolutionPhiXv(
   }
 }
 
+std::unique_ptr<GraphOfConvexSets> GraphOfConvexSets::Clone() const {
+  auto clone = std::make_unique<GraphOfConvexSets>();
+  // Map from original vertices to cloned vertices.
+  std::unordered_map<const Vertex*, Vertex*> original_to_cloned;
+
+  for (const auto& [v_id, v] : vertices_) {
+    original_to_cloned.emplace(v.get(), clone->AddVertexFromTemplate(*v));
+  }
+  for (const auto& [e_id, e] : edges_) {
+    clone->AddEdgeFromTemplate(original_to_cloned.at(&e->u()),
+                               original_to_cloned.at(&e->v()), *e);
+  }
+  return clone;
+}
+
 Vertex* GraphOfConvexSets::AddVertex(const ConvexSet& set, std::string name) {
   if (name.empty()) {
     name = fmt::format("v{}", vertices_.size());
@@ -483,9 +498,59 @@ 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::Id, 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].get_id());
+      if (it == subs.end()) {
+        throw std::runtime_error(fmt::format(
+            "ReplaceVariables: variable {} (id: {}) was not found in the "
+            "substitution map. Probably the Drake implementation of "
+            "AddVertexFromTemplate or AddEdgeFromTemplate needs to be updated.",
+            binding.variables()[i].get_name(),
+            binding.variables()[i].get_id()));
+      }
+      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());
+  v_new->ell_ = symbolic::MakeVectorContinuousVariable(
+      v.ell_.size(), "v_ell");  // These would normally be created by AddCost.
+  std::unordered_map<Variable::Id, Variable> subs;
+  for (int i = 0; i < ssize(v.ell_); ++i) {
+    subs.emplace(v.ell_[i].get_id(), v_new->ell_[i]);
+  }
+  for (int i = 0; i < ssize(v.placeholder_x_); ++i) {
+    subs.emplace(v.placeholder_x_[i].get_id(), 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);
+  DRAKE_DEMAND(u != nullptr && IsValid(*u));
+  DRAKE_DEMAND(v != nullptr && IsValid(*v));
   if (name.empty()) {
     name = fmt::format("e{}", edges_.size());
   }
@@ -498,6 +563,76 @@ Edge* GraphOfConvexSets::AddEdge(Vertex* u, Vertex* v, std::string name) {
   return e;
 }
 
+Edge* GraphOfConvexSets::AddEdgeFromTemplate(Vertex* u, Vertex* v,
+                                             const Edge& e) {
+  DRAKE_DEMAND(u != nullptr && IsValid(*u));
+  DRAKE_DEMAND(v != nullptr && IsValid(*v));
+
+  Edge* e_new = AddEdge(u, v, e.name());
+  e_new->ell_ = symbolic::MakeVectorContinuousVariable(
+      e.ell_.size(),
+      e.name() + "ell");  // These would normally be created by AddCost.
+  std::unordered_map<Variable::Id, Variable> subs;
+  subs.emplace(e.phi_.get_id(), e_new->phi_);
+  for (int i = 0; i < ssize(e.ell_); ++i) {
+    subs.emplace(e.ell_[i].get_id(), e_new->ell_[i]);
+  }
+  for (int i = 0; i < ssize(e.xu()); ++i) {
+    subs.emplace(e.xu()[i].get_id(), e_new->xu()[i]);
+  }
+  for (int i = 0; i < ssize(e.xv()); ++i) {
+    subs.emplace(e.xv()[i].get_id(), e_new->xv()[i]);
+  }
+  if (e.slacks_.size() > 0) {
+    // Slacks get created and inserted into the other variable lists in a way
+    // that's slightly annoying to deal with. We can add this once it's needed.
+    throw std::runtime_error(
+        "AddEdgeFromTemplate: templates with slack variables are not supported "
+        "yet (but could be).");
+  }
+  e_new->costs_ = ReplaceVariables(e.costs_, subs);
+  e_new->constraints_ = ReplaceVariables(e.constraints_, subs);
+  e_new->phi_value_ = e.phi_value_;
+  return e_new;
+}
+
+const Vertex* GraphOfConvexSets::GetVertexByName(
+    const std::string& name) const {
+  for (const auto& [v_id, v] : vertices_) {
+    if (v->name() == name) {
+      return v.get();
+    }
+  }
+  return nullptr;
+}
+
+Vertex* GraphOfConvexSets::GetMutableVertexByName(const std::string& name) {
+  for (auto& [v_id, v] : vertices_) {
+    if (v->name() == name) {
+      return v.get();
+    }
+  }
+  return nullptr;
+}
+
+const Edge* GraphOfConvexSets::GetEdgeByName(const std::string& name) const {
+  for (const auto& [e_id, e] : edges_) {
+    if (e->name() == name) {
+      return e.get();
+    }
+  }
+  return nullptr;
+}
+
+Edge* GraphOfConvexSets::GetMutableEdgeByName(const std::string& name) {
+  for (auto& [e_id, e] : edges_) {
+    if (e->name() == name) {
+      return e.get();
+    }
+  }
+  return nullptr;
+}
+
 void GraphOfConvexSets::RemoveVertex(Vertex* vertex) {
   DRAKE_THROW_UNLESS(vertex != nullptr);
   VertexId vertex_id = vertex->id();

geometry/optimization/graph_of_convex_sets.h

@@ -671,19 +671,59 @@ class GraphOfConvexSets {
     friend class GraphOfConvexSets;
   };
 
+  /** Returns a deep copy of this graph.
+  @throws std::exception if edges have slack variables. We can add this support
+  once it's needed.
+  */
+  std::unique_ptr<GraphOfConvexSets> Clone() const;
+
   /** Adds a vertex to the graph.  A copy of @p set is cloned and stored inside
   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& template_vertex);
+
   // 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.
+  @throws std::exception if edges have slack variables. We can add this support
+  once it's needed.
+  */
+  Edge* AddEdgeFromTemplate(Vertex* u, Vertex* v, const Edge& template_edge);
+
+  /** Returns the first vertex with the given name, or nullptr if no such vertex
+  exists. */
+  const Vertex* GetVertexByName(const std::string& name) const;
+
+  /** Returns the first vertex with the given name, or nullptr if no such vertex
+  exists. */
+  Vertex* GetMutableVertexByName(const std::string& name);
+
+  /** Returns the first edge with the given name, or nullptr if no such edge
+  exists. */
+  const Edge* GetEdgeByName(const std::string& name) const;
+
+  /** Returns the first edge with the given name, or nullptr if no such edge
+  exists. */
+  Edge* GetMutableEdgeByName(const std::string& name);
+
   /** 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.