使用Gazebo和ROS2仿真闭环机器人

Contents

URDF的局限在于它无法表示闭环机器人：指的是结构中存在闭环的机器人。主要原因是，现在并没有有效的闭环运动学求解器，这里指的是解析解。不过SDF是支持闭环结构的，因为Gazebo支持闭环运动学求解，用的可能是一种近似算法。所以在Gazebo的帮助下，我们可以基于闭环机器人的URDF进行仿真，并在Rviz2中显示。遗憾的是Ignition尚不支持闭环结构，所以只能用Gazebo，或者用Gazebo开发组的话来说，Gazebo Classic。

demo：https://gitee.com/linx981002/closed_loop_kinematic_demo

这个项目还包括我自己绘制的一个吊车机器人，觉得有用的话，在这里留言或在gitee上给一个Star吧。

拆环

获得一个闭环机器人的URDF。拆掉闭环中的一个关节，让机器人变成一个开环机器人。一般来说，闭环结构中肯定会有一个无动力的关节（有的时候可能不止一个），拆掉这个关节即可。在URDF中，则体现为删除一个joint描述。

注入Gazebo描述

将上一步删除的joint添加上<gazebo>标签加回URDF中。这使得在我们使用gazebo_spawner通过ros topic——robot_description生成实体的时候，URDF再转换为SDF的过程中，这一块joint会被添加回生成的SDF中，从而补全闭环。如下：

  <gazebo>
    <joint name="boom_support" type="revolute">
      <child>hydraulic_rod</child>
      <parent>main_boom</parent> 
    </joint>
  </gazebo>

需要注意的是，在这一过程中，是无法指定旋转轴心点的，轴心点默认为子link的基准点。

注意：由于Gazebo在寻找Mesh上存在一些不足，因此在URDF中的Mesh路径最好全部换成绝对路径。如果发现在Gazebo中生成的实体不可见，但是切换到Joint显示模式，能够看到关节的frame，那很可能是URDF中mesh路径的问题。

注入gazebo_ros2_control描述，添加ros2_control配置

目前我们只在Gazebo中生成了一个闭环的机器人，我们更希望能够用Rviz监控它的状态。因此我们使用gazebo_ros2_control来将Gazebo中机器人的状态发布到ros topic中。

插一句，gazebo_ros2_control的原理实际上是继承了ros2_control的hadware_interface，重写了read和write函数。一种可能的实现方式是，它不断通过Gazebo的API获取信息，然后通过write函数发布到ros的某个topic上。我么有具体看过代码，上面只是猜想。

在插入描述的时候，注意被删去的关节不应当加入，因为在ros中，并不存在这个关节。这个概念需要谨记，在ros中，这个机器人依然是一个开环机器人。

如果只是发布状态的话，只需要使用joint_state_broadcaster这个controller。

controller_manager:
  ros__parameters:
    update_rate: 100  # H
 
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

我使用xacro来管理这块内容。简洁起见我只列了一个关节。这里的yaml文件可以使用find，因为这一块内容只在ros中起作用，不会通过Gazebo。

<?xml version="1.0"?>
<robot 
    xmlns:xacro="http://www.ros.org/wiki/xacro" >  

    <!-- Macro for gazebo simulation and ros2_control -->
        <gazebo>
            <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
                <parameters>$(find sany)/ros2_control_config/robot_state_publisher.yaml</parameters>
            </plugin>
        </gazebo>

        <ros2_control name="GazeboSystem" type="system">
            <hardware>
                <plugin>gazebo_ros2_control/GazeboSystem</plugin>
            </hardware>
        <!-- Main joints -->
        <joint    name="rotate_joint"    type="revolute">
            <command_interface name="position">
                <param name="min">-1</param>
                <param name="max">1</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>
        </ros2_control>

</robot>

Launch文件

在Launch中，我们需要依次进行以下操作：

发布URDF到/robot_description topic；
启动Gazebo
在Gazebo中生成Entity
生成Controller

一个已知问题是，如果直接在launch中启动rviz，会出现模型丢失的情况，手动启动就不会有这样的问题。具体的实现可见代码。

实现效果

懒得上传了，有人看到并感兴趣的话就留言吧。