10. MoveIt 2 基础与运动规划

有了 URDF 模型，机械臂能在 RViz2 里动起来了。但"动起来"和"规划运动"是两回事——这一讲用 MoveIt Setup Assistant 生成配置包，修复配置文件，触发第一次运动规划。

前言：有了模型，还缺什么？

上一讲我们有了 URDF 模型，机械臂能在 RViz2 里动起来了。但"动起来"和"规划运动"是两回事：

动起来：给关节角，模型跟着显示
规划运动：给末端目标位置，系统自动算出一条无碰撞路径

后者需要 MoveIt 2。

你说：把末端移到某个位置
    ↓
MoveIt 2：
  1. 用 IK 求解关节角
  2. 检查路径上有没有碰撞
  3. 生成平滑轨迹
  4. 发布执行指令
    ↓
机械臂运动

MoveIt 2 不是一个算法，而是把这些能力组合在一起的框架。

10.1 MoveIt 2 的核心组件

10.1.1 组件关系图

用户 / 规划客户端
        ↓ Action (MoveGroup)
   move_group 节点
   ├── 规划插件 (OMPL)      ← 路径规划算法
   ├── IK 插件 (KDL)        ← 逆运动学求解
   ├── Planning Scene       ← 机器人状态 + 障碍物
   └── 执行接口             ← 发布轨迹给控制器
        ↑
   URDF + SRDF（模型和语义）
   TF 树（坐标系）
   /joint_states（当前状态）

10.1.2 各组件职责

组件	职责
URDF	机器人几何结构（link、joint、尺寸）
SRDF	规划语义（规划组、末端执行器、默认姿态）
`move_group`	核心节点，接收规划请求，协调各插件
Planning Scene	维护机器人当前状态和环境障碍物
OMPL	路径规划算法库（MoveIt 2 默认调用）
KDL	默认 IK 求解器

10.1.3 URDF vs SRDF

这是最容易混淆的概念：

	URDF	SRDF
描述什么	机器人长什么样	机器人怎么用于规划
内容	link、joint、几何、惯量	规划组、末端执行器、禁用碰撞
谁生成	手写或 xacro	MoveIt Setup Assistant 生成
类比	身体结构	使用说明书

10.2 安装 MoveIt 2

sudo apt install -y ros-jazzy-moveit ros-jazzy-ros2-controllers ros-jazzy-ros2-control

验证安装：

ros2 pkg list | grep moveit

应该看到 moveit_ros_planning、moveit_ros_move_group 等包。

10.3 给 URDF 添加 ros2_control 标签

MoveIt 2 通过 ros2_control 框架与控制器通信。我们使用 mock_components/GenericSystem 作为假硬件，让机械臂在 RViz2 里真正动起来，而不需要真实硬件。

这一步是在第 9 讲已有的 two_link_arm.urdf 基础上追加内容，不是新建文件。打开文件：

vim $ROS_WS/src/my_robot_description/urdf/two_link_arm.urdf

找到文件末尾的 </robot> 标签，在它前面插入以下内容（即 </robot> 仍保持在文件最后一行）：

  <!-- ros2_control：使用 mock hardware，让控制器能接管关节状态 -->
  <ros2_control name="two_link_arm" type="system">
    <hardware>
      <plugin>mock_components/GenericSystem</plugin>
    </hardware>
    <joint name="joint1">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint2">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>

重新构建描述包：

cd $ROS_WS
colcon build --packages-select my_robot_description
source install/setup.bash

10.4 用 MoveIt Setup Assistant 生成配置包

10.4.1 启动 Setup Assistant

必须先 cd 到工作空间并 source，否则 Setup Assistant 找不到你的包：

cd $ROS_WS
source install/setup.bash
ros2 launch moveit_setup_assistant setup_assistant.launch.py

如果没有 source，会报错：

package 'my_robot_description' not found, searching: [/opt/ros/jazzy]

10.4.2 导入 URDF

选择 Create New MoveIt Configuration Package
点击 Browse，选择 $ROS_WS/src/my_robot_description/urdf/two_link_arm.urdf
点击 Load Files

右侧应该显示机械臂模型。

