Duruofu's blog

Appearance

使用urdf构建机械臂 ​

一、准备 ​

新建工作空间 ：

mkdir -p ~/Desktop/moveit2_learn/ros2_ws/src

这里我使用moveit2_learn 作为项目文件夹，ros2_ws是ROS2的工作目录

在工作目录下建立一个功能包，用于描述机械臂：

cd ros2_ws/src
ros2 pkg create robot_description

在robot_description目录下，建立一个urdf、launch、rviz文件夹：

cd robot_description
mkdir urdf launch rviz

其中urdf用于存储机器人模型，launch用于编写可视化脚本，rviz里存储rviz软件的配置

在CMakelist.txt文件里添加下面的内容：

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

二、创建urdf文件并初步可视化 ​

2.1 基座组件 ​

在urdf文件夹新建arm.urdf

xml
<?xml version="1.0"?>
<robot name="my_robot">
    <material name="grey">
        <color rgba="0.5 0.5 0.5 1"/>
    </material>

	<link name="base_link">
      <visual>
        <geometry>
            <box size="0.4 0.4 0.1"/>
        </geometry>
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

</robot>

这里简单创建一个方块，先尝试预览模型。

安装下面的包urdf-tutorial：

bash
sudo apt install ros-$ROS_DISTRO-urdf-tutorial

这个包是便于我们预览模型的，不用手动编写启动文件，使用下面的命令，预览模型:

bash
ros2 launch urdf_tutorial display.launch.py model:=/home/duruofu/project/moveit2_learn/ros2_ws/src/robot_description/urdf/arm.urdf

这里注意指定自己文件真实的路径。

运行后，就能打开下面的rviz2,看到模型：

这样我们就完成了一个基本的基座。下面添加一个可以旋转的关节(肩部)：

2.2 肩部关节 ​

继续修改arm.urdf：

xml
<?xml version="1.0"?>
<robot name="my_robot">
    <material name="grey">
        <color rgba="0.5 0.5 0.5 1"/>
    </material>
    <material name="blue">
        <color rgba="0 0 0.5 1"/>
    </material>

	<link name="base_link">
      <visual>
        <geometry>
            <box size="0.4 0.4 0.1"/>
        </geometry>
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

    <link name="shoulder_link"> 
      <visual>
        <geometry>
            <cylinder length="0.5" radius="0.1"/>
        </geometry>
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <material name="blue" />
      </visual>
    </link>

    <joint name="joint1" type="revolute">
        <parent link="base_link"/>
        <child link="shoulder_link"/>
        <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit effort="1000.0" velocity="1.0" lower="-3.14" upper="3.14"/>
    </joint>

</robot>

创建了肩部连杆 以及肩部和base_link之间的关节关系。

这里注意关节的参数设置：

xml
    <joint name="joint1" type="revolute">
        <parent link="base_link"/>
        <child link="shoulder_link"/>
        <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit effort="1000.0" velocity="1.0" lower="-3.14" upper="3.14"/>
    </joint>

效果如下：

关节可以通过滑块进行转动。

2.3 其他连杆和关节 ​

所有连杆的参数如下：

Links:
base link (box size 0.40.4 0.1)
shoulder_link (cylinder len 0.5 radius 0.1)
arm_link (cylinder len 0.6 radius 0.05)
elbow_link (cylinder len 0.1 radius 0.05)
forearm_link (cylinder len 0.5 radius 0.05)
wrist_link (box size 0.10.1 0.05)
hand_link (box size 0.10.1 0.02)
tool_link (no visual)

Joints:
joint1 (revolute,axis z,min-3.14,max 3.14)
joint2 (revolute,axis y,min 0,max 2.5)
joint3 (revolute,axis y,min 0,max 2.5)
joint4 (revolute,axis z,min -3.14,max 3.14)
joint5 (revolute,axis y,min -1.57,max 1.57)
joint6 (continuous,axis z)
hand_tool_joint (fixed)

