机器人 / 系统课时
排查控制器拒绝执行轨迹
排查控制器拒绝执行轨迹
MoveIt 2 规划成功,但点击执行后机械臂不动,可能是控制器未激活、动作接口无法连接、轨迹目标被拒绝,或执行过程中触发容差和超时保护。本课以 ROS 2 Jazzy 的 Panda 七轴机械臂为例,建立从控制器状态到轨迹反馈的完整诊断顺序。
学习目标
- 使用 ROS 2 命令检查控制器生命周期、动作服务器和关节映射。
- 验证轨迹关节名称、时间字段及起始状态是否满足执行条件。
- 根据动作结果、控制器反馈和 MoveIt 日志区分拒绝、中止与超时,并验证修复效果。
先判断执行失败发生在哪一层
MoveIt 2 的 plan() 成功只说明找到了符合规划约束的轨迹,并不表示控制器已经接受执行请求。真正执行时,还要经过 MoveIt 的轨迹执行管理器、控制器适配层和 ros2_control 的动作服务器。
正常情况下,MoveGroupInterface::execute() 将轨迹交给 move_group 中的 TrajectoryExecutionManager。执行管理器检查起始状态并选择控制器,通过 FollowJointTrajectory 动作接口提交目标。控制器接受后,驱动硬件或模拟硬件,并通过动作反馈报告执行进度。