10.4.3 填写作者信息

点击左侧 Author Information，填写 name 和 email（随意填，如 user / user@example.com）。

不填会导致生成的 package.xml 邮箱为空，colcon build 报错：

Maintainers must have an email address

10.4.4 配置自碰撞矩阵

点击 Self-Collisions → Generate Collision Matrix

这一步会自动检测哪些 link 对之间不可能发生碰撞（如相邻 link），并禁用这些检测以提高规划速度。

10.4.5 创建规划组

点击 Planning Groups → Add Group：

Group Name：arm
Kinematic Solver：kdl_kinematics_plugin/KDLKinematicsPlugin
Group Default Planner：RRTConnect

点击 Add Joints，把 joint1 和 joint2 加入组，点击 Save。

再点击 Add Chains，填写：

Base Link：base_link
Tip Link：end_effector

点击 Save。

必须同时加 Joints 和 Chain。 只加 Chain 时，MoveIt 会因为 ee_joint 是 fixed 类型而认为组里没有可动关节，报 Group 'arm' must have at least one valid joint 并导致生成失败。Joints 让 MoveIt 知道哪些关节参与规划，Chain 让 MoveIt 知道运动链的起止点（用于 IK 求解和末端位置计算）。

跳过 End Effectors 步骤。 对于两连杆平面臂不需要配置，填写会导致 Group 'arm' cannot be its own parent 报错。

10.4.6 配置 IK 超时

点击左侧 Planning Groups，选中 arm 组，点击 Edit Selected。

找到 Kinematic Solver 配置区域，把 Solver Timeout 从默认的 0.005 改为 0.1。

点击 Save。

默认 0.005s 太短，拖动 RViz2 里的交互标记时 IK 求解会频繁失败，标记弹回原位。

10.4.7 添加默认姿态

点击 Robot Poses → Add Pose：

Pose Name：home
把两个关节角都拖到 0

10.4.8 生成配置包

点击左侧 Configuration Files：

Configuration Package Save Path：填入 $ROS_WS/src/my_robot_moveit_config 对应的绝对路径（在终端执行 echo $ROS_WS/src/my_robot_moveit_config 可查看），此处必须填绝对路径，不能用 ~ 或环境变量，否则会生成到错误位置
点击 Generate Package

生成内容包括：config/（SRDF、kinematics.yaml、joint_limits.yaml 等）、launch/（demo.launch.py 等）、package.xml。

10.5 补充和修复配置文件

Setup Assistant 生成后还需要手动处理以下三项。

10.5.1 修复 joint_limits.yaml

Jazzy 版本的 Setup Assistant 没有 Joint Limits 配置界面，生成的文件速度值是整数且缺少加速度限制，move_group 启动时会报错：

parameter 'joint_limits.joint1.max_velocity' has invalid type: expected [double] got [integer]
No acceleration limit was defined for joint joint1!

vim $ROS_WS/src/my_robot_moveit_config/config/joint_limits.yaml

joint_limits 部分改为：

joint_limits:
  joint1:
    has_velocity_limits: true
    max_velocity: 1.0
    has_acceleration_limits: true
    max_acceleration: 1.0
  joint2:
    has_velocity_limits: true
    max_velocity: 1.0
    has_acceleration_limits: true
    max_acceleration: 1.0

10.5.2 创建 ros2_controllers.yaml

Setup Assistant 不会自动生成控制器配置，需要手动创建。

注意：config/ 目录下只能有一个 *_controllers.yaml 文件，否则 MoveIt 启动时会报 Unable to guess which parameter file to load。

vim $ROS_WS/src/my_robot_moveit_config/config/ros2_controllers.yaml

controller_manager:
  ros__parameters:
    update_rate: 100

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

arm_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    allow_partial_joints_goal: false

10.5.3 创建 demo_sim.launch.py

Setup Assistant 生成的 demo.launch.py 依赖完整的 ros2_control 硬件接口，对教学场景过于复杂。我们创建一个简化版：

vim $ROS_WS/src/my_robot_moveit_config/launch/demo_sim.launch.py

