I am using Drake's GraphOfConvexSets Class to solve the shortest path problem through a graph of convex sets. I have manually built my own graph and have used the class reference to solve a simple A*-type problem from drake::geometry::optimization::GraphOfConvexSets::SolveShortestPath and using that result to get drake::geometry::optimization::GraphOfConvexSets::SolveShortestPath::GetSolutionPath (which is a list of edges that are either 1 or 0 for active or inactive). Note the picture below enter image description here I have made about 9 nodes that each have four boundary points in them. Each node is a convex set. I defined them as polygons and then added as a vertex to the graph. I also added the edges between the vertices.
However, my problem requires a solution that finds where to go inside the convex set. Note the second picture I have attached. enter image description here
I am expecting the results to be locations within the convex sets, or something similar where I could graph the the convex sets and the resulting shortest path through the convex sets similar to the second image shown above.
I am writing this in C++ with the Drake library. I am relatively new to drake, so I am hoping this is an easy answer.
Thanks!
The solve method returns a MathematicalProgramResult, you can get the value of the solutions by calling, e.g. result.GetSolution(e.xu())
I have notes with that example and link code in python here.
For instance you can do
result = gcs.SolveShortestPath(v[0], v[4], options)
assert result.is_success()
print(result.GetSolution(v[0].x()))