机器人 / 系统课时

排查控制器拒绝执行轨迹

排查控制器拒绝执行轨迹

MoveIt 2 规划成功,但点击执行后机械臂不动,可能是控制器未激活、动作接口无法连接、轨迹目标被拒绝,或执行过程中触发容差和超时保护。本课以 ROS 2 Jazzy 的 Panda 七轴机械臂为例,建立从控制器状态到轨迹反馈的完整诊断顺序。

学习目标

  • 使用 ROS 2 命令检查控制器生命周期、动作服务器和关节映射。
  • 验证轨迹关节名称、时间字段及起始状态是否满足执行条件。
  • 根据动作结果、控制器反馈和 MoveIt 日志区分拒绝、中止与超时,并验证修复效果。

先判断执行失败发生在哪一层

MoveIt 2 的 plan() 成功只说明找到了符合规划约束的轨迹,并不表示控制器已经接受执行请求。真正执行时,还要经过 MoveIt 的轨迹执行管理器、控制器适配层和 ros2_control 的动作服务器。

正常情况下,MoveGroupInterface::execute() 将轨迹交给 move_group 中的 TrajectoryExecutionManager。执行管理器检查起始状态并选择控制器,通过 FollowJointTrajectory 动作接口提交目标。控制器接受后,驱动硬件或模拟硬件,并通过动作反馈报告执行进度。

MoveIt 2 轨迹执行链路与控制器拒绝分支

诊断时先记录失败发生在哪个阶段:

  1. 未发送轨迹:MoveIt 找不到覆盖目标关节的控制器,或当前关节状态与轨迹起点相差过大。
  2. 目标直接被拒绝:动作服务器存在,但控制器认为目标不合法,例如关节集合、时间戳或轨迹字段错误。
  3. 接受后中止:动作目标已被接受,但运行时违反路径容差或终点容差。
  4. 执行被取消:MoveIt 监控到执行时间超限,或者用户、上层任务主动取消。

检查控制器激活状态与动作接口

先在已经启动 Panda 仿真系统的 ROS 2 Jazzy 终端运行以下命令。终端应已加载对应的 ROS 环境;示例使用默认的 /controller_manager,自定义命名空间时需要调整控制器管理器路径。

bash
# 查询已加载控制器的类型和生命周期状态
ros2 control list_controllers

预期输出(Panda 示例,空白和其他控制器因环境而异):

text
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 检查关节命令接口是否可用及被正确认领。不要在硬件接口冲突时强行激活多个控制器。

随后查询轨迹动作服务器:

bash
# 确认 Panda 机械臂的动作名称与接口类型
ros2 action list -t | grep '/panda_arm_controller/follow_joint_trajectory'

预期输出:

text
/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:

yaml
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 关节列表:

bash
# 检查 MoveIt 实际读取的控制器名称
ros2 param get /move_group moveit_simple_controller_manager.controller_names
# 检查轨迹控制器负责的关节
ros2 param get /panda_arm_controller joints

预期输出(使用上述配置):

text
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 轨迹控制器发布的反馈消息类型:

bash
ros2 topic type /panda_arm_controller/controller_state

预期输出:

text
control_msgs/msg/JointTrajectoryControllerState

随后执行 ros2 topic echo /panda_arm_controller/controller_state --once,检查实际收到的关节名称、目标状态、实际状态与误差。应在控制器正在运行且主题有发布者时使用;如果一直没有消息,先检查控制器生命周期及发布情况。

同时可以使用 ros2 topic echo /joint_states --once 检查关节名称、位置和消息时间戳。预期能够读到 Panda 对应关节的状态;具体数值取决于当前姿态,不应与示例中的固定角度机械比较。

按照以下顺序完成一次低速仿真复测:

  1. 确认 panda_arm_controller 和 joint_state_broadcaster 均为 active,动作服务器存在。
  2. 确认 MoveIt 控制器映射与实际关节列表一致,/joint_states 持续更新。
  3. 在 RViz 的 MotionPlanning 面板中规划一个幅度较小、无碰撞的 Panda 关节运动目标,先检查轨迹预览。
  4. 执行前重新核对当前关节姿态与规划起点,避免使用长时间前生成的轨迹。
  5. 执行期间观察控制器状态反馈,并记录动作目标是否被接受、最终结果和错误信息。
  6. 验收时确认动作执行成功、关节实际位置接近规划终点,而且没有出现异常中止或超时取消。

如果仍然失败,应保留同一次执行的 move_group 日志、控制器日志、实际关节状态和动作结果,按时间顺序对齐,不能只截取最后一行报错。

真实机械臂还需要独立检查急停、安全模式、驱动使能和控制权限。即使 MoveIt 返回成功,也不能代替硬件侧的运动安全验证。最可靠的修复验收是:动作目标被接受、运行反馈持续更新、关节到达预期位置,并且整条执行链没有异常取消或中止。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

排查控制器拒绝执行轨迹

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通