根据上面的描述，我们可以完善urdf如下：

xml
<?xml version="1.0"?>
<robot name="my_robot">
    <material name="grey">
        <color rgba="0.5 0.5 0.5 1"/>
    </material>
    <material name="blue">
        <color rgba="0 0 0.5 1"/>
    </material>

	<link name="base_link">
      <visual>
        <geometry>
            <box size="0.4 0.4 0.1"/>
        </geometry>
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

    <link name="shoulder_link"> 
      <visual>
        <geometry>
            <cylinder length="0.5" radius="0.1"/>
        </geometry>
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <material name="blue" />
      </visual>
    </link>


    <link name="arm_link"> 
      <visual>
        <geometry>
            <cylinder length="0.6" radius="0.05"/>
        </geometry>
        <origin xyz="0.0 0.0 0.3" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

    <link name="elbow_link"> 
      <visual>
        <geometry>
            <cylinder length="0.1" radius="0.05"/>
        </geometry>
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        <material name="blue" />
      </visual>
    </link>

    <link name="forearm_link"> 
      <visual>
        <geometry>
            <cylinder length="0.5" radius="0.05"/>
        </geometry>
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

	<link name="wrist_link">
      <visual>
        <geometry>
            <box size="0.1 0.1 0.05"/>
        </geometry>
        <origin xyz="0.0 0.0 0.025" rpy="0.0 0.0 0.0"/>
        <material name="blue" />
      </visual>
    </link>

	<link name="hand_link">
      <visual>
        <geometry>
            <box size="0.1 0.1 0.02"/>
        </geometry>
        <origin xyz="0.0 0.0 0.025" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

	<link name="tool_link" />

    <joint name="joint1" type="revolute">
        <parent link="base_link"/>
        <child link="shoulder_link"/>
        <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit effort="1000.0" velocity="1.0" lower="-3.14" upper="3.14"/>
    </joint>

    <joint name="joint2" type="revolute">
        <parent link="shoulder_link"/>
        <child link="arm_link"/>
        <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="1000.0" velocity="1.0" lower="0" upper="2.5"/>
    </joint>

    <joint name="joint3" type="revolute"> 
        <parent link="arm_link"/>
        <child link="elbow_link"/>
        <origin xyz="0.0 0.0 0.6" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="500.0" velocity="1.0" lower="0" upper="2.5"/>
    </joint>

    <joint name="joint4" type="revolute"> 
        <parent link="elbow_link"/>
        <child link="forearm_link"/>
        <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 0 1"/>
        <limit effort="500.0" velocity="1.0" lower="-3.14" upper="3.14"/>
    </joint>

    <joint name="joint5" type="revolute"> 
        <parent link="forearm_link"/>
        <child link="wrist_link"/>
        <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="300.0" velocity="1.0" lower="-1.57" upper="1.57"/>
    </joint>


    <joint name="joint6" type="continuous"> 
        <parent link="wrist_link"/>
        <child link="hand_link"/>
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 0 1"/>
    </joint>

    <joint name="hand_tool_joint" type="fixed"> 
        <parent link="hand_link"/>
        <child link="tool_link"/>
        <origin xyz="0.0 0.0 0.02" rpy="0.0 0.0 0.0"/>
    </joint>

</robot>

最后的效果是这样的：

代码： https://github.com/DuRuofu/moveit2_learn/commit/d97588f00cdfc64b1f2e12f9d425949e5018ebf6

三、使用Xacro扩展urdf ​

在urdf文件下新建 robot.urdf.xacro,作为机器人的主体：

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

</robot>

再建立一个common_properties.xacro 用于放置一些通用的标签，这里把之前定义的颜色材质挪到这个文件里来。

xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> 
    <material name="grey">
        <color rgba="0.5 0.5 0.5 1"/>
    </material>
    <material name="blue">
        <color rgba="0 0 0.5 1"/>
    </material>
</robot>

