如何用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 setupfrom 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='[e-mail]',description='TODO: Package description',license='TODO: License declaration',tests_require=['pytest'],entry_points={'console_scripts': [],},
)

最后启动一下