🦾 ROS2 实战进阶:URDF 与 MoveIt2 机械臂仿真

1 URDF 深入:link 与 joint 的完整语法

URDF(Unified Robot Description Format,统一机器人描述格式)是 ROS 描述机器人几何、运动学、动力学的 XML 文件。人形机器人的每一个关节模组(髋、膝、踝、肩、肘、腕)在 URDF 里最终都落成两样东西:link(连杆,刚体)和 joint(关节,连接两个连杆的约束)。

1.1 link:一个刚体,三种几何

一个 <link> 通常包含三块内容,它们的几何可以各不相同,这是初学者最容易混的点:

三者的 <origin> 都相对该 link 自身坐标系描述;而 link 之间的相对位姿则由 <joint> 里的 <origin> 定义。

1.2 joint:六种类型与关键字段

joint 通过 type 指定约束类型,并通过 parent / child 确定父子关系(child 相对 parent 运动,这是 TF 树的根)。常见六种:

一个 revolute 关节的关键字段:<origin>(关节坐标系相对父连杆的位姿)、<axis>(旋转轴单位向量,如 0 0 1 表示绕 Z 轴)、<limit>(lower/upper 限位、effort 最大力矩、velocity 最大速度)。

1.3 从零写一个 2 连杆机械臂(纯 URDF)

下面是一个 2 自由度的平面机械臂:基座 + 两节绕 Z 轴旋转的连杆。把下面内容存成 two_link_arm.urdf 即可用于 RViz2 可视化:

<?xml version="1.0"?>
<robot name="two_link_arm">

  <!-- ===== 基座:静止连杆 ===== -->
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder radius="0.06" length="0.12"/>
      </geometry>
      <origin xyz="0 0 0.06" rpy="0 0 0"/>
      <material name="grey">
        <color rgba="0.4 0.4 0.4 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.06" length="0.12"/>
      </geometry>
      <origin xyz="0 0 0.06" rpy="0 0 0"/>
    </collision>
    <inertial>
      <origin xyz="0 0 0.06" rpy="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
  </link>

  <!-- ===== 关节 1:base_link --> link1,绕 Z 轴 -->
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.12" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="3.14"/>
  </joint>

  <link name="link1">
    <visual>
      <geometry>
        <box size="0.08 0.08 0.3"/>
      </geometry>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="0.2 0.5 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.08 0.08 0.3"/>
      </geometry>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
    </collision>
    <inertial>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
  </link>

  <!-- ===== 关节 2:link1 --> link2 -->
  <joint name="joint2" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.3" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="3.14"/>
  </joint>

  <link name="link2">
    <visual>
      <geometry>
        <box size="0.06 0.06 0.25"/>
      </geometry>
      <origin xyz="0 0 0.125" rpy="0 0 0"/>
      <material name="red">
        <color rgba="1.0 0.3 0.3 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.06 0.06 0.25"/>
      </geometry>
      <origin xyz="0 0 0.125" rpy="0 0 0"/>
    </collision>
    <inertial>
      <origin xyz="0 0 0.125" rpy="0 0 0"/>
      <mass value="0.6"/>
      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
    </inertial>
  </link>

</robot>

1.4 xacro:把 URDF 参数化、复用

URDF 写长了会又臭又长:xacro(XML 宏)给它加上了常量、变量、数学运算、宏函数、文件包含。核心用法:

• <xacro:property name=".." value=".."/> —— 定义常量;
• ${表达式} —— 引用常量或做运算(如 ${link1_len / 2});
• <xacro:macro name=".." params="..">...</xacro:macro> —— 定义宏(函数);
• <xacro:include filename=".."/> —— 包含其它 xacro 文件;
• 调用宏:<xacro:宏名 参数="值"/>。

把上面的 2 连杆臂改写成 xacro(节选核心部分):

