drake
drake copied to clipboard
RobotLocomotion
Need getters in GcsTrajOptimization
Is your feature request related to a problem? Please describe.
Access to vertices and edges in GcsTrajectoryOptimization is available via its GCS from
const geometry::optimization::GraphOfConvexSets& graph_of_convex_sets() const
however it is impossible to track which vertex was which - a user that constructs GcsTrajectoryOptimization has access to only Subgraphs. There is no access to vertices from Subgraph
TLDR: GcsTrajectoryOptimization and Subgraph classes are encapsulating way too much information.
Describe the solution you'd like
1- A public getter for vertex_to_subgraph_ because users may want to, for example, set their own solution path of vertices. The getter can be const Subgraph* get_subgraph(const Vertex& vertex) const or similar. This would allow users to easily get the subgraph that a vertex belongs to.
2- Class Subgraph should also give public getters for vertices_ and edges_. This would allow users to easily get the vertices and edges in a subgraph. Right now when one adds a subgraph, it is very hacky to retrieve the vertices. The only ways may be getting them from vertex names (see next) or, assuming that the vertex IDs are well-ordered, by bookkeeping the ID numbers.
3- API to rename the vertices in Subgraph . Right now this is hard coded:
vertices_.emplace_back(traj_opt_.gcs_.AddVertex(
CartesianProduct(vertex_set), fmt::format("{}: Region{}", name_, i)));
Users may want to rename their vertices in GcsTrajopt as they may carry meanings.
4- I would also like to have a mutable getter for the underlying GCS so a consumer can remove vertices (related to discussion with @RussTedrake: one wants to remove the vertices from an old problem). Constructing a GcsTrajectoryOptimization can be expensive so users may want to do it only once and then keep maintaining it.
I pinged @RussTedrake about this today. He'll reply with API suggestions, after which point @sadraddini and myself can collaborate to implement it.
In my view, the important encapsulation here is the one provided by the GCS class itself: we want people to be working at the level of costs and constraints on vertices and edges, but not to much directly with the underlying mathematical program (it would be easy to screw up).
The Subgraphs in GcsTrajectoryOptimization are really only meant as a convenience -- often we want to add/operate on vertices and edges in bulk. But I see no problem whatsoever in letting users dig in to work with the subgraph vertex and edges directly. The Subgraph should really be sugar; it's not designed to encapsulate functionality.
W.r.t. removing vertices/edges specifically, I could imagine doing that either at the level of subgraphs or for individual vertices. To be clear, I think that the most immediate use case is solving many GcsTrajectoryOptimization problems with different starts and goals, but on the same subgraphs. Currently users have to add the start and the goal as subgraphs (and then want to remove them). @wrangelvid and I had discussed early on the possible sugar on top of [SolvePath](https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1trajectory__optimization_1_1_gcs_trajectory_optimization.html#a8ddb1df26a2324afc83f84c90281c249) which takes a start Point and a goal Point; underneath it would add the vertices and edges, then solve the problem, then remove the vertices and edges. The reason this is subtle is that we might want to restrict the subgraphs that they connect to and if we remove the vertices too quickly, then the user won't be able to fully explore their solutions (or solve the ConvexRestrictions). But I do think we should try to make that workflow easier.
@jwnimmer-tri -- sorry I was slow. Thanks in advance for any help.
I understand the encapsulation concerns and I agree that the mathematical program inside GCS must be invisible.
I want to make this point that the current API is blocking users like me from the following operations given an GcsTrajectoryOptimization object: (in the order of importance)
1- Manually defining an edge path and solving convex restriction on it.
2- Adding and removing vertices (or a mutable getter to the underlying GCS)
3- Naming a vertex in GcsTrajectoryOptimization (I really like working with the graphviz strings)
I see two approaches for us to close this issue:
a) GcsTrajectoryOptimization is the wrong class for your needs - go and build a GCS yourself! I'm fine if this is the answer.
b) We need to add the needed aforementioned methods to GcsTrajectoryOptimization class. Then I or someone can open a PR for this.
My response was completely supportive of adding the extra methods. I agree that we need them.
It would be great to extend the API of GcsTrajectoryOptimization. I will reply to each point separately.
Solving the convex restriction: Providing subgraph::get_vertices(), subgraph::get_edges(), subgraph::get_vertices() and some refactoring with the addition of a SolveConvexRestriction should do it, right?
Adding/removing vertices: Do you end up adding or removing a single vertex or the whole Subgraph/EdgesBetweenSubgraphs? We can definitely make the underlying GCS problem mutable, but it would be convenient to remove vertices in bulk. A Clone function could be handy too.
Naming the vertices: How about changing the constructor of Subgraph to take a map of names to ConvexSet? An overload in AddRegions could generate some standard naming if only a list is provided.
Coming back to this after sometime. I have implemented a version of this for our internal use. But it never felt satisfying.
How about the following proposal so that we don't expose the users to the underlying graph:
Introduce using RegionIndex = TypeSafeIndex<class RegionTag> for regions in the GCS, and add some API such as getting the std::vector<RegionIndex> GetSolutionPath or SolveConvexRestriction(const std::vector<RegionIndex>& regions_path) to the GcsTrajectoryOptimization.
Let me know what you think.
In #20895, we addressed the getters for the subgraph vertices and solving the convex restriction. There is also a getter to the underlying GCS. The following remains:
- A getter for the Subgraph/EdgesBetweenSubgraph edges, for transparency sake.
- Naming the vertices (maybe by providing AddRegions a dictionary that maps a region name to a convex set).
- Removing subgraphs and associated edges from GcsTrajOpt.
Is there anything I missed?
Now that we have the notion of vertices in GcsTrajectoryOptimization, I have one other discussion point:
Add edge between vertices: The use case for this is the following. I have a new region and I already know which regions are connected to it (I can dive deeper on the specific application that this happens). However, using the current machinery I need to register that new region with a new subgraph, and call AddEdges to a target subgraph, which checks all possible intersections with all regions in the target subgraph. It is so wasteful. It would be nice if we have a AddEdge(const Vertex* source, const Vertex* target) method. Of course the gcs-level cost and constraints should be added to it under the hood.
Interesting. AddEdges makes use of the from/target subgraph by using their order, alternatively one could provide a list of vertex indices similar to AddRegions like so: AddEdges(from_subgraph, to_subgraph, const std::vector<std::pair<int, int>>& edges_between_subgraph, subspace)
In #20980, we found that exposing the edges associated to Subgraph/EdgesBetweenSubgraph doesn't add much benefit and only complexity, hence we are not adding the API for now.
Removing subgraphs and associated edges from GcsTrajOpt is now supported with #21006.
@wrangelvid , @sadraddini -- it seems like perhaps this issue can now be closed? are there still missing getters?
I think I addressed @sadraddini requests. Although he asked for a way in AddEdges to not check for intersections and manual addition, which should be a separate issue.