然后在robot.urdf.xacro 中引用通用属性：

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

    <xacro:include filename="common_properties.xacro"/>
    
</robot>

修改原来的arm.urdf改为arm.xacro:

这里仅仅删除了通用材质和 robot标签的机器人名字属性，这个属性全局唯一，已经在robot.urdf.xacro里定义了。

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

	<link name="base_link">
      <visual>
        <geometry>
            <box size="0.4 0.4 0.1"/>
        </geometry>
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

    <link name="shoulder_link"> 
      <visual>
        <geometry>
            <cylinder length="0.5" radius="0.1"/>
        </geometry>
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <material name="blue" />
      </visual>
    </link>


    <link name="arm_link"> 
      <visual>
        <geometry>
            <cylinder length="0.6" radius="0.05"/>
        </geometry>
        <origin xyz="0.0 0.0 0.3" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

    <link name="elbow_link"> 
      <visual>
        <geometry>
            <cylinder length="0.1" radius="0.05"/>
        </geometry>
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        <material name="blue" />
      </visual>
    </link>

    <link name="forearm_link"> 
      <visual>
        <geometry>
            <cylinder length="0.5" radius="0.05"/>
        </geometry>
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

	<link name="wrist_link">
      <visual>
        <geometry>
            <box size="0.1 0.1 0.05"/>
        </geometry>
        <origin xyz="0.0 0.0 0.025" rpy="0.0 0.0 0.0"/>
        <material name="blue" />
      </visual>
    </link>

	<link name="hand_link">
      <visual>
        <geometry>
            <box size="0.1 0.1 0.02"/>
        </geometry>
        <origin xyz="0.0 0.0 0.025" rpy="0.0 0.0 0.0"/>
        <material name="grey" />
      </visual>
    </link>

	<link name="tool_link" />

    <joint name="joint1" type="revolute">
        <parent link="base_link"/>
        <child link="shoulder_link"/>
        <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit effort="1000.0" velocity="1.0" lower="-3.14" upper="3.14"/>
    </joint>

    <joint name="joint2" type="revolute">
        <parent link="shoulder_link"/>
        <child link="arm_link"/>
        <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="1000.0" velocity="1.0" lower="0" upper="2.5"/>
    </joint>

    <joint name="joint3" type="revolute"> 
        <parent link="arm_link"/>
        <child link="elbow_link"/>
        <origin xyz="0.0 0.0 0.6" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="500.0" velocity="1.0" lower="0" upper="2.5"/>
    </joint>

    <joint name="joint4" type="revolute"> 
        <parent link="elbow_link"/>
        <child link="forearm_link"/>
        <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 0 1"/>
        <limit effort="500.0" velocity="1.0" lower="-3.14" upper="3.14"/>
    </joint>

    <joint name="joint5" type="revolute"> 
        <parent link="forearm_link"/>
        <child link="wrist_link"/>
        <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="300.0" velocity="1.0" lower="-1.57" upper="1.57"/>
    </joint>


    <joint name="joint6" type="continuous"> 
        <parent link="wrist_link"/>
        <child link="hand_link"/>
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0"/>
        <axis xyz="0 0 1"/>
    </joint>

    <joint name="hand_tool_joint" type="fixed"> 
        <parent link="hand_link"/>
        <child link="tool_link"/>
        <origin xyz="0.0 0.0 0.02" rpy="0.0 0.0 0.0"/>
    </joint>

</robot>

然后在robot.urdf.xacro 中引用机械臂文件：

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

    <xacro:include filename="common_properties.xacro"/>
    <xacro:include filename="arm.xacro"/>

</robot>

再次预览模型，这次改用调用robot.urdf.xacro 文件：

bash
ros2 launch urdf_tutorial display.launch.py model:=/home/duruofu/project/moveit2_learn/ros2_ws/src/robot_description/urdf/robot.urdf.xacro

也是可以正常显示的：