<?xml version="1.0"?>
<robot name="two_link_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- 常量:连杆长度,改这里即可整体改变臂长 -->
  <xacro:property name="link1_len" value="0.3"/>
  <xacro:property name="link2_len" value="0.25"/>
  <xacro:property name="radius" value="0.04"/>

  <!-- 宏:生成一节连杆(参数化) -->
  <xacro:macro name="arm_link" params="name length color">
    <link name="${name}">
      <visual>
        <geometry>
          <box size="${2*radius} ${2*radius} ${length}"/>
        </geometry>
        <origin xyz="0 0 ${length/2}" rpy="0 0 0"/>
        <material name="m_${name}">
          <color rgba="${color}"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <box size="${2*radius} ${2*radius} ${length}"/>
        </geometry>
        <origin xyz="0 0 ${length/2}" rpy="0 0 0"/>
      </collision>
      <inertial>
        <origin xyz="0 0 ${length/2}" rpy="0 0 0"/>
        <mass value="${length}"/>
        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
      </inertial>
    </link>
  </xacro:macro>

  <!-- 宏:生成一个 revolute 关节 -->
  <xacro:macro name="arm_joint" params="name parent child z">
    <joint name="${name}" type="revolute">
      <parent link="${parent}"/>
      <child link="${child}"/>
      <origin xyz="0 0 ${z}" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <limit lower="-3.14" upper="3.14" effort="10" velocity="3.14"/>
    </joint>
  </xacro:macro>

  <!-- 调用宏:三行搞定三节 + 两个关节 -->
  <arm_link name="link1" length="${link1_len}" color="0.2 0.5 1.0 1.0"/>
  <arm_link name="link2" length="${link2_len}" color="1.0 0.3 0.3 1.0"/>
  <arm_joint name="joint1" parent="base_link" child="link1" z="${0.12}"/>
  <arm_joint name="joint2" parent="link1" child="link2" z="${link1_len}"/>

</robot>

xacro 文件要用 xacro 工具转成 URDF 才能被下游读取:

xacro two_link_arm.urdf.xacro > two_link_arm.urdf
# 也可以直接转成可视化 PDF 检查(部分发行版需装 graphviz/urdfdom)
check_urdf two_link_arm.urdf        # 校验结构是否合法
urdf_to_graphiz two_link_arm.urdf   # 生成 tf 结构图(可选)

2 RViz2 实操:把 URDF 显示出来

RViz2 只负责"显示",它自己不解析 URDF —— 解析和发布 TF 靠 robot_state_publisher,关节角度靠 joint_state_publisher。完整流程四步:

# ① 转 URDF(可选,便于 check_urdf 校验)
xacro two_link_arm.urdf.xacro > two_link_arm.urdf

# ② 启动 robot_state_publisher,发布模型与 TF
ros2 run robot_state_publisher robot_state_publisher --ros-args \
  -p robot_description:="$(xacro /path/to/two_link_arm.urdf.xacro)"

# ③ 启动 joint_state_publisher_gui,可拖动滑杆改关节角
ros2 run joint_state_publisher_gui joint_state_publisher_gui

# ④ 新开终端,启动 RViz2
rviz2

RViz2 打开后按下面顺序配置(每一步都有坑):

1. 设 Fixed Frame:左侧 Global Options → Fixed Frame 填 base_link(注意大小写、不能有前导 /)。设错会整屏报红或模型消失。
2. 添加 RobotModel:左下角 Add → 按主题(RobotModel)。它默认订阅 /robot_description 话题;若模型不出现,检查话题名是否一致。
3. 添加 TF:Add → TF,可显示所有坐标系与连线,直观看到 base_link → link1 → link2 的树。
4. 添加 Grid:Add → Grid,给一个地面参考,方便看方向和尺度。
5. 保存配置:File → Save Config As,存成 display.rviz,下次用 rviz2 -d display.rviz 直接加载。

3 TF2:机器人的"坐标变换总线"

TF2 维护一棵有向树上所有坐标系之间的相对位姿,并缓存一段时间让异步消息也能对齐时间戳。人形机器人典型的 tf 树:

map ──> odom ──> base_link ──> torso ──> l_arm ──> l_hand ──> l_gripper
                                        └────> r_arm ──> r_hand ──> r_gripper

3.1 三个根级坐标系:map / odom / base_link

为什么要分三层?因为里程计会累积误差(odom → base_link 随运动漂移),而地图需要稳定;定位节点定期发布 map → odom 来"纠正"漂移,从而隔离两种误差源。这是导航栈的标准设计。

3.2 发布静态变换

两坐标系若刚性固定(如给机器人装一个固定位置的激光雷达),用 static_transform_publisher 一次发布即可:

# 形式:xyz + 欧拉角(rpy),再跟 父系 子系
ros2 run tf2_ros static_transform_publisher 0.1 0 0.2 0 0 0 base_link laser

