机器人 / 系统课时
从错误码判断规划失败类型
从错误码判断规划失败类型
MoveIt 2 规划失败时,先读取返回的 MoveItErrorCode,再结合 move_group 日志确定检查方向。本篇以 ROS 2 Jazzy、MoveIt 2 和 Panda 机械臂为例,建立从起始状态、目标、路径约束到超时的定位顺序,避免反复盲调规划参数。
学习目标
- 读取规划返回码并区分成功、明确错误和笼统失败。
- 根据错误码选择起点、目标、约束或时限的第一项检查。
- 用单变量复测验证修复,失败时阻止轨迹执行。
先保留真实的规划返回码
MoveGroupInterface::plan() 返回 moveit::core::MoveItErrorCode;其中 val 是整数错误码。SUCCESS=1 表示规划成功,不代表轨迹已经执行。调用方应在 plan() 返回后立即记录代码,不要只看 RViz 的“Failed”提示。
片段示例(放入已有 C++ 规划节点的 void 处理函数中;node 为已初始化的 rclcpp::Node,move_group 为已创建的 MoveGroupInterface):
moveit::planning_interface::MoveGroupInterface::Plan plan;
const auto result = move_group.plan(plan);
RCLCPP_INFO(node->get_logger(), "plan error_code=%d", result.val);
// 规划失败时立即退出,不能把旧轨迹交给执行器。
if (result != moveit::core::MoveItErrorCode::SUCCESS) {
return;
}这段代码依赖现有节点、规划组和目标设置,不是独立程序。它在日志中打印真实 val;如果结果不是 1,后续执行逻辑不会继续。还应保存该次请求的规划组、起点时间戳、目标、约束和规划时限,确保日志与同一次请求对应。
核对常见错误码
已安装 moveit_msgs 且使用 ROS 2 Jazzy 时,可以直接从消息定义读取常量。下面的完整命令不要求启动机器人:
source /opt/ros/jazzy/setup.bash
python3 - <<'PY'
from moveit_msgs.msg import MoveItErrorCodes
# 从已安装的消息类型取值,不手写易过时的编号。
names = (
"SUCCESS", "START_STATE_IN_COLLISION", "GOAL_IN_COLLISION",
"GOAL_VIOLATES_PATH_CONSTRAINTS", "TIMED_OUT", "NO_IK_SOLUTION"
)
for name in names:
print(f"{name}: {getattr(MoveItErrorCodes, name)}")
PY预期输出:
SUCCESS: 1
START_STATE_IN_COLLISION: -10
GOAL_IN_COLLISION: -12
GOAL_VIOLATES_PATH_CONSTRAINTS: -13
TIMED_OUT: -6
NO_IK_SOLUTION: -31重点区分下列返回值。相同表象不一定返回同一码,最终还要对照 move_group、OMPL 和运动学插件的日志。
| 错误码 | 典型含义 | 第一项检查 |
|---|---|---|
-10 START_STATE_IN_COLLISION |
规划起点碰撞 | 起始关节状态与场景碰撞体 |
-11 START_STATE_VIOLATES_PATH_CONSTRAINTS |
起点不满足路径约束 | 起始末端姿态与约束容差 |
-12 GOAL_IN_COLLISION |
目标状态碰撞 | 目标处机械臂与工作台间隙 |
-13 GOAL_VIOLATES_PATH_CONSTRAINTS |
目标违反路径约束 | 目标姿态与路径约束是否冲突 |
-14 GOAL_CONSTRAINTS_VIOLATED |
未满足目标约束 | 目标允许误差与实际目标状态 |
-16 INVALID_GOAL_CONSTRAINTS |
目标约束无效 | 约束字段及对应关节、连杆名 |
-17 INVALID_ROBOT_STATE |
机器人状态无效 | 关节名、关节数量与数据有效性 |
-31 NO_IK_SOLUTION |
逆运动学求解失败 | 目标参考帧、位置及姿态 |
-6 TIMED_OUT |
规划超过允许时间 | 时限、狭窄通道及采样难度 |
-1 PLANNING_FAILED |
未细分的规划失败 | 同次请求的 OMPL 和碰撞日志 |
-23 ROBOT_STATE_STALE 提示机器人状态过期,应先检查 /joint_states 的更新和时钟;-21 FRAME_TRANSFORM_FAILURE 则先检查目标帧与 TF。全部常量可对照 官方消息定义。不要把这些输入问题归咎于规划器“找不到路径”。
按错误类别逐层排查

检查顺序以输入有效性优先,而不是先加大超时:
- 起点:在 RViz 的 MotionPlanning 中查看当前状态与障碍物;核对
/joint_states包含 Panda 规划组的全部活动关节,时间戳未过期。遇到-10先确认是否把工作台碰撞体放到了机械臂内部,不能为消除报警直接关闭碰撞检测。 - 目标:遇到
-12或-31,先暂时只设置一个容易到达的已知安全目标,再核对PoseStamped.header.frame_id、末端连杆和关节限位。无逆解与发生碰撞不是同一回事。 - 约束:遇到
-11或-13,对照OrientationConstraint的absolute_x_axis_tolerance、absolute_y_axis_tolerance、absolute_z_axis_tolerance(单位弧度)及约束参考帧。起点必须已经满足整条路径的约束;仅在隔离测试中放宽约束,不能删除任务的安全限制。 - 规划器:输入均有效后才处理
-6、-1。例如将setPlanningTime(5.0)试改为setPlanningTime(10.0),保持同一目标、场景和约束,再比较三次规划结果;若仍失败,应检查 OMPL 日志中的无效状态、采样失败或无法连接路径,而非继续无限增时。
用可重复试验确认根因
假设 Panda 的 panda_arm 规划在带姿态约束时返回 -11:若起始姿态相对要求方向偏差约 0.25 rad,而对应约束容差设为 0.10 rad,当前起点就可能不符合约束。先记录当前关节值和约束参考帧,在仿真中改用符合约束的安全起点,然后保持目标与障碍物不变,重新请求规划。
验收时至少核对三项:错误码由原失败码变成 1;规划轨迹含非空关节点并通过碰撞检查;轨迹起点与最新机器人状态一致。如果仍出现 -1,记录前后两次完整日志再比较。不要因为 RViz 出现绿色轨迹就自动执行;真实硬件还必须单独确认控制器、限速和急停状态。