From 19ec0dda87fde31e7d58c24e3618d768584e5c2d Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Wed, 1 Jan 2025 11:24:50 -0500 Subject: [PATCH] Makes GCS clonable and implements ImplicitGcsFromExplicit Resolves #22307. --- .../geometry/geometry_py_optimization.cc | 35 +++++ .../geometry/test/optimization_test.py | 38 ++++- geometry/optimization/graph_of_convex_sets.cc | 139 +++++++++++++++++- geometry/optimization/graph_of_convex_sets.h | 42 +++++- .../implicit_graph_of_convex_sets.cc | 54 +++++++ .../implicit_graph_of_convex_sets.h | 38 ++++- .../test/graph_of_convex_sets_test.cc | 82 +++++++++++ .../implicit_graph_of_convex_sets_test.cc | 33 +++++ 8 files changed, 446 insertions(+), 15 deletions(-) diff --git a/bindings/pydrake/geometry/geometry_py_optimization.cc b/bindings/pydrake/geometry/geometry_py_optimization.cc index a982ade6a2e1..f525acdf189a 100644 --- a/bindings/pydrake/geometry/geometry_py_optimization.cc +++ b/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::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::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_( + m, "ImplicitGraphOfConvexSetsFromExplicit", cls_doc.doc) + .def(py::init(), 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. diff --git a/bindings/pydrake/geometry/test/optimization_test.py b/bindings/pydrake/geometry/test/optimization_test.py index 93c3a26ebe7a..9793470bdeaf 100644 --- a/bindings/pydrake/geometry/test/optimization_test.py +++ b/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): diff --git a/geometry/optimization/graph_of_convex_sets.cc b/geometry/optimization/graph_of_convex_sets.cc index 42d0a48cc945..ee11f29c267f 100644 --- a/geometry/optimization/graph_of_convex_sets.cc +++ b/geometry/optimization/graph_of_convex_sets.cc @@ -473,6 +473,21 @@ std::optional Edge::GetSolutionPhiXv( } } +std::unique_ptr GraphOfConvexSets::Clone() const { + auto clone = std::make_unique(); + // Map from original vertices to cloned vertices. + std::unordered_map 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 +std::vector, std::unordered_set>> +ReplaceVariables( + const std::vector, std::unordered_set>>& + bindings, + const std::unordered_map& subs) { + std::vector, std::unordered_set>> + new_bindings; + for (const auto& pair : bindings) { + const Binding& binding = pair.first; + const std::unordered_set& transcriptions = pair.second; + VectorX 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(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 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 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(); diff --git a/geometry/optimization/graph_of_convex_sets.h b/geometry/optimization/graph_of_convex_sets.h index 4077df9775d5..0ddcedfaef68 100644 --- a/geometry/optimization/graph_of_convex_sets.h +++ b/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 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. diff --git a/geometry/optimization/implicit_graph_of_convex_sets.cc b/geometry/optimization/implicit_graph_of_convex_sets.cc index 12344ba03b04..d03c33574067 100644 --- a/geometry/optimization/implicit_graph_of_convex_sets.cc +++ b/geometry/optimization/implicit_graph_of_convex_sets.cc @@ -52,6 +52,60 @@ 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; + implicit_to_explicit_map_[v_implicit] = &v_explicit; + 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. + const Vertex* u_explicit = implicit_to_explicit_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 (const 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_implicit{nullptr}; + auto it = explicit_to_implicit_map_.find(&e_explicit->v()); + if (it == explicit_to_implicit_map_.end()) { + v_implicit = mutable_gcs().AddVertexFromTemplate(e_explicit->v()); + explicit_to_implicit_map_[&e_explicit->v()] = v_implicit; + implicit_to_explicit_map_[v_implicit] = &e_explicit->v(); + } 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_implicit = it->second; + } + DRAKE_DEMAND(v_implicit != nullptr); + mutable_gcs().AddEdgeFromTemplate(u_implicit, v_implicit, *e_explicit); + } +} + } // namespace optimization } // namespace geometry } // namespace drake diff --git a/geometry/optimization/implicit_graph_of_convex_sets.h b/geometry/optimization/implicit_graph_of_convex_sets.h index 10956cf55589..159fc906b442 100644 --- a/geometry/optimization/implicit_graph_of_convex_sets.h +++ b/geometry/optimization/implicit_graph_of_convex_sets.h @@ -2,6 +2,7 @@ #include #include +#include #include #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,36 @@ class ImplicitGraphOfConvexSets { std::set 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 + explicit_to_implicit_map_{}; + std::unordered_map + implicit_to_explicit_map_{}; +}; } // namespace optimization } // namespace geometry diff --git a/geometry/optimization/test/graph_of_convex_sets_test.cc b/geometry/optimization/test/graph_of_convex_sets_test.cc index 4eb10ac4f77f..a5992473a42e 100644 --- a/geometry/optimization/test/graph_of_convex_sets_test.cc +++ b/geometry/optimization/test/graph_of_convex_sets_test.cc @@ -114,6 +114,11 @@ GTEST_TEST(GraphOfConvexSetsTest, AddVertex) { EXPECT_EQ(v->ambient_dimension(), 3); EXPECT_EQ(v->name(), "point"); + EXPECT_EQ(g.GetVertexByName("point"), v); + EXPECT_EQ(g.GetVertexByName("garbage"), nullptr); + EXPECT_EQ(g.GetMutableVertexByName("point"), v); + EXPECT_EQ(g.GetMutableVertexByName("garbage"), nullptr); + EXPECT_EQ(v->x().size(), 3); EXPECT_EQ(v->x()[0].get_name(), "point(0)"); EXPECT_TRUE(v->set().PointInSet(p.x())); @@ -161,6 +166,11 @@ GTEST_TEST(GraphOfConvexSetsTest, AddEdge) { EXPECT_EQ(e->u().name(), u->name()); EXPECT_EQ(e->v().name(), v->name()); + EXPECT_EQ(g.GetEdgeByName("e"), e); + EXPECT_EQ(g.GetEdgeByName("garbage"), nullptr); + EXPECT_EQ(g.GetMutableEdgeByName("e"), e); + EXPECT_EQ(g.GetMutableEdgeByName("garbage"), nullptr); + EXPECT_EQ(Variables(e->xu()), Variables(u->x())); EXPECT_EQ(Variables(e->xv()), Variables(v->x())); } @@ -2098,6 +2108,78 @@ TEST_F(ThreeBoxes, SolveConvexRestriction) { } } +TEST_F(ThreeBoxes, AddFromTemplate) { + const Vector2d b{.5, .3}; + + // Vertex cost. + source_->AddCost( + static_cast>(source_->x()).squaredNorm()); + // Vertex constraint. + source_->AddConstraint(source_->x() <= -b); + + // Edge costs. + e_on_->AddCost((e_on_->xu() - e_on_->xv()).squaredNorm()); + e_off_->AddCost((e_off_->xu() - e_off_->xv()).squaredNorm()); + + // Edge constraints. + e_on_->AddConstraint(e_on_->xv() >= b); + e_off_->AddConstraint(e_off_->xv() >= b); + + // Now construct another GCS using g_ as a template. + GraphOfConvexSets clone; + Vertex* source_clone = clone.AddVertexFromTemplate(*source_); + Vertex* target_clone = clone.AddVertexFromTemplate(*target_); + Vertex* sink_clone = clone.AddVertexFromTemplate(*sink_); + EXPECT_EQ(source_clone->ambient_dimension(), source_->ambient_dimension()); + EXPECT_EQ(source_clone->GetCosts().size(), source_->GetCosts().size()); + EXPECT_EQ(source_clone->GetConstraints().size(), + source_->GetConstraints().size()); + + Edge* e_on_clone = + clone.AddEdgeFromTemplate(source_clone, target_clone, *e_on_); + clone.AddEdgeFromTemplate(source_clone, sink_clone, *e_off_); + EXPECT_EQ(e_on_clone->GetCosts().size(), e_on_->GetCosts().size()); + EXPECT_EQ(e_on_clone->GetConstraints().size(), + e_on_->GetConstraints().size()); + + auto result = g_.SolveShortestPath(*source_, *target_, options_); + ASSERT_TRUE(result.is_success()); + + auto clone_result = + clone.SolveShortestPath(*source_clone, *target_clone, options_); + ASSERT_TRUE(clone_result.is_success()); + + EXPECT_NEAR(result.get_optimal_cost(), clone_result.get_optimal_cost(), 1e-6); + + e_on_->NewSlackVariables(1, "s"); + DRAKE_EXPECT_THROWS_MESSAGE( + clone.AddEdgeFromTemplate(source_clone, target_clone, *e_on_), + ".*slack variables.*"); +} + +TEST_F(ThreeBoxes, Clone) { + e_on_->AddCost((e_on_->xu() - e_on_->xv()).squaredNorm()); + e_off_->AddCost((e_off_->xu() - e_off_->xv()).squaredNorm()); + + auto clone = g_.Clone(); + EXPECT_EQ(clone->Vertices().size(), g_.Vertices().size()); + EXPECT_EQ(clone->Edges().size(), g_.Edges().size()); + + auto result = g_.SolveShortestPath(*source_, *target_, options_); + ASSERT_TRUE(result.is_success()); + + Vertex* source_clone = clone->GetMutableVertexByName("source"); + ASSERT_NE(source_clone, nullptr); + Vertex* target_clone = clone->GetMutableVertexByName("target"); + ASSERT_NE(target_clone, nullptr); + + auto clone_result = + clone->SolveShortestPath(*source_clone, *target_clone, options_); + ASSERT_TRUE(clone_result.is_success()); + + EXPECT_NEAR(result.get_optimal_cost(), clone_result.get_optimal_cost(), 1e-6); +} + // A simple shortest-path problem where the continuous variables do not affect // the problem (they are all equality constrained). The GraphOfConvexSets class // should still solve the problem, and the convex relaxation should be optimal. diff --git a/geometry/optimization/test/implicit_graph_of_convex_sets_test.cc b/geometry/optimization/test/implicit_graph_of_convex_sets_test.cc index d41a380acad9..231e5f3e3e7b 100644 --- a/geometry/optimization/test/implicit_graph_of_convex_sets_test.cc +++ b/geometry/optimization/test/implicit_graph_of_convex_sets_test.cc @@ -223,6 +223,39 @@ 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); + + EXPECT_NE(dut.gcs().GetVertexByName("source"), nullptr); + EXPECT_NE(dut.gcs().GetVertexByName("target"), nullptr); + EXPECT_NE(dut.gcs().GetVertexByName("sink"), nullptr); + EXPECT_NE(dut.gcs().GetEdgeByName("e_on"), nullptr); + EXPECT_NE(dut.gcs().GetEdgeByName("e_off"), nullptr); +} + } // namespace optimization } // namespace geometry } // namespace drake