# 或使用命名参数(更不易写错顺序)
ros2 run tf2_ros static_transform_publisher --x 0.1 --y 0 --z 0.2 \
  --roll 0 --pitch 0 --yaw 0 --frame-id base_link --child-frame-id laser

3.3 lookupTransform:查任意两系变换

程序里查变换的核心是 Buffer + TransformListener(负责接收),再调 lookup_transform:

import rclpy
from rclpy.node import Node
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

class TfLookup(Node):
    def __init__(self):
        super().__init__('tf_lookup')
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.timer = self.create_timer(1.0, self.lookup)

    def lookup(self):
        try:
            # 查 base_link 到 link2 的变换,time 用 Time() 表示"取最新"
            t = self.buffer.lookup_transform('base_link', 'link2', rclpy.time.Time())
            pos = t.transform.translation
            self.get_logger().info('link2 相对 base_link: x=%.3f y=%.3f z=%.3f'
                                   % (pos.x, pos.y, pos.z))
        except Exception as e:
            self.get_logger().warn('查询失败(通常系还没发布): %s' % e)

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

if __name__ == '__main__':
    main()

调试命令:ros2 run tf2_ros tf2_echo base_link link2 持续打印变换;view_frames(生成 frames.pdf)可视化整棵树。

4 launch 文件:一个文件拉起一堆节点

真实机器人启动时少则几个、多则几十个节点,不可能手动敲。ROS2 推荐 Python 版 launch 文件(*.launch.py),它其实是普通 Python 脚本,入口函数返回一个 LaunchDescription。核心构件:

一个把 URDF 显示起来的完整多节点示例(display.launch.py):

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    # 参数:运行时可用 use_sim_time:=true 覆盖
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')

    robot_description = Command([
        'xacro ',
        PathJoinSubstitution([
            FindPackageShare('my_arm'),
            'urdf', 'two_link_arm.urdf.xacro',
        ]),
    ])

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value='false',
                              description='是否使用 /clock 仿真时间'),

        # 节点 1:发布模型与 TF
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            parameters=[{'robot_description': robot_description,
                         'use_sim_time': use_sim_time}],
            output='screen',
        ),

        # 节点 2:关节角度(带 GUI 滑杆)
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            name='joint_state_publisher_gui',
        ),

        # 节点 3:可视化
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', PathJoinSubstitution([
                FindPackageShare('my_arm'), 'rviz', 'display.rviz'])],
        ),
    ])

运行:ros2 launch my_arm display.launch.py use_sim_time:=true。若要"分组",把一组 Node 包进 GroupAction([...]) 并可加 namespace;若要"包含",用 IncludeLaunchDescription(PythonLaunchDescriptionSource([...])) 复用别的 launch 文件。

5 ros2_control:让关节真正被"控制"

URDF 只描述"长什么样",真正给每个关节模组下发力矩/位置/速度、并读回编码器反馈的是 ros2_control。它的架构把"控制逻辑"和"硬件驱动"解耦:

• controller_manager:生命周期管理器,负责加载/激活/停用/卸载各个控制器;
• controller(控制器):计算控制律,读写命令/状态接口(如 joint_state_broadcaster、joint_trajectory_controller);
• hardware_interface(硬件接口):真实硬件或仿真器的抽象,把"给关节下命令"翻译成电机驱动,把"编码器读数"上报成状态。人形机器人常用 SystemInterface 派生类实现 export_state_interfaces() 与 export_command_interfaces()。

┌───────────── controller_manager ─────────────┐
│  joint_state_broadcaster   joint_trajectory_controller │
└───────────────┬───────────────────────────────┘
                │ 通过 state/cmd 接口
┌───────────────▼───────────────────────────────┐
│      hardware_interface (SystemInterface)      │
│  真机驱动(CAN/EtherCAT) 或 仿真插件(ign_ros2_control) │
└───────────────────────────────────────────────┘

5.1 joint_state_broadcaster 配置要点

它是最基础的控制器:把硬件读到的关节角度/速度/力矩广播到 /joint_states,供 TF 和 MoveIt2 使用。配置文件(controllers.yaml):

controller_manager:
  ros__parameters:
    update_rate: 50   # 控制循环 Hz

joint_state_broadcaster:
  ros__parameters:
    type: joint_state_broadcaster/JointStateBroadcaster
    publish_rate: 50   # 发布 /joint_states 的频率
    extra_joints: []   # 可额外并入非受控关节

