Makes GCS clonable and implements ImplicitGcsFromExplicit

[RussTedrake]

Resolves #22307.

+@shaoyuancc for feature review, please.

---

This change is 

[shaoyuancc]

geometry/optimization/test/implicit_graph_of_convex_sets_test.cc line 226 at r1 (raw file):

}

GTEST_TEST(ImplicitGraphOfConvexSetsTest, FromExplicit) {

Should we also have a test like in graph_of_convex_sets_test.cc that checks that costs and constraints are correctly cloned or is that not necessary?

[shaoyuancc]

geometry/optimization/graph_of_convex_sets.cc line 537 at r1 (raw file):

Vertex* GraphOfConvexSets::AddVertexFromTemplate(const Vertex& v) {
  Vertex* v_new = AddVertex(v.set(), v.name());
  v_new->ell_ = v.ell_;  // These would normally be created by AddCost.

This is probably just me not understanding how this all works but it seems to me that the variables in the costs and constraints are replaced with v_new's placeholder_x_ variables, but the v_new.ell_ variables are overwritten with v.ell_ variables, and I'm not understanding why we don't want v_new to have it's own new ell_ variables...? (same question for AddEdgeFromTemplate below)

[shaoyuancc]

but I feel unqualified to review this, so it might be good to have someone else review it as well?

Reviewed 8 of 8 files at r1, all commit messages.
Reviewable status: 2 unresolved discussions, needs platform reviewer assigned, needs at least two assigned reviewers

[RussTedrake]

+@cohnt for an additional feature review, please.
FTR -- the role of the feature reviewer is to make sure that the changes are good for the feature (here GCS / ImplicitGCS). The additional platform review will be able to provide some backup on e.g. the c++ implementation.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned

---

geometry/optimization/graph_of_convex_sets.cc line 537 at r1 (raw file):

Previously, shaoyuancc (Shao Yuan) wrote…

This is probably just me not understanding how this all works but it seems to me that the variables in the costs and constraints are replaced with v_new's placeholder_x_ variables, but the v_new.ell_ variables are overwritten with v.ell_ variables, and I'm not understanding why we don't want v_new to have it's own new ell_ variables...? (same question for AddEdgeFromTemplate below)

Done. My thinking was that since the ell variables were strictly internal, there was zero risk of using the same variables. But I think you're right that it's just cleaner to make new variables here, too.

---

geometry/optimization/test/implicit_graph_of_convex_sets_test.cc line 226 at r1 (raw file):

Previously, shaoyuancc (Shao Yuan) wrote…

Should we also have a test like in graph_of_convex_sets_test.cc that checks that costs and constraints are correctly cloned or is that not necessary?

It's the job of the tests in graph_of_convex_sets_test to confirm that the AddFromTemplate methods do the right thing. The tests in this file can assume that this part of the implementation is correct, and must test the new logic (e.g. that we call the AddFromTemplate methods the correct number of times)

[cohnt]

Mostly looks good. My only concern is related to the determinism of the order vertices and edges are added, since they're internally being stored in std::map objects. (Presumably, this could lead to nondeterminism downstream, which I'm guessing we want to avoid. For example, in breaking ties when expanding vertices in GCS*.) If this isn't a problem, or if I'm misunderstanding what std::map does behind the scenes, then feel free to ignore.

Reviewed 5 of 8 files at r1, 3 of 3 files at r2, all commit messages.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned

---

geometry/optimization/test/graph_of_convex_sets_test.cc line 116 at r2 (raw file):

  EXPECT_EQ(v->ambient_dimension(), 3);
  EXPECT_EQ(v->name(), "point");

btw do we need test coverage for the case where multiple vertices have the same name? Ditto below for edges

---

geometry/optimization/graph_of_convex_sets.h line 687 at r2 (raw file):

  (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`. */

nit: This new vertex will now have a new VertexId, correct? Might be useful to document that this is also different.

Suggestion: Adds a new vertex (and assigns a new unique VertexId).

---

geometry/optimization/graph_of_convex_sets.h line 706 at r2 (raw file):

  @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

nit: "We can add this support once it's needed." Should probably be in a TODO, rather than in the docstring?

---

geometry/optimization/graph_of_convex_sets.h line 711 at r2 (raw file):

  Edge* AddEdgeFromTemplate(Vertex* u, Vertex* v, const Edge& template_edge);

  /** Returns the first vertex with the given name, or nullptr if no such vertex

What does "first" mean in this context? Since the vertices are stored internally as a map, I think this method might not be deterministic.

Ditto for GetMutableVertexByName, GetEdgeByName, and GetMutableEdgeByName

[shaoyuancc]

Reviewed 3 of 3 files at r2, all commit messages.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee cohnt, needs platform reviewer assigned

[RussTedrake]

The entire reason that VertexId and EdgeId exist here is to avoid the non-determinism. If we did std::map keyed on a vertex*, then the order would be potentially nondeterministic.

The std::unordered_map with a Vertex* key in the implicit_from_explicit code is the one that could be dangerous, but it's only ever used as a lookup. We never do anything like for v in vertex_map that would lead to non-determinism; the storage order doesn't matter. I suppose I could even use VertexId there, just for future proofing. (I've done that)

Reviewable status: 1 unresolved discussion, LGTM missing from assignee cohnt, needs platform reviewer assigned

---

geometry/optimization/graph_of_convex_sets.h line 687 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit: This new vertex will now have a new VertexId, correct? Might be useful to document that this is also different.

Suggestion: Adds a new vertex (and assigns a new unique VertexId).

Done.

---

geometry/optimization/graph_of_convex_sets.h line 706 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

nit: "We can add this support once it's needed." Should probably be in a TODO, rather than in the docstring?

My reasoning is that if it's in the doc string then people will see it in the public docs and know they can ask for it.

---

geometry/optimization/graph_of_convex_sets.h line 711 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

What does "first" mean in this context? Since the vertices are stored internally as a map, I think this method might not be deterministic.

Ditto for GetMutableVertexByName, GetEdgeByName, and GetMutableEdgeByName

Done. https://stackoverflow.com/questions/7648756/is-the-order-of-iterating-through-stdmap-known-and-guaranteed-by-the-standard

---

geometry/optimization/test/graph_of_convex_sets_test.cc line 116 at r2 (raw file):

Previously, cohnt (Thomas Cohn) wrote…

btw do we need test coverage for the case where multiple vertices have the same name? Ditto below for edges

Done.

[cohnt]

Ah, I hadn't realized that iterating through std::map is deterministic. But good call not keying on a pointer.

Reviewed 4 of 4 files at r3, all commit messages.
Reviewable status: needs platform reviewer assigned

[RussTedrake]

+@sammy-tri for platform review, per rotation

Reviewable status: LGTM missing from assignee sammy-tri(platform)