如何从 Drake 中的 GraphOfConvexSets 类获取特定结果？

Question

我正在使用 Drake 的 GraphOfConvexSets 类通过凸集图来解决最短路径问题。我手动构建了自己的图形，并使用类引用解决了 drake::geometry::optimization::GraphOfConvexSets::SolveShortestPath 中的简单 A* 型问题，并使用该结果获得 drake::geometry::optimization： :GraphOfConvexSets::SolveShortestPath::GetSolutionPath（这是一个边列表，1 或 0 表示活动或非活动）。注意下图 在此输入图片描述 我制作了大约 9 个节点，每个节点都有四个边界点。每个节点都是一个凸集。我将它们定义为多边形，然后将其作为顶点添加到图形中。我还添加了顶点之间的边。

但是，我的问题需要一个解决方案来找到凸集内的位置。请注意我附上的第二张图片。 在此输入图片描述

我期望结果位于凸集内的位置，或者类似的位置，我可以在其中绘制凸集以及通过凸集生成的最短路径，类似于上面显示的第二张图像。

我正在使用 Drake 库用 C++ 编写此内容。我对德雷克比较陌生，所以我希望这是一个简单的答案。

谢谢！

Answer 1

solve方法返回一个MathematicalProgramResult，您可以通过调用获取解的值，例如

result.GetSolution(e.xu())

我有该示例的注释以及 python 中的链接代码这里。

例如你可以这样做

result = gcs.SolveShortestPath(v[0], v[4], options)
assert result.is_success()

print(result.GetSolution(v[0].x()))