六轴机械臂 MoveIt2 抓取实录: 从 URDF 建模、ros2_control 对齐到运动规划落地踩坑

很多做自动化或者具身智能的朋友，跑完 Gazebo 里的 Demo 感觉抓取流程闭环了，但一拉到真机上，往往会卡在几个经典问题上：末端到位了但解算抽搐、夹爪刚碰到工件就报碰撞、或者逆运动学直接返回无解。

这篇博客把我们团队在 ROS 2 Humble 下调试一台六轴机械臂（配气动两指夹爪）做静态抓取的全流程整理出来。内容尽量剔除废话，直接给可落地的步骤、配置示例和现场踩坑 Checklist。

一、URDF / Xacro 建模：别把视觉模型直接当碰撞体

从 CAD 导出的 STL 文件如果直接塞进 URDF，MoveIt2 的规划速度会直接降到几帧甚至卡死。

1. 碰撞模型做减面与包围体化

• visual 标签可以用高精度网格，用于 RViz 可视化。
• collision 标签严禁直接复用复杂的 CAD 模型。尽量用基础几何体（box, cylinder）包裹；如果是曲面结构，用 Blender 做一次 Decimate 减面，将面片数压缩到 500 个三角面以内。

2. 末端坐标系的定义规范

很多开源机械臂的 URDF 只定义到末端法兰（link6 或 tool0）。抓取任务必须严格拆分以下坐标系：

• tool0：法兰盘物理接触面（遵循 ROS REP 103 标准，Z 轴朝外）。
• grasp_frame：虚拟的抓取中心点（通常在两指夹爪闭合时的指尖几何中心）。

<!-- 在 xacro 中定义一个独立的 grasp_frame -->
<link name="grasp_frame"/>
<joint name="flange_to_grasp" type="fixed">
  <parent link="tool0"/>
  <child link="grasp_frame"/>
  <!-- 假设指尖中心距离法兰沿 Z 轴偏移 15cm -->
  <origin xyz="0 0 0.15" rpy="0 0 0"/>
</joint>

在规划逆运动学（IK）时，抓取位姿的目标直接给 grasp_frame，能省去大量在业务代码里手推 TF 偏移的麻烦。

二、ROS 2 Control 与 MoveIt2 协同配置

MoveIt2 通过 FollowJointTrajectory Action 与 ros2_control 交互。两边配置不一致是导致“规划成功但在真机上完全不动”的最常见原因。

1. moveit_controllers.yaml 映射

确保 MoveIt 控制器配置文件中的名字与 ros2_control 的 Controller 实例完全对齐：

moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

moveit_simple_controller_manager:
  controller_names:
    - arm_controller
    - gripper_controller

  arm_controller:
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6

  gripper_controller:
    action_ns: gripper_action
    type: GripperCommand
    default: true
    joints:
      - finger_joint

2. 轨迹容差配置

真机由于机械间隙或底层驱动延迟，很难做到微秒级的无误差跟随。在 ros2_control 的 YAML 里，给 trajectory 和 goal 适当留出容差，否则驱动节点会直接抛出 GOAL_TOLERANCE_VIOLATED 导致机械臂急停。

arm_controller:
  ros__parameters:
    constraints:
      goal_time: 0.5
      joint1: { trajectory: 0.05, goal: 0.02 }
      joint2: { trajectory: 0.05, goal: 0.02 }
      # 其余关节同理

三、抓取流程的典型设计：进刀与退刀

不要直接指挥机械臂从待机点一路规划到抓取姿态。一旦目标在桌面上，由于障碍物靠近，全局采样算法很容易生成出贴着桌面飞行的危险轨迹。

通用的逻辑是分三段：

1. Pre-grasp 位姿：目标位姿沿工具 Z 轴（接近方向）后退 10-15 cm。采用全局规划（OMPL）。
2. Grasp 位姿（进刀）：从 Pre-grasp 直线匀速下探至 Grasp。采用笛卡尔路径规划（computeCartesianPath）。
3. Post-grasp 位姿（提刀）：夹爪闭合后，沿绝对坐标系 Z 轴向上平移 10 cm。

以下为基于 C++ moveit::planning_interface::MoveGroupInterface 的笛卡尔进刀逻辑骨架：

#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/pose.hpp>
#include <vector>

