如何用URDF文件构建机械手模型并与MoveIt集成

机械手URDF文件的编写

我们用urdf文件来描述我们的机械手的外观以及物理性能。这里为了简便，就只用了基本的圆柱、立方体了。追求美观的朋友，还可以用dae文件来描述机械手的外形。

import re
def remove_comments(text):
    pattern = r'<!--(.*?)-->'
    return re.sub(pattern, '', text, flags=re.DOTALL)

文件six_arm.urdf

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

    <!-- Base link -->
    <link name="base_link">
        <visual>
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="blue">
                <color rgba="0 0 1.0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.1 0.1 0.1"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="10"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Link 1 -->
    <link name="link1">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="green">
                <color rgba="0 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 1: rotation around X-axis -->
    <joint name="joint1" type="continuous">
        <parent link="base_link"/>
        <child link="link1"/>
        <axis xyz="0 0 1"/>
        <origin xyz="0 0 0.1" rpy="0 0 0"/>
    </joint>

    <!-- Link 2 -->
    <link name="link2">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="red">
                <color rgba="0.8 0 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 2: rotation around Y-axis -->
    <joint name="joint2" type="continuous">
        <parent link="link1"/>
        <child link="link2"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 3 -->
    <link name="link3">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="yellow">
                <color rgba="0.8 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 3: rotation around x-axis -->
    <joint name="joint3" type="continuous">
        <parent link="link2"/>
        <child link="link3"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 4 -->
    <link name="link4">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="green">
                <color rgba="0 0.8 0 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 4: rotation around X-axis -->
    <joint name="joint4" type="continuous">
        <parent link="link3"/>
        <child link="link4"/>
        <axis xyz="0 1 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 5 -->
    <link name="link5">
        <visual>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
            <material name="purple">
                <color rgba="0.8 0 0.8 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <cylinder length="0.1" radius="0.03"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.05"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 5: rotation around Y-axis -->
    <joint name="joint5" type="continuous">
        <parent link="link4"/>
        <child link="link5"/>
        <axis xyz="1 0 0"/>
        <origin xyz="0 0 0.1"/>
    </joint>

    <!-- Link 6 -->
    <link name="link6">
        <visual>
            <geometry>
                <box size="0.1 0.1 0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.1"/>
            <material name="pink">
                <color rgba="0.8 0.4 0.8 1"/>
            </material>
        </visual>
        <collision>
            <geometry>
                <box size="0.1 0.1 0.2"/>
            </geometry>
            <origin rpy="0 0 0" xyz="0 0 0.1"/>
        </collision>
        <inertial>
            <mass value="0.2"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>

    <!-- Joint 6: rotation around Z-axis -->
    <joint name="joint6" type="continuous">
        <parent link="link5"/>
        <child link="link6"/>
        <axis xyz="0 0 1"/>
        <origin xyz="0 0 0.1"/>
    </joint>


    <!-- Used for fixing robot to Gazebo 'base_link' 将机械手的基座固定在世界坐标上-->
    <link name="world"/>

    <joint name="fixed" type="fixed">
        <parent link="world"/>
        <child link="base_link"/>
    </joint>

    <gazebo reference="base_link">
        <material>Gazebo/Black</material>
        <gravity>true</gravity>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link1">
        <material>Gazebo/Gray</material>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link2">
        <material>Gazebo/Red</material>
        <selfCollide>false</selfCollide>
    </gazebo>
    <gazebo reference="link3">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="link4">
        <material>Gazebo/Green</material>
    </gazebo>
    <gazebo reference="link5">
        <material>Gazebo/Yellow</material>
    </gazebo>
    <gazebo reference="link6">
        <material>Gazebo/Orange</material>
    </gazebo>

    <!-- 在运行demo.launch.py时，需要注释这个ros2_control节点，因为它使用了xxx.ros2_control.xacro来生成了ros2_control节点-->
    <ros2_control name="GazeboSystem" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
        <joint name="joint1">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <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">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint3">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint4">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint5">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
        <joint name="joint6">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position">
                <param name="initial_value">0.0</param>
            </state_interface>
            <state_interface name="velocity"/>
        </joint>
    </ros2_control>

    <gazebo>
        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
            <parameters>$(find mybot)/config/ros2_controllers.yaml</parameters>
            <robot_param_node>robot_state_publisher</robot_param_node>
        </plugin>
    </gazebo>

</robot>

机械手的外型

可以在vscode中打开这个文件然后用插件就可以看到这个机械手的外形。

有同学不知道如何在vscode中查看urdf模型，这里介绍一下： 先安装vscode的ROS拓展，

然后可以在urdf文件中右键–》command palette–》ROS:Preview URDF

机械手link的inertial的设置

真正设置的话，有公式，自己可以参考一下。
urdf里面的link必须要有旋转惯量矩阵‘intertial’的，否则在gazebo里面导入模型urdf时，会报下面的错。
[gazebo-1] [Err] [Model.cc:123] Error Code 23 Msg: FrameAttachedToGraph error, Non-LINK vertex with name [model] is disconnected; it should have 1 outgoing edge in MODEL attached_to graph.

ros2_control插件

该插件是在gazbo导入该模型文件时，创建与Ros2交互的接口。

上面这个是之前的，我现在更新成这样了主要是parameter节点不一样。用了$(find mybot)就方便一点。但是相应地launch文件中就要用xacro了。

上面的ros2_controllers.yaml文件是在下一步创建出来的，先不用管。
同时，ros2_control这个节点下的内容也是要和ros2_controllers.yaml对应的，也可以先不管。
这个节点在开始阶段先注释掉，否则会与通过moveit_setup_assistant创建的一个 fake_system 的 ros2_control冲突。

机械手与MoveIt的关联

通过前面的操作，我们拥有了一个描述机械手的文件 six_arm.urdf，接下来我们利用该文件创建一个可以利用MoveIt进行路径规划的“工程”。

建立一个文件夹myRobot，然后再在此文件夹中建立一个src文件夹

然后进入src文件夹路径，创建包

ros2 pkg create mybot_description --build-type ament_python

在 src/mybot_description文件夹下，创建urdf文件夹，然后把six_arm.urdf放进去，如下图所示。

返回myRobot目录，然后编译一下

colcon build

from setuptools import setup

from glob import glob #这里
import os #这里

package_name = 'mybot_description'

setup(
    name=package_name,
    version='0.0.0',
    packages=[package_name],
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'urdf'), glob('urdf/**')), #这里
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='yong',
    maintainer_email='yong@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
        ],
    },
)

最后启动一下