from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch, generate_moveit_rviz_launch
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    moveit_config = (
        MoveItConfigsBuilder("two_link_arm", package_name="my_robot_moveit_config")
        .to_moveit_configs()
    )

    # robot_state_publisher：发布 URDF 和 TF 树
    rsp_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[moveit_config.robot_description],
    )

    # ros2_control_node：加载 mock hardware，接管关节状态
    ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            moveit_config.robot_description,
            str(moveit_config.package_path / "config/ros2_controllers.yaml"),
        ],
    )

    # spawner：等 controller_manager 就绪后自动激活控制器
    joint_state_broadcaster_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_state_broadcaster"],
    )

    arm_controller_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["arm_controller"],
    )

    # move_group：MoveIt 2 核心节点
    move_group = generate_move_group_launch(moveit_config)

    # RViz2 + MotionPlanning 插件
    rviz = generate_moveit_rviz_launch(moveit_config)

    return LaunchDescription(
        [rsp_node, ros2_control_node, joint_state_broadcaster_spawner, arm_controller_spawner]
        + move_group.entities
        + rviz.entities
    )

10.6 构建并启动

cd $ROS_WS
colcon build --packages-select my_robot_description my_robot_moveit_config
source install/setup.bash
ros2 launch my_robot_moveit_config demo_sim.launch.py

启动成功后终端应该看到：

[move_group-3] [INFO]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-3] [INFO]: MoveGroup context initialization complete

10.6.1 在 RViz2 中触发规划

RViz2 启动后，左侧面板有 MotionPlanning 插件。

步骤 1：确认规划组

在左侧 Displays 面板里展开 MotionPlanning → Planning Request，找到 Planning Group，确认值为 arm。

步骤 2：设置目标

切换到顶部 MotionPlanning 面板的 Planning 子标签页，在 Goal State 下拉菜单里选择：

<random>：随机生成一个合法目标姿态，适合验证规划是否工作
home：回到 SRDF 里定义的零位姿态

3D 视口里也有交互式坐标轴标记可以拖动，但对于这个平面臂，KDL IK 求解器在 3D 空间里解不稳定，拖动后标记容易弹回原位。推荐用下拉菜单选目标。

步骤 3：规划并执行

点击 Plan & Execute。

规划成功后，3D 视口里会出现一条橙黄色的轨迹线，机械臂随即沿路径运动到目标位置。

如果提示 Goal constraints are already satisfied，说明当前状态已经是目标状态，换一个不同的 Goal State 再试。

10.6.2 用 CLI 验证 move_group 在运行

# 新开终端
ros2 action list | grep move_group

应该看到 /sequence_move_group 等 Action，说明 move_group 节点正在运行。

10.7 用 Python 调用规划接口

前提：10.6 的 demo_sim.launch.py 必须保持运行，不要关闭。本节的 Python 节点依赖 move_group 提供的规划服务，demo_sim.launch.py 就是启动 move_group 的。

终端分工：

终端 1：ros2 launch my_robot_moveit_config demo_sim.launch.py（保持运行）

终端 2：本节的构建和运行命令

10.7.1 安装 moveit_py

sudo apt install -y ros-jazzy-moveit-py

10.7.2 创建规划客户端包

cd $ROS_WS/src
ros2 pkg create --build-type ament_python my_robot_planner

10.7.3 编写规划节点

vim $ROS_WS/src/my_robot_planner/my_robot_planner/planner_node.py

import rclpy
from rclpy.node import Node
from moveit.planning import MoveItPy, PlanRequestParameters
from moveit.core.robot_state import RobotState
import math


