diff --git a/geometry/optimization/graph_of_convex_sets.cc b/geometry/optimization/graph_of_convex_sets.cc index 42d0a48cc945..1bc57c9349f7 100644 --- a/geometry/optimization/graph_of_convex_sets.cc +++ b/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 +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]); + 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(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 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 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(); diff --git a/geometry/optimization/graph_of_convex_sets.h b/geometry/optimization/graph_of_convex_sets.h index 4077df9775d5..7cd7fe2af141 100644 --- a/geometry/optimization/graph_of_convex_sets.h +++ b/geometry/optimization/graph_of_convex_sets.h @@ -411,7 +411,7 @@ class GraphOfConvexSets { const VertexId id_{}; const std::unique_ptr set_; const std::string name_{}; - const VectorX placeholder_x_{}; + VectorX placeholder_x_{}; // Note: ell_[i] is associated with costs_[i]. solvers::VectorXDecisionVariable ell_{}; std::vector, @@ -438,6 +438,9 @@ class GraphOfConvexSets { ~Edge(); + /** Returns a deep copy of this edge. */ + std::unique_ptr 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. diff --git a/geometry/optimization/implicit_graph_of_convex_sets.cc b/geometry/optimization/implicit_graph_of_convex_sets.cc index 12344ba03b04..a6872c384453 100644 --- a/geometry/optimization/implicit_graph_of_convex_sets.cc +++ b/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 diff --git a/geometry/optimization/implicit_graph_of_convex_sets.h b/geometry/optimization/implicit_graph_of_convex_sets.h index 10956cf55589..1c7a87d2c9fe 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,33 @@ 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_{}; +}; } // namespace optimization } // namespace geometry 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..1f2baf7fb5b3 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,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