Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Makes GCS clonable and implements ImplicitGcsFromExplicit #22369

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+461 −15
Open

Makes GCS clonable and implements ImplicitGcsFromExplicit #22369

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions bindings/pydrake/geometry/geometry_py_optimization.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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.
Expand Down
38 changes: 32 additions & 6 deletions bindings/pydrake/geometry/test/optimization_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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):
Expand Down
139 changes: 137 additions & 2 deletions geometry/optimization/graph_of_convex_sets.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand All @@ -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());
}
Expand All @@ -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();
Expand Down
43 changes: 42 additions & 1 deletion geometry/optimization/graph_of_convex_sets.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -671,19 +671,60 @@ 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 (and assigns a new unique VertexId) 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 (and assigns a
new unique EdgeId), 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 (by the order added to `this`) with the given
name, or nullptr if no such vertex exists. */
const Vertex* GetVertexByName(const std::string& name) const;

/** Returns the first vertex (by the order added to `this`) with the given
name, or nullptr if no such vertex exists. */
Vertex* GetMutableVertexByName(const std::string& name);

/** Returns the first edge (by the order added to `this`) with the given
name, or nullptr if no such edge exists. */
const Edge* GetEdgeByName(const std::string& name) const;

/** Returns the first edge (by the order added to `this`) 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.
Expand Down
Loading