bool executeCartesianApproach(
    moveit::planning_interface::MoveGroupInterface& move_group,
    const geometry_msgs::msg::Pose& target_grasp_pose,
    double approach_distance)
{
    // 1. 设置末端链接为我们在 URDF 中定义的虚拟点
    move_group.setEndEffectorLink("grasp_frame");

    // 2. 构造进刀路径点列表
    std::vector<geometry_msgs::msg::Pose> waypoints;
    
    // 假设从当前位姿直线走向 target_grasp_pose
    waypoints.push_back(target_grasp_pose);

    moveit_msgs::msg::RobotTrajectory trajectory;
    const double jump_threshold = 0.0; // 严格禁止关节突跳
    const double eef_step = 0.005;     // 5mm 插值步长

    double fraction = move_group.computeCartesianPath(
        waypoints, eef_step, jump_threshold, trajectory);

    if (fraction < 0.95) {
        RCLCPP_WARN(rclcpp::get_logger("GraspPipeline"), 
                    "Cartesian path planning incomplete: %.2f%%", fraction * 100.0);
        return false;
    }

    // 3. 执行轨迹
    move_group.execute(trajectory);
    return true;
}

四、真机落地踩坑清单 (Checklist)

这里汇总现场调试中最容易耗费时间的几个坑：

1. 默认 KDL 求解器频繁无解（换 PickIK 或 TRAC-IK）

ROS 2 MoveIt2 默认的数值求解器 kdl_kinematics_plugin/KDLKinematicsPlugin 在靠近奇异点或关节限位时表现较差，经常出现明明有解却报 Timeout。

• 排查建议：在六轴臂工程中，优先使用 pick_ik（支持代价值函数约束与避限位）或移植后的 trac_ik。在 kinematics.yaml 中将 kinematics_solver_timeout 提高到 0.05 秒，并将解算模式配置为靠近当前关节状态（Position-only 或带局部优化）。

2. 闭合夹爪时触发自碰撞报错

当夹爪闭合抓取物体，或者两指接触时，MoveIt 会把这一动作判定为自碰撞（Self-collision），直接拒绝后续的提刀规划。

• 解决方法：
  1. 使用 MoveIt Setup Assistant 重新生成 SRDF 时，适当调整两指之间的碰撞检测矩阵（ACM, Allowed Collision Matrix）。
  2. 在抓取成功的瞬间，通过 PlanningScene 接口动态挂载物体（attachObject），告知 MoveIt 该物体此时已成为夹爪的一部分，同时暂时忽略该物体与夹爪内侧指垫之间的碰撞。

3. 笛卡尔规划中的关节突跳（Joint Jump）

computeCartesianPath 算出来的位姿在笛卡尔空间是平滑直线的，但由于六轴臂有多个逆解分支，如果没做好约束，第 4 轴或第 6 轴可能会在某一帧瞬间反转 180 度。

• 解决方法： 把 jump_threshold 设为一个严格的非零浮点数（例如 1.5 到 2.0，代表相邻插值点之间关节弧度变化的比值），或在执行前遍历 trajectory.joint_trajectory.points，检查相邻点之间是否有超过物理极限的速度突变。

4. 轨迹执行时机械臂抖动（时间参数化算法选择）

MoveIt2 默认采用 TOTG（Time-Optimal Trajectory Generation）进行速度规划。但在硬件刚度较弱或控制器周期略有波动的机型上，TOTG 容易生成加速度跃变，导致真机抖动。

• 排查建议：在 MoveIt2 中切换为 Ruckig 算法。Ruckig 支持三阶轨迹生成（Jerk-limited，加加速度受限），生成的轨迹在真机执行时更柔和，能明显改善进刀和提刀阶段的末端震颤。在 MoveIt 配置的 joint_limits.yaml 中，务必给每个关节明确填入合理的 has_jerk_limits: true 以及 max_jerk。

把 URDF 的碰撞体简化、grasp_frame 规范化、控制器容差合理放开，再配合带进刀段的笛卡尔规划与 Ruckig 算法，一套稳定可靠的六轴抓取系统基本就能在真机上跑通了。后续如果有上游视觉接入的需求，只需要通过 tf2_ros 把检测到的物体位姿变换到机械臂基坐标系（base_link），输入给进刀逻辑即可。