class PlannerNode(Node):
    def __init__(self):
        super().__init__('planner_node')
        self.get_logger().info('规划节点启动，初始化 MoveIt...')

        self.moveit = MoveItPy(node_name='planner_node')
        self.arm = self.moveit.get_planning_component('arm')

        self.get_logger().info('MoveIt 初始化完成，2 秒后开始规划...')
        self.timer = self.create_timer(2.0, self.plan_and_execute)

    def plan_and_execute(self):
        self.timer.cancel()

        self.arm.set_start_state_to_current_state()

        robot_state = RobotState(self.moveit.get_robot_model())
        robot_state.set_joint_group_positions(
            'arm',
            [math.radians(30), math.radians(45)]
        )
        self.arm.set_goal_state(robot_state=robot_state)

        self.get_logger().info('开始规划...')
        # 必须显式指定规划 pipeline，否则报 "No planning pipeline available"
        plan_params = PlanRequestParameters(self.moveit, 'ompl')
        plan_params.planning_pipeline = 'ompl'
        plan_params.planner_id = 'RRTConnect'
        plan_result = self.arm.plan(single_plan_parameters=plan_params)

        if plan_result:
            self.get_logger().info('规划成功，执行中...')
            # 必须指定控制器名，否则报 "No active controllers configured"
            self.moveit.execute(plan_result.trajectory, controllers=["arm_controller"])
            self.get_logger().info('执行完成')
        else:
            self.get_logger().warn('规划失败')


def main(args=None):
    rclpy.init(args=args)
    node = PlannerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

10.7.4 编写 launch 文件

MoveItPy 初始化时需要从参数服务器读取规划 pipeline 配置，必须通过 launch 文件注入，不能直接 ros2 run。

mkdir -p $ROS_WS/src/my_robot_planner/launch
vim $ROS_WS/src/my_robot_planner/launch/planner.launch.py

from moveit_configs_utils import MoveItConfigsBuilder
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    moveit_config = (
        MoveItConfigsBuilder("two_link_arm", package_name="my_robot_moveit_config")
        .to_moveit_configs()
    )

    # MoveItPy 底层（MoveItCpp）读取的是 planning_pipelines.pipeline_names，
    # 而 moveit_config.planning_pipelines 传的 key 是 planning_pipelines（列表），
    # 两者不匹配，需要手动构造正确格式的参数字典。
    pp = moveit_config.planning_pipelines
    pipeline_params = {
        "planning_pipelines.pipeline_names": ["ompl"],
        "default_planning_pipeline": "ompl",
    }
    for k, v in pp.get("ompl", {}).items():
        if k != "planner_configs":
            pipeline_params[f"ompl.{k}"] = v

    planner_node = Node(
        package="my_robot_planner",
        executable="planner_node",
        output="screen",
        parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
            moveit_config.trajectory_execution,
            pipeline_params,
        ],
    )

    return LaunchDescription([planner_node])

10.7.5 配置 setup.py 和 package.xml

编辑 my_robot_planner 包的配置文件：

vim $ROS_WS/src/my_robot_planner/setup.py

data_files 里注册 launch 目录，entry_points 里注册节点：

data_files=[
    ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
    ('share/' + package_name, ['package.xml']),
    ('share/' + package_name + '/launch', ['launch/planner.launch.py']),
],
...
entry_points={
    'console_scripts': [
        'planner_node = my_robot_planner.planner_node:main',
    ],
},

再编辑 package.xml：

vim $ROS_WS/src/my_robot_planner/package.xml

加入依赖：

<exec_depend>rclpy</exec_depend>
<exec_depend>moveit_py</exec_depend>
<exec_depend>moveit_configs_utils</exec_depend>

10.7.6 构建并运行

cd $ROS_WS
colcon build --packages-select my_robot_planner
source install/setup.bash

终端 1（保持运行）：

ros2 launch my_robot_moveit_config demo_sim.launch.py

终端 2（等 RViz2 完全启动后）：

source install/setup.bash
ros2 launch my_robot_planner planner.launch.py

成功输出：

[INFO] [planner_node]: MoveIt 初始化完成，2 秒后开始规划...
[INFO] [planner_node]: 开始规划...
[INFO] [planner_node]: 规划成功，执行中...
[INFO] [planner_node]: 执行完成

注意：执行时终端可能出现 Execution failed! No active controllers configured for group 'arm' 的 ERROR 日志，但机械臂实际已经运动。这是 Jazzy 版本 moveit_cpp 执行接口的已知问题，不影响功能，忽略即可。

RViz2 里可以看到机械臂运动到 joint1=30°、joint2=45° 的位置。

10.8 练习题

10.8.1 练习 1：用 RViz2 规划到 home 姿态

