如何从Drake的GraphOfConvexSets类获取凸集内路径位置结果？

要获取穿过凸集的连续路径，你需要从ShortestPathResult中提取节点位置变量的最优解，而非仅依赖激活边的二进制列表。以下是具体步骤和C++实现方案：

核心思路 #

• 保留SolveShortestPath的完整返回结果（drake::geometry::optimization::ShortestPathResult），这是获取路径点的关键载体。
• 提取路径经过的节点序列，而非仅边的激活状态。
• 对每个路径节点，获取其内部位置变量的最优值，这些点就是路径在凸集内部的具体位置。
• 按节点顺序连接这些点，即可得到穿过凸集的最短路径。

C++代码示例 #

假设你已构建好GraphOfConvexSets对象graph，并指定了起点start_node和终点end_node：

#include "drake/geometry/optimization/graph_of_convex_sets.h"

// 执行最短路径求解
auto result = graph.SolveShortestPath(start_node, end_node);

// 检查求解是否成功
if (!result.is_success()) {
  throw std::runtime_error("Shortest path solve failed");
}

// 获取路径经过的节点序列（按遍历顺序）
const auto& path_nodes = result.GetSolutionPathNodes();

// 收集路径上的坐标点
std::vector<Eigen::VectorXd> path_points;

// 若起点是指定的固定点（而非凸集节点），先加入路径
path_points.push_back(start_point);

// 遍历路径节点，提取每个节点内部的最优位置
for (const auto& node : path_nodes) {
  Eigen::VectorXd node_pos = result.GetSolution(*node);
  path_points.push_back(node_pos);
}

// 若终点是指定的固定点，加入路径末尾
path_points.push_back(end_point);

// 此时path_points中就是按顺序排列的路径点，可直接用于绘制

注意事项 #

• 定义凸集节点时，需确保正确关联位置变量：比如用HPolyhedron定义多边形后，通过graph.AddVertex(hpoly, "node_name")添加节点，Drake会自动为每个节点创建位置决策变量。
• 如果边定义了额外的凸集约束（比如路径必须经过边的凸区域），可通过result.GetSolution(*edge)提取边的内部变量值，补充到路径点序列中。
• 绘制路径时，直接连接path_points中的点即可得到穿过各凸集的连续路径，与示例图效果一致。

内容的提问来源于stack exchange，提问作者JacobSmith