Implement SamplePaths for GraphOfConvexSets #21402
Conversation
|
@TobiaMarcucci FYI |
There was a problem hiding this comment.
+@RussTedrake for feature review, please (or delegate)
Reviewable status: LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @bernhardpg)
There was a problem hiding this comment.
Awesome. Thanks! (and sorry for my delay!)
Reviewed 4 of 4 files at r1, all commit messages.
Reviewable status: 6 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @bernhardpg)
geometry/optimization/graph_of_convex_sets.h line 631 at r1 (raw file):
an edge. This function implements the first part of the rounding scheme put forth in Section 4.2 of "Motion Planning around Obstacles with Convex Optimization": https://arxiv.org/abs/2205.04422
it would be good to have more details here. are the returned paths guaranteed to be unique? what, exactly, it the role of max_rounded_paths, etc. and we should check those invariants in the test.
geometry/optimization/graph_of_convex_sets.h line 633 at r1 (raw file):
Optimization": https://arxiv.org/abs/2205.04422 */ std::vector<std::vector<const Edge*>> SamplePaths(
I want to make sure we are moving this GCS class towards supporting more general GCS problems (not only shortest path). I think this method name is still appropriate; I can't think of a better one -- the notion of a path from a source to target is broadly applicable, I guess.
The term "flow value" is not used consistently throughout this documentation. (we use AddPhiConstraint, etc on the Edge class, which I don't like as much as flow). Recommendation:
"flow values" => "flow values (the relaxed binary variables associated with each Edge)".
geometry/optimization/graph_of_convex_sets.h line 636 at r1 (raw file):
const Vertex& source, const Vertex& target, const solvers::MathematicalProgramResult& result, const GraphOfConvexSetsOptions& options) const;
document your use of options. it would not have been clear to me that options.preprocessing would have had an impact on this method.
geometry/optimization/graph_of_convex_sets.cc line 1370 at r1 (raw file):
if (!result.is_success()) { throw std::runtime_error( "Cannot sample paths when result.is_success() is false.");
is this really a hard requirement? if the solver does not declare success (e.g. early termination...?), but still gives a best effort solution, can people not call this method?
geometry/optimization/graph_of_convex_sets.cc line 1392 at r1 (raw file):
if (*options.convex_relaxation && *options.max_rounded_paths > 0 && result.is_success()) {
- definitely don't need result.is_success again here.
- in this method, I would think you should run regardless of the convex_relaxation values.
- move the throw for max_rounded_paths to the top (it's an argument check)? and prefer DRAKE_THROW_UNLESS(*options.max_rounded_paths > 0);
- you need to declare any requirements like this in the doc string.
bindings/pydrake/geometry/geometry_py_optimization.cc line 898 at r1 (raw file):
py::arg("target"), py::arg("result"), py::arg("options") = GraphOfConvexSetsOptions(), py_rvp::reference_internal, cls_doc.SamplePaths.doc)
we'll need to have a call to SamplePaths in the python test, too... to make sure it runs and the arguments are spelled correctly.
There was a problem hiding this comment.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @bernhardpg)
geometry/optimization/graph_of_convex_sets.cc line 1364 at r1 (raw file):
} std::vector<std::vector<const Edge*>> GraphOfConvexSets::SamplePaths(
we don't want to duplicate all this code. can we not call this method from the SolveShortestPath call? (is it because of the duplicate preprocessing?)
geometry/optimization/graph_of_convex_sets.cc line 1388 at r1 (raw file):
std::set<EdgeId> unusable_edges; if (*options.preprocessing) { unusable_edges = PreprocessShortestPath(source_id, target_id, options);
is the result of preprocessing not sufficiently encoded in the results? (e.g. phi is zero for those edges?)
bernhardpg
force-pushed
the
implement-gcs-sample-paths
branch
from
e939d1e
to
0df209e
Compare
bernhardpg
force-pushed
the
implement-gcs-sample-paths
branch
from
18c53b5
to
3a05123
Compare
There was a problem hiding this comment.
Addressed the comments.
I also noticed this line:
What is the implications of this, considering we might now call SamplePaths with a result where result.is_success() is False?
Reviewable status: 8 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @bernhardpg)
geometry/optimization/graph_of_convex_sets.h line 631 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
it would be good to have more details here. are the returned paths guaranteed to be unique? what, exactly, it the role of max_rounded_paths, etc. and we should check those invariants in the test.
Agreed. The sampling logic is copy/paste from the current SolveShortestPath. So then all the returned paths should be unique, correct? I will add this to the docstrings.
Also, for the same reason of copy/paste, the role of max_rounded_paths and max_rounding_trials are the same their description in GraphOfConvexSetsOptions. I would think it is sufficient and desirable to not repeat those descriptions here. WDYT?
geometry/optimization/graph_of_convex_sets.h line 633 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
I want to make sure we are moving this GCS class towards supporting more general GCS problems (not only shortest path). I think this method name is still appropriate; I can't think of a better one -- the notion of a path from a source to target is broadly applicable, I guess.
The term "flow value" is not used consistently throughout this documentation. (we use AddPhiConstraint, etc on the Edge class, which I don't like as much as flow). Recommendation:
"flow values" => "flow values (the relaxed binary variables associated with each Edge)".
Done.
geometry/optimization/graph_of_convex_sets.h line 636 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
document your use of options. it would not have been clear to me that options.preprocessing would have had an impact on this method.
See the above discussion, as well as the new changes.
geometry/optimization/graph_of_convex_sets.cc line 1364 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
we don't want to duplicate all this code. can we not call this method from the SolveShortestPath call? (is it because of the duplicate preprocessing?)
Done. (I had to reshuffle parts of the SolveShortestPath code)
geometry/optimization/graph_of_convex_sets.cc line 1370 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
is this really a hard requirement? if the solver does not declare success (e.g. early termination...?), but still gives a best effort solution, can people not call this method?
Here I was following the code in GetSolutionPath where this is similarly checked. While I think one can argue both for removing and keeping this check, I believe we should be consistent across all the functions that use a result to retrieve paths from the graph.
Personally I think it makes more sense to remove this check (for both functions).
geometry/optimization/graph_of_convex_sets.cc line 1388 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
is the result of preprocessing not sufficiently encoded in the results? (e.g. phi is zero for those edges?)
Done.
geometry/optimization/graph_of_convex_sets.cc line 1392 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
- definitely don't need result.is_success again here.
- in this method, I would think you should run regardless of the convex_relaxation values.
- move the throw for max_rounded_paths to the top (it's an argument check)? and prefer DRAKE_THROW_UNLESS(*options.max_rounded_paths > 0);
- you need to declare any requirements like this in the doc string.
Done.
bindings/pydrake/geometry/geometry_py_optimization.cc line 898 at r1 (raw file):
Previously, RussTedrake (Russ Tedrake) wrote…
we'll need to have a call to SamplePaths in the python test, too... to make sure it runs and the arguments are spelled correctly.
Done.
There was a problem hiding this comment.
Notice that I added two signatures for the SamplePaths function, one that takes in a MathematicalProgramResult and one that takes in just a dictionary of flow values for all edges. This greatly simplified removing the duplicated code in SolveShortestPath. If we don't like supporting both, we can make the one that takes the flows private/internal. But personally I don't see why we shouldn't support both.
Reviewable status: 8 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @bernhardpg)
geometry/optimization/graph_of_convex_sets.cc line 1370 at r1 (raw file):
Previously, bernhardpg wrote…
Here I was following the code in
GetSolutionPathwhere this is similarly checked. While I think one can argue both for removing and keeping this check, I believe we should be consistent across all the functions that use aresultto retrieve paths from the graph.Personally I think it makes more sense to remove this check (for both functions).
(I did remove the check for SamplePaths)
There was a problem hiding this comment.
What is the implications of this, considering we might now call
SamplePathswith a result whereresult.is_success()is False?
If it is possible that there are no paths from the start to the goal, we would have to handle that case here.
If we don't like supporting both, we can make the one that takes the
flowsprivate/internal. But personally I don't see why we shouldn't support both.
I was going to suggest the private while reading below. One reason why we might not want to support both is
another reason is the comment I made below about my other open PR.
Reviewed 5 of 5 files at r3, all commit messages.
Reviewable status: 5 unresolved discussions, LGTM missing from assignee RussTedrake(platform), needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @bernhardpg)
geometry/optimization/graph_of_convex_sets.h line 631 at r1 (raw file):
Previously, bernhardpg wrote…
Agreed. The sampling logic is copy/paste from the current
SolveShortestPath. So then all the returned paths should be unique, correct? I will add this to the docstrings.Also, for the same reason of copy/paste, the role of
max_rounded_pathsandmax_rounding_trialsare the same their description inGraphOfConvexSetsOptions. I would think it is sufficient and desirable to not repeat those descriptions here. WDYT?
Agreed. Referencing the GraphOfConvexSetsOptions docs is the best solution for that case.
geometry/optimization/graph_of_convex_sets.h line 638 at r3 (raw file):
options.max_rounded_paths, options.max_rounding_trials, and options.flow_tolerance, as described in `GraphOfConvexSetsOptions`. Note that this function will throw unless options.max_rounded_paths > 0,
the spelling is
@throws std::exception if options.max_rounded_path < 1.
so it shows consistently in doxygen.
geometry/optimization/graph_of_convex_sets.h line 658 at r3 (raw file):
options.max_rounded_paths, options.max_rounding_trials, and options.flow_tolerance, as described in `GraphOfConvexSetsOptions`. Note that this function will throw unless options.max_rounded_paths > 0,
@throws std::exception if options.max_rounded_path < 1.
geometry/optimization/graph_of_convex_sets.cc line 1364 at r1 (raw file):
Previously, bernhardpg wrote…
Done. (I had to reshuffle parts of the
SolveShortestPathcode)
btw -- please take a look at #21506 . I'm happy to land this first, and then resolve the merge conflicts in that PR. But there is a chance that the change I made there could impact you here? Specifically, I fill out the placeholder variables in the results before doing the rounding call. If we're going to do that anyhow, then perhaps we only need the single entry point for SamplePaths which takes the results? WDYT?
geometry/optimization/graph_of_convex_sets.cc line 1370 at r1 (raw file):
Previously, bernhardpg wrote…
(I did remove the check for
SamplePaths)
Agreed. GetSolutionPath has solution in the name, so maybe it feels a little more reasonable to demand that a solution was found. But I could be convinced we should change that, too.
Adresses #21000.
Implements a function
SamplePaths(source, target, result, options)that uses the flow variables specified inresultto sample a number of random paths through theGraphOfConvexSetsaccording to the parameters inoptions. Returns a vector of potential paths from the source to target (where each path is a vectors of edges).I decided to not add the
SolveConvexRestrictionto this function, and leave it to the user what to do with the paths (in #21000 I proposed having a function that does both the sampling AND solve the convex restrictions per path - but I think the cleaner thing to do is leave them separated).The code is for the most part a straight copy-paste of the relevant parts of
SolveShortestPath. Hence, this function could potentially be called in theSolveShortestPathfunction and replace the relevant parts of that function. I think that solution would be better in terms of code style and for future code maintenance, but I do not think it is not super important if we prefer to leave theSolveShortestPathcode untouched.Note that the functionality of this function would be equivalent for any discrete graph, and hence in principle does not really have anything to do with the
GraphOfConvexSetsclass. I don't think that this is actionable at this point, but I could add a comment to the code as a reminder of this, if we at some point decide to build GCS on top of a more general graph codebase.This change is