在 Goal State 下拉菜单里选择 home，点击 Plan & Execute，观察机械臂回到零位。

10.8.2 练习 2：修改目标关节角

修改 planner_node.py 里的目标角度，改为 joint1=60°, joint2=-30°，重新 build 并运行，观察机械臂运动到新位置。

10.8.3 练习 3：梳理组件依赖关系

用自己的话回答：

move_group 启动需要哪些前置条件？
SRDF 里的 planning group 和 URDF 里的 joint 是什么关系？
为什么规划前要调用 set_start_state_to_current_state()？
为什么 planner_node 必须用 ros2 launch 而不能直接 ros2 run？

本讲核心总结

概念	一句话理解
MoveIt 2	机械臂运动规划框架，整合 IK、路径规划、碰撞检测
SRDF	在 URDF 之上补充规划语义，定义规划组和末端执行器
`move_group`	MoveIt 2 的核心节点，接收规划请求，协调各插件
Planning Scene	维护机器人当前状态和环境障碍物，是规划的"世界模型"
Setup Assistant	图形化工具，从 URDF 生成 MoveIt 配置包
`moveit_py`	Python 侧的 MoveIt 规划 API

参考代码

本讲对应的完整参考代码位于：

ros_ws/reference/src/my_robot_planner/
├── my_robot_planner/planner_node.py    ← 10.7 Python 规划节点
├── launch/planner.launch.py            ← 10.7 注入 MoveIt 参数的 Launch
├── package.xml
└── setup.py

注意：my_robot_moveit_config 包需要通过 MoveIt Setup Assistant 生成，不在参考代码中预置。本讲 10.5 详细说明了生成后需要手动修复的几处配置（package.xml 邮箱、joint_limits.yaml 类型、kinematics.yaml 超时、SRDF 运动链、ros2_controllers.yaml）。

使用方式：

cp $ROS_WS/reference/src/my_robot_planner/my_robot_planner/planner_node.py \
   $ROS_WS/src/my_robot_planner/my_robot_planner/
cp $ROS_WS/reference/src/my_robot_planner/launch/planner.launch.py \
   $ROS_WS/src/my_robot_planner/launch/

下一讲预告

11. MoveIt 2 仿真项目实战

本讲我们已经能触发单次规划了。下一讲我们会把前面所有知识串成一个完整系统：

整理项目包结构（description、moveit_config、planner、bringup）
写一个接收 Topic 目标点、自动触发规划的节点
加入障碍物，验证避障效果
写一个一键启动整个系统的 Launch 文件
完成从"给目标点"到"看到机械臂运动"的完整闭环

前言：有了模型，还缺什么？​

10.1 MoveIt 2 的核心组件​

10.1.1 组件关系图​

10.1.2 各组件职责​

10.1.3 URDF vs SRDF​

10.2 安装 MoveIt 2​

10.3 给 URDF 添加 ros2_control 标签​

10.4 用 MoveIt Setup Assistant 生成配置包​

10.4.1 启动 Setup Assistant​

10.4.2 导入 URDF​

10.4.3 填写作者信息​

10.4.4 配置自碰撞矩阵​

10.4.5 创建规划组​

10.4.6 配置 IK 超时​

10.4.7 添加默认姿态​

10.4.8 生成配置包​

10.5 补充和修复配置文件​

10.5.1 修复 joint_limits.yaml​

10.5.2 创建 ros2_controllers.yaml​

10.5.3 创建 demo_sim.launch.py​

10.6 构建并启动​

10.6.1 在 RViz2 中触发规划​

10.6.2 用 CLI 验证 move_group 在运行​

10.7 用 Python 调用规划接口​

10.7.1 安装 moveit_py​

10.7.2 创建规划客户端包​

10.7.3 编写规划节点​

10.7.4 编写 launch 文件​

10.7.5 配置 setup.py 和 package.xml​

10.7.6 构建并运行​

10.8 练习题​

10.8.1 练习 1：用 RViz2 规划到 home 姿态​

10.8.2 练习 2：修改目标关节角​

10.8.3 练习 3：梳理组件依赖关系​

本讲核心总结​

参考代码​

下一讲预告​