诊断时先记录失败发生在哪个阶段:
- 未发送轨迹:MoveIt 找不到覆盖目标关节的控制器,或当前关节状态与轨迹起点相差过大。
- 目标直接被拒绝:动作服务器存在,但控制器认为目标不合法,例如关节集合、时间戳或轨迹字段错误。
- 接受后中止:动作目标已被接受,但运行时违反路径容差或终点容差。
- 执行被取消:MoveIt 监控到执行时间超限,或者用户、上层任务主动取消。
检查控制器激活状态与动作接口
先在已经启动 Panda 仿真系统的 ROS 2 Jazzy 终端运行以下命令。终端应已加载对应的 ROS 环境;示例使用默认的 /controller_manager,自定义命名空间时需要调整控制器管理器路径。
# 查询已加载控制器的类型和生命周期状态
ros2 control list_controllers预期输出(Panda 示例,空白和其他控制器因环境而异):
panda_arm_controller[joint_trajectory_controller/JointTrajectoryController] active
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active重点查看 panda_arm_controller 是否为 active:
active:控制器已经激活,可以继续检查动作接口。inactive:控制器已加载但未激活,检查硬件接口是否可用、是否被其他控制器占用。unconfigured:控制器尚未成功配置,应检查参数及初始化日志。- 列表中不存在:检查启动文件是否加载该控制器,以及 controller spawner 是否正常完成。
可继续使用 ros2 control list_hardware_interfaces 检查关节命令接口是否可用及被正确认领。不要在硬件接口冲突时强行激活多个控制器。
随后查询轨迹动作服务器:
# 确认 Panda 机械臂的动作名称与接口类型
ros2 action list -t | grep '/panda_arm_controller/follow_joint_trajectory'预期输出:
/panda_arm_controller/follow_joint_trajectory [control_msgs/action/FollowJointTrajectory]如果没有匹配结果,说明当前 ROS 图中未发现该名称的动作接口。应检查控制器是否激活、动作名称是否被命名空间或重映射改变,而不是立即重新运行规划器。
对于只在模拟硬件中出现的 inactive 状态,排除接口冲突后可以使用 ros2 control switch_controllers --activate panda_arm_controller 激活,并再次检查是否变为 active。真实机械臂必须先完成安全检查,不能直接照搬模拟环境的激活操作。
核对 MoveIt 控制器映射与轨迹格式
控制器已经激活却仍然无法执行时,需要区分两份配置:ros2_controllers.yaml 决定 ros2_control 实际加载什么控制器及其硬件接口;moveit_controllers.yaml 决定 MoveIt 将轨迹发送给哪个控制器动作服务器。
下面是 Panda 七轴机械臂在 moveit_controllers.yaml 中的完整相关配置示例,实际生效前必须由启动文件加载到 move_group:
trajectory_execution:
allowed_execution_duration_scaling: 1.2
allowed_goal_duration_margin: 0.5
allowed_start_tolerance: 0.01
execution_duration_monitoring: true
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- panda_arm_controller
panda_arm_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7这里的控制器名称与 action_ns 共同对应 /panda_arm_controller/follow_joint_trajectory。如果实际控制器叫作其他名称,必须同步修改 MoveIt 映射,不能只修改其中一份文件。
检查运行中加载的 MoveIt 控制器列表与 ros2_control 关节列表:
# 检查 MoveIt 实际读取的控制器名称
ros2 param get /move_group moveit_simple_controller_manager.controller_names
# 检查轨迹控制器负责的关节
ros2 param get /panda_arm_controller joints预期输出(使用上述配置):
String values are: ['panda_arm_controller']
String values are: ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7']若运行中的参数与配置文件不同,先检查启动文件是否加载了正确的 YAML,再重启相关节点验证。修改磁盘文件并不会自动改变已经运行的全部只读控制器参数。
检查轨迹目标的合法性
对于 trajectory_msgs/msg/JointTrajectory,重点检查以下字段:
| 检查项 | 正确条件 | 常见错误 |
|---|---|---|
joint_names |
与控制器要求的关节集合一致 | 缺少关节、拼写错误、重复名称 |
points[].positions |
每个点包含与关节数量匹配的数值 | 七轴轨迹只提供六个位置 |
points[].velocities |
如果提供,长度应与关节数量一致 | 非空数组维度错误 |
points[].time_from_start |
轨迹时间有效,路点时间严格递增 | 后一个路点时间不大于前一个 |
header.stamp |
零时间戳表示立即开始;非零时间戳应有效 | 使用已经过期的绝对时间 |
默认 allow_partial_joints_goal: false 时,不能仅提交部分受控关节。positions 必须按消息中 joint_names 的顺序逐一对应,但消息的关节顺序不必与控制器内部列表顺序完全一致。
例如,七轴轨迹的两个路点分别设置为 time_from_start=1.0 s 和 3.0 s,每个路点都有七个位置值,才满足这里的基本维度和时间顺序检查。具体关节角度还必须满足机器人模型限制和安全约束。
还应检查轨迹首点与实时关节状态的差距。示例配置中的 allowed_start_tolerance: 0.01 对转动关节表示约 0.01 rad 的起点允许偏差。若某关节实际起点比规划起点偏离 0.03 rad,MoveIt 就可能在发送动作目标之前阻止执行。
根据错误结果区分拒绝、中止与超时
FollowJointTrajectory 的结果包含 error_code 和 error_string,但如果目标在 Action Goal 阶段直接被拒绝,客户端可能根本不会收到通常意义上的执行结果。应同时查看动作客户端反馈、move_group 日志与控制器节点日志。
| 现象或结果 | 可能原因 | 优先处理 |
|---|---|---|
| MoveIt 无法选择控制器 | 关节覆盖范围或控制器映射错误 | 核对 controller_names、joints |
Goal rejected |
控制器不接受目标,或轨迹校验失败 | 检查状态、时间戳、关节集合与路点字段 |
INVALID_JOINTS (-2)、OLD_HEADER_TIMESTAMP (-3) |
关节不匹配或时间戳过期 | 修正轨迹关节列表和时间基准 |
PATH_TOLERANCE_VIOLATED (-4) |
执行过程中实际轨迹偏离允许范围 | 检查跟踪误差、硬件状态及容差 |
GOAL_TOLERANCE_VIOLATED (-5) |
终点未在规定时间内满足容差 | 检查终点误差、速度和目标时间容差 |
INVALID_GOAL (-1) 也属于接口定义的错误码,可结合 error_string 检查无效轨迹的具体原因。不同控制器实现可能直接拒绝目标,而不是返回上述全部错误码。
对于动作已接受却超时的情况,还需要区分 MoveIt 的执行时限 与 控制器自身的目标容差和超时机制。
示例配置中,假设轨迹计划执行 4.0 s,MoveIt 的允许执行时间可按以下参数计算:
- 计划时间:
4.0 s。 allowed_execution_duration_scaling: 1.2。allowed_goal_duration_margin: 0.5,单位为秒。- 允许执行时间:
4.0 × 1.2 + 0.5 = 5.3 s。
开启 execution_duration_monitoring 后,如果执行超过该时限,MoveIt 可能主动取消目标。此时不能直接判定控制器拒绝了轨迹,应查明实际执行速度、反馈延迟及取消请求来源。
控制器自己的 constraints.goal_time、goal_time_tolerance 等决定终点达标的时间要求,与 MoveIt 的总执行时限并非同一个参数。修改超时阈值前,必须先确认轨迹本身的时间参数化和机器人运行速度是否合理。
读取执行反馈并完成复测
控制器接受目标后,可以通过状态主题判断是否真正发生关节运动。先确认 Panda 轨迹控制器发布的反馈消息类型:
ros2 topic type /panda_arm_controller/controller_state预期输出:
control_msgs/msg/JointTrajectoryControllerState随后执行 ros2 topic echo /panda_arm_controller/controller_state --once,检查实际收到的关节名称、目标状态、实际状态与误差。应在控制器正在运行且主题有发布者时使用;如果一直没有消息,先检查控制器生命周期及发布情况。
同时可以使用 ros2 topic echo /joint_states --once 检查关节名称、位置和消息时间戳。预期能够读到 Panda 对应关节的状态;具体数值取决于当前姿态,不应与示例中的固定角度机械比较。
按照以下顺序完成一次低速仿真复测:
- 确认
panda_arm_controller和joint_state_broadcaster均为active,动作服务器存在。 - 确认 MoveIt 控制器映射与实际关节列表一致,
/joint_states持续更新。 - 在 RViz 的 MotionPlanning 面板中规划一个幅度较小、无碰撞的 Panda 关节运动目标,先检查轨迹预览。
- 执行前重新核对当前关节姿态与规划起点,避免使用长时间前生成的轨迹。
- 执行期间观察控制器状态反馈,并记录动作目标是否被接受、最终结果和错误信息。
- 验收时确认动作执行成功、关节实际位置接近规划终点,而且没有出现异常中止或超时取消。
如果仍然失败,应保留同一次执行的 move_group 日志、控制器日志、实际关节状态和动作结果,按时间顺序对齐,不能只截取最后一行报错。
真实机械臂还需要独立检查急停、安全模式、驱动使能和控制权限。即使 MoveIt 返回成功,也不能代替硬件侧的运动安全验证。最可靠的修复验收是:动作目标被接受、运行反馈持续更新、关节到达预期位置,并且整条执行链没有异常取消或中止。