機械手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='yong@todo.todo',description='TODO: Package description',license='TODO: License declaration',tests_require=['pytest'],entry_points={'console_scripts': [],},
)

最后啟動一下