代码: https://github.com/DuRuofu/moveit2_learn/commit/41afb21ac60329f8e2f33dae5e952f92b1a6e7bc

四、使用启动文件发布urdf ​

在launch文件夹下新建：display.launch.xml(也可以使用python ,看个人喜好);

xml
<launch>
    <let name="urdf_path"
            value="$(find-pkg-share robot_description)/urdf/robot.urdf.xacro"/>

    <node pkg="robot_state_publisher" exec="robot_state_publisher">
        <param name="robot_description" 
        value="$(command 'xacro $(var urdf_path)')"/>
    </node>       

    <node pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui"/>

    <node pkg="rviz2" exec="rviz2" output="screen"/>
</launch>

这段代码是一个 ROS1 的 launch 文件（使用了 XML 格式）用于启动一些常见的 ROS 节点，用来加载 URDF 文件、发布机器人状态、并在 RViz 中显示机器人模型。下面是代码的详细解释：

  1. <let name="urdf_path" value="..."/> 这行定义了一个名为 urdf_path 的变量，表示 URDF 文件的路径。 $(find-pkg-share robot_description) 是一种 ROS1 方式，它会根据 robot_description 包的位置，找到该包中的 urdf/robot.urdf.xacro 文件。 这个文件是描述机器人模型的 Xacro 格式的 URDF 文件。

  2. <node pkg="robot_state_publisher" exec="robot_state_publisher"> 启动一个 robot_state_publisher 节点，这个节点的作用是将机器人的状态（即关节角度等）发布到 ROS 话题上供其他节点使用。 在这里，我们传递 robot_description 参数，它包含了机器人的 URDF（或者 Xacro）描述。 具体来说，xacro 命令会将 urdf_path 中指定的 Xacro 文件解析成 URDF 格式并传递给 robot_state_publisher。

  3. <node pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui"/> 启动一个 joint_state_publisher_gui 节点，它提供了一个图形界面来控制机器人的关节状态。用户可以通过这个界面来手动调整机器人各个关节的角度，发布到 ROS 系统。

  4. <node pkg="rviz2" exec="rviz2" output="screen"/> 启动 rviz2 节点，这是 ROS2 中的可视化工具，用于显示机器人的模型、传感器数据、状态等。 启动后，rviz2 会自动打开一个窗口，显示机器人的 URDF 模型，用户可以在此环境中进行交互和调试。

当然，也可以使用python编写，如下：

python
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import Command, FindExecutable

def generate_launch_description():
    # 获取 robot_description 包的 share 目录路径
    robot_description_share_dir = get_package_share_directory('robot_description')

    # 拼接 URDF 文件路径
    urdf_path = os.path.join(robot_description_share_dir, 'urdf', 'robot.urdf.xacro')

    # 创建 launch description
    # 使用 Command + FindExecutable 来执行 xacro 并把输出作为 robot_description 参数
    robot_description_cmd = Command([FindExecutable(name='xacro'), ' ', urdf_path])

    return LaunchDescription([
        # 启动 robot_state_publisher 节点，传递 robot_description 参数（机器人的 URDF）
        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name='robot_state_publisher',
            output='screen',
            parameters=[{'robot_description': robot_description_cmd}]
        ),
        
        # 启动 joint_state_publisher_gui 节点，用于显示关节控制 GUI
        Node(
            package='joint_state_publisher_gui',
            executable='joint_state_publisher_gui',
            name='joint_state_publisher_gui',
            output='screen'
        ),
        
        # 启动 rviz2 节点，用于可视化机器人模型
        Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            output='screen'
        ),
    ])

编译运行：

cd ros2_ws 
colcon build
source install/setup.bash
ros2 launch robot_description display.launch.xml

是可以正常显示的（需要在rviz里手动添加）

至此，我们完成了机械臂原型的构建工作。

代码: https://github.com/DuRuofu/moveit2_learn/commit/d30e6cc52d9a3aeb564ac60804377ee3705d0f18