3. 创建机械臂URDF模型

任务目标

编写一个包含6个旋转关节的机械臂URDF模型文件，并在RViz中正确显示。该URDF将作为后续MoveIt2配置与运动规划的基础模型。

实现步骤

创建ROS2功能包并添加URDF文件
编写6自由度机械臂URDF（基座+6连杆+6关节）
使用robot_state_publisher发布TF树
在RViz中加载并验证模型显示

1. 创建功能包

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_cmake six_dof_arm_description
mkdir -p six_dof_arm_description/urdf
mkdir -p six_dof_arm_description/launch
mkdir -p six_dof_arm_description/rviz

2. 完整URDF代码

创建 six_dof_arm_description/urdf/arm.urdf，内容如下：

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

  <!-- 基座 -->
  <link name="base_link">
    <visual>
      <geometry><box size="0.2 0.2 0.1"/></geometry>
      <material name="base_color"/>
    </visual>
    <collision>
      <geometry><box size="0.2 0.2 0.1"/></geometry>
    </collision>
  </link>

  <!-- 关节1：基座旋转 -->
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1.0"/>
  </joint>

  <link name="link1">
    <visual>
      <geometry><cylinder radius="0.05" length="0.3"/></geometry>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
      <material name="link_color"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.05" length="0.3"/></geometry>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
    </collision>
  </link>

  <!-- 关节2：肩部俯仰 -->
  <joint name="joint2" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.3" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.57" upper="1.57" effort="10" velocity="1.0"/>
  </joint>

  <link name="link2">
    <visual>
      <geometry><box size="0.06 0.06 0.3"/></geometry>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
      <material name="link_color"/>
    </visual>
    <collision>
      <geometry><box size="0.06 0.06 0.3"/></geometry>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
    </collision>
  </link>

  <!-- 关节3：肘部俯仰 -->
  <joint name="joint3" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 0.3" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.09" upper="2.09" effort="10" velocity="1.0"/>
  </joint>

  <link name="link3">
    <visual>
      <geometry><cylinder radius="0.04" length="0.25"/></geometry>
      <origin xyz="0 0 0.125" rpy="0 0 0"/>
      <material name="link_color"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.04" length="0.25"/></geometry>
      <origin xyz="0 0 0.125" rpy="0 0 0"/>
    </collision>
  </link>

  <!-- 关节4：腕部旋转 -->
  <joint name="joint4" type="revolute">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="0 0 0.25" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1.0"/>
  </joint>

  <link name="link4">
    <visual>
      <geometry><box size="0.05 0.05 0.2"/></geometry>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
      <material name="link_color"/>
    </visual>
    <collision>
      <geometry><box size="0.05 0.05 0.2"/></geometry>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
    </collision>
  </link>

  <!-- 关节5：腕部俯仰 -->
  <joint name="joint5" type="revolute">
    <parent link="link4"/>
    <child link="link5"/>
    <origin xyz="0 0 0.2" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.57" upper="1.57" effort="10" velocity="1.0"/>
  </joint>

  <link name="link5">
    <visual>
      <geometry><cylinder radius="0.03" length="0.15"/></geometry>
      <origin xyz="0 0 0.075" rpy="0 0 0"/>
      <material name="link_color"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.03" length="0.15"/></geometry>
      <origin xyz="0 0 0.075" rpy="0 0 0"/>
    </collision>
  </link>

  <!-- 关节6：末端旋转 -->
  <joint name="joint6" type="revolute">
    <parent link="link5"/>
    <child link="tool0"/>
    <origin xyz="0 0 0.15" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14" upper="3.14" effort="10" velocity="1.0"/>
  </joint>

  <link name="tool0">
    <visual>
      <geometry><box size="0.04 0.04 0.05"/></geometry>
      <origin xyz="0 0 0.025" rpy="0 0 0"/>
      <material name="tool_color"/>
    </visual>
    <collision>
      <geometry><box size="0.04 0.04 0.05"/></geometry>
      <origin xyz="0 0 0.025" rpy="0 0 0"/>
    </collision>
  </link>

  <!-- 材质定义 -->
  <material name="base_color">
    <color rgba="0.3 0.3 0.3 1.0"/>
  </material>
  <material name="link_color">
    <color rgba="0.2 0.6 0.8 1.0"/>
  </material>
  <material name="tool_color">
    <color rgba="0.9 0.4 0.1 1.0"/>
  </material>

</robot>

3. 启动文件

创建 six_dof_arm_description/launch/display.launch.py：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    pkg_share = get_package_share_directory('six_dof_arm_description')
    urdf_path = os.path.join(pkg_share, 'urdf', 'arm.urdf')

    with open(urdf_path, 'r') as f:
        robot_desc = f.read()

    return LaunchDescription([
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            parameters=[{'robot_description': robot_desc}],
            output='screen'
        ),
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            output='screen'
        ),
        Node(
            package='rviz2',
            executable='rviz2',
            arguments=['-d', os.path.join(pkg_share, 'rviz', 'view.rviz')],
            output='screen'
        )
    ])

4. CMakeLists.txt 配置

编辑 six_dof_arm_description/CMakeLists.txt，添加安装规则：

install(DIRECTORY urdf launch rviz
  DESTINATION share/${PROJECT_NAME}
)

5. 编译与运行

cd ~/ros2_ws
colcon build --packages-select six_dof_arm_description
source install/setup.bash
ros2 launch six_dof_arm_description display.launch.py

6. RViz配置与验证

首次启动后需手动配置RViz：

将 Fixed Frame 设置为 base_link
点击 Add → 选择 RobotModel，此时应显示完整机械臂模型
点击 Add → 选择 TF，可查看各坐标系TF树
拖动 joint_state_publisher_gui 窗口中的滑条，观察各关节运动

配置完成后可通过 File → Save Config As 保存为 rviz/view.rviz，后续启动将自动加载。

验证判定标准

RViz中显示6个蓝色连杆+灰色基座+橙色末端，无红色报错
TF树中base_link到tool0共7个坐标系层级正确
拖动GUI滑条时，对应关节在RViz中同步旋转

关节方向说明：joint1/joint4/joint6绕Z轴旋转（偏航），joint2/joint3/joint5绕Y轴旋转（俯仰），构成典型的6R串联机械臂结构。各关节限位已设置，避免运动学奇异位置。

📚 课程目录