ROS2中，从SolidWorks导出的urdf，联合moveit、gazebo进行控制及仿真

1.前言

本文是对之前发的文章【在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手】的一个拓展。建议先看看之前的文章再看这个。
之前的文章的机械手是一个纯urdf自带几何图像（圆柱、立方体）等组成的所谓机械手，基本没有什么实用性。在后续的文章【ROS2中用MoveIt2控制自己的舵机机械手（1）】，我这边也已经实现了控制一个实物机械手，所以当时就没拿这个机械手放到gazebo中去仿真，因为我已经有实物了嘛。
但是后来陆续有人问我怎么把他们带有stl模型的机械手（可能也是从SolidWorks导出的），导入到gazebo仿真，因为他们可能也还是概念设计阶段，还没把实物做出来，的确有在gazebo里面仿真的必要。
因此，还是介绍一下如何把urdf导入moveit、urdf导入gazebo、moveit控制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。（不太建议使用这个办法，会破坏一致性）（可以看到这里的惯性参数是有问题的，解决方案看这里【sw2urdf导出的urdf文件中的惯性参数（inertial）错误的问题】）
二是可以在后面的xacro文件中取消各个link的重力模拟。（这个可能好一点，这样就相当于没修改原urdf，保证了moveit、gazebo的一致性）

上面两个办法二选一。

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不能这么用，会导致 
         Warning [parser_urdf.cc:1134] multiple inconsistent <static> exists due to fixed joint reduction overwriting previous value [true] with [false].
-->
        <!-- <static>true</static> -->
        <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>

本来想直接的，但是里面的一些东西不通用，所以还是重新写一遍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 PythonLaunchDescriptionSource

from launch.event_handlers import OnProcessExit

from ament_index_python.packages import get_package_share_directory

import xacro

def 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())

    # 启动gazebo
    gazebo = 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文件，也就是不同的两个文件，可以这样用吗？（claw_description.urdf中的link、joint等元素，在gazebo这边都是有的，而且这些元素的相互关系也是一致的，所以，应该可以这么干？）
2.在gazebo中的机械手，有时候会“脱节”，零件散开。（发现是机械手运动时撞到地板了，只要不撞地板，貌似都是正常的）
3.对于惯性参数，目前我们选择的是躺平操作，更合理的操作是啥？

---

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