1.前言

本文是对之前发的文章【在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手】的一个拓展。建议先看看之前的文章再看这个。
之前的文章的机械手是一个纯urdf自带几何图像（圆柱、立方体）等组成的所谓机械手，基本没有什么实用性。在后续的文章【ROS2中用MoveIt2控制自己的舵机机械手（1）】，我这边也已经实现了控制一个实物机械手，所以当时就没拿这个机械手放到gazebo中去仿真，因为我已经有实物了嘛。
但是后来陆续有人问我怎么把他们带有stl模型的机械手（可能也是从SolidWorks导出的），导入到gazebo仿真，因为他们可能也还是概念设计阶段，还没把实物做出来，的确有在gazebo里面仿真的必要。
因此，还是介绍一下如何把urdf导入moveit、urdf导入gazebo、moveit控制gazebo的整个流程吧。

2.从urdf到moveit

拿到从SolidWork导出的urdf、meshes文件夹后，建立一个软件包，利用moveit_setup_assistant工具，配置好相关参数与文件。参考【在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手】的【3.机械手与MoveIt的关联】，此时我们要确保运行【demo.launch.py】 时是正常的，是可以规划、执行路径的。

3.从urdf到gazebo

当能够在moveit中跑我们的机械手之后，说明我们的urdf文件基本没什么问题了，尽量就不要去更改它（下面会打脸）。
但是，要让gazebo能够成功加载并仿真一个urdf文件，势必要加上很多额外的xml的节点，也就是要修改urdf，这可怎么办？
幸好，xacro能够帮我们解决这个矛盾。

3.1.urdf文件的修改

3.1.1.mesh路径

有个尴尬的地方，对于mesh文件（dae、stl），gazebo那边识别不了 package://claw_description 这种ros系统的路径查找方式（尝试运行的话，gazebo直接卡在那里），只能用 $(find claw_description)这种方式找到绝对路径并替换。
但是，rviz这边又不认这种绝对路径的。

幸好，在【Unstable behaviour of Position Controller in UR10 #73】找到了一种解决办法，属于是双管齐下了。

        <mesh filename="file://$(find claw_description)/meshes/link1.STL" />

3.1.2.零件起飞

另外，假如直接导入原来的urdf文件，过了一段时间后，会发现机械手的各个零件各自脱离，漫天飞。

这是仿真的“锅”。惯性参数（inertial）+质量+重力三者一起模拟驱动了各个零件受力，从而发生了相应的运动。
而我们其实大多数情况下，都是希望gazebo把整个机械手当成一个静态刚体来对待。空间中的其他物体只需要与机械手发生碰撞、摩擦计算就行。
因此我们可以把原来从SolidWorks导出来的urdf文件的惯性参数改成0.
或者，可以在后面的xacro文件中取消各个link的重力：

上面两个办法二选一。

3.1.3.文件保存

因此最后得到的urdf文件为claw_description1.urdf，是从原来的文件claw_description.urdf拷贝修改而来的

主要就是修改了惯性参数、stl路径表达

3.2.xacro文件的修改

我们先到moveit_setup_assistant生成的config下面看看，偷偷师。先看看他们原来的重要文件：
这里的核心文件是claw_description.urdf.xacro，其引用、使用了SolidWorks导出的claw_description.urdf以及ros2_control的claw_description.ros2_control.xacro。最终效果就是既使用了描述模型的urdf文件，也定义了ros2_control的相关节点。
因此，我们也可以模仿一下，写一个gazebo_claw_description.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="claw_description"><!-- Used for fixing robot to Gazebo 'base_link' 将机械手的基座固定在世界坐标上--><link name="world"/><joint name="fixed" type="fixed"><parent link="world"/><child link="link1"/></joint><!-- Import claw_description urdf file --><xacro:include filename="$(find claw_description)/urdf/claw_description1.urdf" /><!-- 对一些link进行gazebo的属性设置 --><gazebo reference="link1"><material>Gazebo/Purple</material><self_collide>false</self_collide><gravity>false</gravity></gazebo><gazebo reference="link2"><material>Gazebo/Red</material><gravity>false</gravity></gazebo><gazebo reference="link3"><material>Gazebo/Blue</material><gravity>false</gravity></gazebo><gazebo reference="link4"><material>Gazebo/Green</material><gravity>false</gravity></gazebo><gazebo reference="link5"><material>Gazebo/Yellow</material><gravity>false</gravity></gazebo><gazebo reference="link6"><material>Gazebo/Orange</material><gravity>false</gravity></gazebo><!-- 设置不了静态，不知为啥 --><gazebo><is_static>true</is_static><static>true</static><isStatic>true</isStatic><self_collide>true</self_collide></gazebo><!-- 声明马达,好像没什么卵用 --><!-- <xacro:macro name="joint_transmission" params="joint_name"><transmission name="${joint_name}_trans"><type>transmission_interface/SimpleTransmission</type><joint name="${joint_name}"><hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface></joint><actuator name="${joint_name}_motor"><hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission></xacro:macro><xacro:joint_transmission joint_name="joint1"/><xacro:joint_transmission joint_name="joint2"/><xacro:joint_transmission joint_name="joint3"/><xacro:joint_transmission joint_name="joint4"/><xacro:joint_transmission joint_name="joint5"/> --><!-- 声明ros2_control --><ros2_control name="GazeboSystem" type="system"><hardware><plugin>gazebo_ros2_control/GazeboSystem</plugin></hardware><joint name="joint1"><command_interface name="position"/><state_interface name="position"><param name="initial_value">0</param></state_interface><state_interface name="velocity"/></joint><joint name="joint2"><command_interface name="position"/><state_interface name="position"><param name="initial_value">0</param></state_interface><state_interface name="velocity"/></joint><joint name="joint3"><command_interface name="position"/><state_interface name="position"><param name="initial_value">0</param></state_interface><state_interface name="velocity"/></joint><joint name="joint4"><command_interface name="position"/><state_interface name="position"><param name="initial_value">0</param></state_interface><state_interface name="velocity"/></joint><joint name="joint5"><command_interface name="position"/><state_interface name="position"><param name="initial_value">0</param></state_interface><state_interface name="velocity"/></joint></ros2_control><!-- 加载ros2_control插件 --><gazebo><plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control"><parameters>$(find arm_claw)/config/ros2_controllers.yaml</parameters><robot_param>robot_description</robot_param><robot_param_node>robot_state_publisher</robot_param_node></plugin></gazebo></robot>

