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

文章目录

  • 1.前言
  • 2.从urdf到moveit
  • 3.从urdf到gazebo
    • 3.1.urdf文件的修改
      • 3.1.1.mesh路径
      • 3.1.2.零件起飞
      • 3.1.3.文件保存
    • 3.2.xacro文件的修改
    • 3.3.launch
  • 4.用moveit控制gazebo
  • 5.结语

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】 时是正常的,是可以规划、执行路径的。
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第1张图片

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这边又不认这种绝对路径的。
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第2张图片

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

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

3.1.2.零件起飞

另外,假如直接导入原来的urdf文件,过了一段时间后,会发现机械手的各个零件各自脱离,漫天飞。
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第3张图片
这是仿真的“锅”。惯性参数(inertial)+质量+重力三者一起模拟驱动了各个零件受力,从而发生了相应的运动。
而我们其实大多数情况下,都是希望gazebo把整个机械手当成一个静态刚体来对待。空间中的其他物体只需要与机械手发生碰撞、摩擦计算就行。
有两个办法解决这个问题:
一是可以把原来从SolidWorks导出来的urdf文件的惯性参数改成0。(不太建议使用这个办法,会破坏一致性)(可以看到这里的惯性参数是有问题的,解决方案看这里【sw2urdf导出的urdf文件中的惯性参数(inertial)错误的问题】)
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第4张图片二是可以在后面的xacro文件中取消各个link的重力模拟。(这个可能好一点,这样就相当于没修改原urdf,保证了moveit、gazebo的一致性)
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第5张图片
上面两个办法二选一。

3.1.3.文件保存

因此最后得到的urdf文件为claw_description1.urdf,是从原来的文件claw_description.urdf拷贝修改而来的
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第6张图片
主要就是修改了惯性参数、stl路径表达
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第7张图片

3.2.xacro文件的修改

我们先到moveit_setup_assistant生成的config下面看看,偷偷师。先看看他们原来的重要文件:
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第8张图片这里的核心文件是claw_description.urdf.xacro,其引用、使用了SolidWorks导出的claw_description.urdf以及ros2_control的claw_description.ros2_control.xacro。最终效果就是既使用了描述模型的urdf文件,也定义了ros2_control的相关节点。
因此,我们也可以模仿一下,写一个gazebo_claw_description.urdf.xacro
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第9张图片

<?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 里面的文件写个执行文件吧:
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第10张图片

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

运行一下,得到
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第11张图片还可以。
然后,直接在另外一个控制台运行一下rviz2,在rviz2中看看(注意此时还没启动moveit):
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第12张图片

4.用moveit控制gazebo

还是参考之前的文章【在ROS2中,通过MoveIt2控制Gazebo中的自定义机械手】,5.2节的内容,写一个moveit+rviz的启动文件:gazebo_moveit_rviz.launch.py。
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第13张图片
然后分别打开两个控制台,分别执行:

ros2 launch arm_claw gazebo_moveit_rviz.launch.py 
ros2 launch arm_claw gazebo.launch.py 

然后就可以愉快地玩耍了。
ROS2中,从SolidWorks导出的urdf,联合moveit、gazebo进行控制及仿真_第14张图片

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】

你可能感兴趣的:(ros,ROS2)