drake

How do I get specific results from the GraphOfConvexSets class in Drake?


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!


Solution

  • 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()))