本来想直接<xacro:include filename=“claw_description.ros2_control.xacro” />的，但是里面的一些东西不通用，所以还是重新写一遍ros2_control的节点算了。
这个gazebo_claw_description.urdf.xacro就可以用来作为我们的gazebo模型文件了。

3.3.launch

至此，urdf、xacro文件都修改好了，那么参考 /opt/ros/humble/share/gazebo_ros2_control_demos/launch 里面的文件写个执行文件吧：

import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSourcefrom launch.event_handlers import OnProcessExitfrom ament_index_python.packages import get_package_share_directoryimport xacrodef generate_launch_description():package_name = 'arm_claw'robot_name_in_model = 'claw_description'pkg_share = FindPackageShare(package=package_name).find(package_name) urdf_model_path = os.path.join(pkg_share, f'config/gazebo_claw_description.urdf.xacro')print("---", urdf_model_path)doc = xacro.parse(open(urdf_model_path))xacro.process_doc(doc)params = {'robot_description': doc.toxml()}print("urdf", doc.toxml())# 启动gazebogazebo = IncludeLaunchDescription(PythonLaunchDescriptionSource([os.path.join(get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),)# gazebo =  ExecuteProcess(#     cmd=['gazebo', '--verbose','-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],#     output='screen')# 启动了robot_state_publisher节点后，该节点会发布 robot_description 话题，话题内容是模型文件urdf的内容# 并且会订阅 /joint_states 话题，获取关节的数据，然后发布tf和tf_static话题.# 这些节点、话题的名称可不可以自定义？node_robot_state_publisher = Node(package='robot_state_publisher',executable='robot_state_publisher',parameters=[{'use_sim_time': True}, params, {"publish_frequency":15.0}],output='screen')spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',arguments=['-topic', 'robot_description','-entity', f'{robot_name_in_model}'], output='screen')# # Launch the robot, 这个是通过传递文件路径来在gazebo里生成模型.此时要求urdf文件里面没有xacro的语句# spawn_entity = Node(#     package='gazebo_ros', #     executable='spawn_entity.py',#     arguments=['-file', urdf_model_path,#                '-entity', robot_name_in_model,   ], #     output='screen')# gazebo在加载urdf时，根据urdf的设定，会启动一个joint_states节点?# 关节状态发布器load_joint_state_controller = ExecuteProcess(cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','joint_state_broadcaster'],output='screen')# 路径执行控制器，也就是那个action？# 这个my_group_controller需要根据urdf文件里面引用的ros2_controllers.yaml里面的名字确定load_joint_trajectory_controller = ExecuteProcess(cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','my_group_controller'],output='screen')# 用下面这两个估计是想控制好各个节点的启动顺序# 监听 spawn_entity_cmd，当其退出（完全启动）时，启动load_joint_state_controller？close_evt1 =  RegisterEventHandler( event_handler=OnProcessExit(target_action=spawn_entity,on_exit=[load_joint_state_controller],))# 监听 load_joint_state_controller，当其退出（完全启动）时，启动load_joint_trajectory_controller？# moveit是怎么和gazebo这里提供的action连接起来的？？close_evt2 = RegisterEventHandler(event_handler=OnProcessExit(target_action=load_joint_state_controller,on_exit=[load_joint_trajectory_controller],))ld = LaunchDescription([close_evt1,close_evt2,gazebo,node_robot_state_publisher,spawn_entity,])return ld

运行一下，得到
还可以。
然后，直接在另外一个控制台运行一下rviz2，在rviz2中看看（注意此时还没启动moveit）：

4.用moveit控制gazebo

还是参考之前的文章【在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手】，5.2节的内容，写一个moveit+rviz的启动文件：gazebo_moveit_rviz.launch.py。

然后分别打开两个控制台，分别执行：

ros2 launch arm_claw gazebo_moveit_rviz.launch.py 

ros2 launch arm_claw gazebo.launch.py 

然后就可以愉快地玩耍了。

5.结语

虽然好像是弄了出来，但是其实还有很多问题。
这些问题也许是错误操作，也许是概念偷换，总之就是一堆等待花精力、时间去操作的事情，后面再慢慢修正吧。
这些问题包括但是不限于：

1.在利用moveit_setup_assisatant对urdf文件进行读取时，我们用的是claw_description.urdf文件，也就是moveit用的是claw_description.urdf文件，而gazebo用的是claw_description1.urdf + xacro文件，也就是不同的两个文件，可以这样用吗？
2.在gazebo中的机械手，有时候会“脱节”，零件散开。（发现是机械手运动时撞到地板了，只要不撞地板，貌似都是正常的）
3.对于惯性参数，目前我们选择的是躺平操作，更合理的操作是啥？

---

参考：
【Gazebo仿真小例程一（通过例程熟悉整个仿真步骤）】
【Unstable behaviour of Position Controller in UR10 #73】
【ur10.urdf.xacro】