启动与激活(两种方式):

# 方式 A:spawner(推荐,自动处理 lifecycle)
ros2 run controller_manager spawner joint_state_broadcaster

# 方式 B:ros2 control 命令行手动管理
ros2 control load_controller joint_state_broadcaster
ros2 control set_controller_state joint_state_broadcaster active
ros2 control list_controllers   # 查看所有控制器状态

6 MoveIt2:机械臂运动规划与执行

MoveIt2 是 ROS2 生态里最主流的操作与运动规划框架:给定起始位姿和目标位姿,它在避开障碍和自碰撞的前提下算出一条轨迹,再交给控制器执行。核心对象是 MoveGroup(一组关节/规划组的封装)。

6.1 MoveIt Setup Assistant:从 URDF 到配置包

一个交互式 GUI 工具,把 URDF 变成 MoveIt2 可用的配置包。流程:

# 生成后即可在 RViz 里做交互式规划(带 MotionPlanning 插件)
ros2 launch my_arm_moveit_config demo.launch.py

6.2 MoveGroup 接口:规划 + 执行(C++)

程序化规划的最小示例(以规划组名 arm 为例):

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("move_group_demo");

  // 用默认规划组 "arm" 构造 MoveGroup
  moveit::planning_interface::MoveGroupInterface move_group(node, "arm");

  // 目标 1:关节空间目标(单位弧度)
  std::vector<double> joint_goal = {0.0, -0.785, 1.57};
  move_group.setJointValueTarget(joint_goal);

  // 规划
  moveit::planning_interface::MoveGroupInterface::Plan plan;
  bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);

  // 成功才执行
  if (success) {
    RCLCPP_INFO(node->get_logger(), "plan ok, executing...");
    move_group.execute(plan);
  } else {
    RCLCPP_WARN(node->get_logger(), "plan failed");
  }

  rclcpp::shutdown();
  return 0;
}

常用接口速查:setPoseTarget()(笛卡尔位姿目标)、setJointValueTarget()(关节角目标)、setMaxVelocityScalingFactor()(限速,真机必设)、plan()(只规划)、move()(规划并执行一步到位)、execute(plan)(执行已有轨迹)。

6.3 RViz 交互:拖拽目标 + Execute

在 demo.launch.py 打开的 RViz 里:

1. 左下角切换到 MotionPlanning 面板,确认 Planning Group 为 arm;
2. 在 3D 视图里拖动机械臂末端的交互标记(interactive marker)到想要的位置/姿态;
3. 点 Plan(或 Plan & Execute)查看规划轨迹,绿色为通过;
4. 点 Execute 让机器人执行。真机务必先点 Plan 目视确认无碰撞再 Execute。

7 与仿真器桥接:Gazebo 与 Isaac Sim

URDF 描述模型、MoveIt2 负责规划,而"物理世界"由仿真器提供:重力、接触、传感器、电机。仿真器与 ROS2 之间靠"桥"交换消息(关节状态、力矩命令、TF、时钟等)。两大类桥接:

典型 Gazebo Classic 启动(URDF 经 gazebo_ros 插件带进仿真):

# 启动 Gazebo + 加载 URDF 模型(spawn 到仿真)
ros2 launch gazebo_ros gazebo.launch.py
ros2 run gazebo_ros spawn_entity.py -topic robot_description -entity my_arm

# 若用 ros2_control 驱动仿真关节(需在 URDF 里配置 gazebo_ros2_control 插件)
ros2 run controller_manager spawner joint_state_broadcaster
ros2 run controller_manager spawner joint_trajectory_controller

8 常见坑(踩过才知道)

9 本节自测

10 参考来源

• ROS2 官方 · URDF 系列教程(构建可视化模型 / 添加物理与碰撞属性)
• ROS2 官方 · TF2 坐标变换教程
• ROS2 官方 · Launch 文件教程
• ros2_control 官方文档
• joint_state_broadcaster 用户文档
• MoveIt 2 官方文档
• MoveIt2 · Move Group C++ Interface 教程
• NVIDIA Isaac Sim · ROS / ROS2 桥接教程
• Gazebo · ros_gz_bridge 官方仓库
• 本站 04-09 · ROS2 入门实战:第一个节点(前置)
• 本站 04-04 · 仿真与强化学习:MuJoCo 与 Isaac Lab(仿真器选型)