机械手URDF文件的编写

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

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

文件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>
  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.
  • 11.
  • 12.
  • 13.
  • 14.
  • 15.
  • 16.
  • 17.
  • 18.
  • 19.
  • 20.
  • 21.
  • 22.
  • 23.
  • 24.
  • 25.
  • 26.
  • 27.
  • 28.
  • 29.
  • 30.
  • 31.
  • 32.
  • 33.
  • 34.
  • 35.
  • 36.
  • 37.
  • 38.
  • 39.
  • 40.
  • 41.
  • 42.
  • 43.
  • 44.
  • 45.
  • 46.
  • 47.
  • 48.
  • 49.
  • 50.
  • 51.
  • 52.
  • 53.
  • 54.
  • 55.
  • 56.
  • 57.
  • 58.
  • 59.
  • 60.
  • 61.
  • 62.
  • 63.
  • 64.
  • 65.
  • 66.
  • 67.
  • 68.
  • 69.
  • 70.
  • 71.
  • 72.
  • 73.
  • 74.
  • 75.
  • 76.
  • 77.
  • 78.
  • 79.
  • 80.
  • 81.
  • 82.
  • 83.
  • 84.
  • 85.
  • 86.
  • 87.
  • 88.
  • 89.
  • 90.
  • 91.
  • 92.
  • 93.
  • 94.
  • 95.
  • 96.
  • 97.
  • 98.
  • 99.
  • 100.
  • 101.
  • 102.
  • 103.
  • 104.
  • 105.
  • 106.
  • 107.
  • 108.
  • 109.
  • 110.
  • 111.
  • 112.
  • 113.
  • 114.
  • 115.
  • 116.
  • 117.
  • 118.
  • 119.
  • 120.
  • 121.
  • 122.
  • 123.
  • 124.
  • 125.
  • 126.
  • 127.
  • 128.
  • 129.
  • 130.
  • 131.
  • 132.
  • 133.
  • 134.
  • 135.
  • 136.
  • 137.
  • 138.
  • 139.
  • 140.
  • 141.
  • 142.
  • 143.
  • 144.
  • 145.
  • 146.
  • 147.
  • 148.
  • 149.
  • 150.
  • 151.
  • 152.
  • 153.
  • 154.
  • 155.
  • 156.
  • 157.
  • 158.
  • 159.
  • 160.
  • 161.
  • 162.
  • 163.
  • 164.
  • 165.
  • 166.
  • 167.
  • 168.
  • 169.
  • 170.
  • 171.
  • 172.
  • 173.
  • 174.
  • 175.
  • 176.
  • 177.
  • 178.
  • 179.
  • 180.
  • 181.
  • 182.
  • 183.
  • 184.
  • 185.
  • 186.
  • 187.
  • 188.
  • 189.
  • 190.
  • 191.
  • 192.
  • 193.
  • 194.
  • 195.
  • 196.
  • 197.
  • 198.
  • 199.
  • 200.
  • 201.
  • 202.
  • 203.
  • 204.
  • 205.
  • 206.
  • 207.
  • 208.
  • 209.
  • 210.
  • 211.
  • 212.
  • 213.
  • 214.
  • 215.
  • 216.
  • 217.
  • 218.
  • 219.
  • 220.
  • 221.
  • 222.
  • 223.
  • 224.
  • 225.
  • 226.
  • 227.
  • 228.
  • 229.
  • 230.
  • 231.
  • 232.
  • 233.
  • 234.
  • 235.
  • 236.
  • 237.
  • 238.
  • 239.
  • 240.
  • 241.
  • 242.
  • 243.
  • 244.
  • 245.
  • 246.
  • 247.
  • 248.
  • 249.
  • 250.
  • 251.
  • 252.
  • 253.
  • 254.
  • 255.
  • 256.
  • 257.
  • 258.
  • 259.
  • 260.
  • 261.
  • 262.
  • 263.
  • 264.
  • 265.
  • 266.
  • 267.
  • 268.
  • 269.
  • 270.
  • 271.
  • 272.
  • 273.
  • 274.
  • 275.
  • 276.
  • 277.
  • 278.
  • 279.
  • 280.
  • 281.
  • 282.
  • 283.
  • 284.
  • 285.
  • 286.
  • 287.
  • 288.
  • 289.
  • 290.
  • 291.
  • 292.
  • 293.
  • 294.
  • 295.
  • 296.
  • 297.
  • 298.
  • 299.
  • 300.
  • 301.
  • 302.
  • 303.
  • 304.
  • 305.
  • 306.
  • 307.
  • 308.
  • 309.
  • 310.
  • 311.
  • 312.
  • 313.
  • 314.
  • 315.
  • 316.
  • 317.
  • 318.
  • 319.
  • 320.
  • 321.
  • 322.

机械手的外型

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

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

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

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

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

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

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

机械手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交互的接口。

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

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

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

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

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

机械手与MoveIt的关联

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

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

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

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

ros2 pkg create mybot_description --build-type ament_python

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

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

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

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

colcon build

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

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': [
        ],
    },
)
  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.
  • 11.
  • 12.
  • 13.
  • 14.
  • 15.
  • 16.
  • 17.
  • 18.
  • 19.
  • 20.
  • 21.
  • 22.
  • 23.
  • 24.
  • 25.
  • 26.
  • 27.
  • 28.
  • 29.

最后启动一下

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