24 插件读取模型参数和插件参数

一、概述

Gazebo 插件可以读取 URDF 和 SDF 文件中定义的参数，这通常在插件的 Load 方法中实现。当 Gazebo 加载插件时，**它会传递一个指向model元素的指针和一个指向SDF Element的指针给插件。

插件使用SDF Element指针访问 URDF 或 SDF 文件中定义的参数，使用model指针获取模型在gazebo仿真中的数据。

即model指针与gazebo进行交互，SDF Element指针与URDF 或 SDF 文件进行交互。

model指针和SDF Element指针分别可访问不同类别的参数。

• model指针：用来访问模型参数，例如关节名称、尺寸、质量等，还可控制模型运动；

• SDF Element指针：用来访问插件参数，例如话题名、更新频率等。

二、model参数

1.1 什么是model参数？

model参数大致可分为几何参数，动力学参数，关节参数等三方面。

• 1. 几何参数

  • 尺寸：定义机器人各个部件的大小，如长度、宽度、高度。例如，一个机器人臂的长度或者轮子的直径。
  • 形状：描述各个部件的几何形状，如立方体、球体、圆柱体等。

• 2. 动力学参数

  • 质量：机器人各部件的质量。
  • 惯性矩阵：描述物体对旋转的抵抗程度，对于计算物体如何在受力时旋转非常重要。

• 3. 关节参数

  • 关节类型：如旋转关节（revolute）、滑动关节（prismatic）等。
  • 限制：关节的运动范围限制，例如旋转关节的最大和最小旋转角度。
  • 弹簧和阻尼系数：影响关节运动的物理特性。

2.2 model参数在URDF和SDF中位置

• URDF文件中的model参数(两个link，一个joint)

<?xml version="1.0"?>
<robot name="simple_robot">
  <!-- Base Link -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="1 1 0.1"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 1"/>
      </material>
    </visual>
  </link>

  <!-- Top Link -->
  <link name="top_link">
    <visual>
      <geometry>
        <cylinder radius="0.5" length="0.2"/>
      </geometry>
      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
  </link>

  <!-- Fixed Joint -->
  <joint name="fixed_joint" type="fixed">
    <parent link="base_link"/>
    <child link="top_link"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
  </joint>
</robot>

• SDF文件中的model参数(两个link，一个joint)

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="simple_robot">
    <!-- Base Link -->
    <link name="base_link">
      <visual name="base_visual">
        <geometry>
          <box>
            <size>1 1 0.1</size>
          </box>
        </geometry>
        <material>
          <ambient>0 0 1 1</ambient>
        </material>
      </visual>
    </link>

    <!-- Top Link -->
    <link name="top_link">
      <visual name="top_visual">
        <geometry>
          <cylinder>
            <radius>0.5</radius>
            <length>0.2</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>1 0 0 1</ambient>
        </material>
      </visual>
    </link>

    <!-- Fixed Joint -->
    <joint name="fixed_joint" type="fixed">
      <parent>base_link</parent>
      <child>top_link</child>
      <pose>0 0 0.05 0 0 0</pose>
    </joint>
  </model>
</sdf>

2.3 插件访问model参数

插件通过Load函数的ModelPtr指针访问模型在gazebo中的joint和link。

public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // 获取模型名称
  std::string modelName = _model->GetName();

  // 获取模型的位置
  ignition::math::Pose3d modelPose = _model->WorldPose();

  // 遍历模型的所有关节
  for (unsigned int i = 0; i < _model->GetJointCount(); ++i)
  {
    auto joint = _model->GetJoint(i);
    std::string jointName = joint->GetName();
    // 处理每个关节名称...
  }

  // 获取特定关节
  auto joint = _model->GetJoint("joint_name");
  if (joint)
  {
    // 获取关节类型
    std::string jointType = joint->GetTypeStr();

    // 获取关节限制（如果有的话）
    if (joint->HasUpperLimit(0) && joint->HasLowerLimit(0))
    {
      double lowerLimit = joint->LowerLimit(0);
      double upperLimit = joint->UpperLimit(0);
      // 处理关节限制...
    }
  }

  // 获取特定链接
  auto link = _model->GetLink("link_name");
  if (link)
  {
    // 获取链接的质量
    double mass = link->GetInertial()->Mass();

    // 获取链接的惯性矩阵
    auto inertia = link->GetInertial()->InertiaMatrix();
    // 处理质量和惯性...
  }

}

可以看到，插件通过ModelPtr仿真model的joint和link，并可以获取每个joint和link相关的参数。

三、插件参数

model参数多被用于gazebo进行物理仿真，而插件参数多为算法等配置参数，是重点

3.1 什么是插件参数？

插件参数通过是在<plugin>标签下的参数，通常是自定义的标签参数，也有一些常用的配置参数如话题名、更新频率、自定义属性、特定设置等。

3.2 插件参数在URDF 和 SDF 文件的位置

在URDF文件中，插件参数为<plugin>标签下的参数。

<robot name="my_robot">
  <!-- 模型的其他定义 -->

  <!-- 插件定义 -->
  <gazebo>
    <plugin name="example_plugin" filename="libexample_plugin.so">
      <param1>value1</param1>
      <param2>value2</param2>
      <vector_param>1 2 3</vector_param>
    </plugin>
  </gazebo>
</robot>

在SDF文件中，插件参数为<plugin>标签下的参数。

<sdf version="1.6">
    <model name="my_robot_model">
        <!-- 模型的其他定义部分 -->

        <plugin name="my_control_plugin" filename="libmy_control_plugin.so">
            <param1>value1</param1>
            <param2>value2</param2>
            <vector_param>1 2 3</vector_param>
        </plugin>
    </model>
</sdf>

3.3 读取插件参数

插件通过Load函数中的 ElementPtr指针访问URDF和SDF中的插件参数。

这些插件配置参数可以是多种类型，如简单的bool、int、float、double、string等类型，也可以是向量类型等，如下所示。

<plugin name="example_plugin" filename="libexample_plugin.so">
  <param1>some_string</param1>
  <param2>42</param2>
  <param3>3.14</param3>
  <param4>true</param4>
  <position>1 2 3</position>
  <param_color>1 0 0 1</param_color>
  <orientation>0 0 0 1</orientation>
</plugin>

插件内部处理代码如下：

public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
      // 确保 SDF 元素存在
      if (!_sdf)
      {
        gzerr << "No SDF element found. Plugin won't load." << std::endl;
        return;
      }

      // 读取布尔值参数
      if (_sdf->HasElement("param_bool"))
      {
        bool param_bool = _sdf->Get<bool>("param_bool");
        gzmsg << "Param bool: " << param_bool << std::endl;
      }

      // 读取整型参数
      if (_sdf->HasElement("param_int"))
      {
        int param_int = _sdf->Get<int>("param_int");
        gzmsg << "Param int: " << param_int << std::endl;
      }

      // 读取浮点型参数
      if (_sdf->HasElement("param_float"))
      {
        float param_float = _sdf->Get<float>("param_float");
        gzmsg << "Param float: " << param_float << std::endl;
      }

      // 读取双精度浮点型参数
      if (_sdf->HasElement("param_double"))
      {
        double param_double = _sdf->Get<double>("param_double");
        gzmsg << "Param double: " << param_double << std::endl;
      }

      // 读取字符串参数
      if (_sdf->HasElement("param_string"))
      {
        std::string param_string = _sdf->Get<std::string>("param_string");
        gzmsg << "Param string: " << param_string << std::endl;
      }

      // 读取向量参数
      if (_sdf->HasElement("param_vector"))
      {
        ignition::math::Vector3d param_vector = _sdf->Get<ignition::math::Vector3d>("param_vector");
        gzmsg << "Param vector: " << param_vector << std::endl;
      }

      // 读取 color 参数
      if (_sdf->HasElement("param_color"))
      {
        ignition::math::Color color = _sdf->Get<ignition::math::Color>("param_color");
      }

       // 读取 rotation 参数
      if (_sdf->HasElement("param_rotation"))
      {
        ignition::math::Quaterniond rotation = _sdf->Get<ignition::math::Quaterniond>("param_rotation");
      }
}

3.4 复杂例子

1.多个子参数

<plugin name="my_plugin" filename="libmy_plugin.so">
  <param_list>
    <value>1</value>
    <value>2</value>
    <value>3</value>
    <!-- 更多的值 -->
  </param_list>
</plugin>

通过GetNextElement函数进行读取：

void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf){
      // 确保 SDF 元素存在
      if (!_sdf){
        gzerr << "No SDF element found. Plugin won't load." << std::endl;
        return;
      }

      // 读取列表参数
      if (_sdf->HasElement("param_list")){
        sdf::ElementPtr listElem = _sdf->GetElement("param_list");
        
        std::vector<int> values;
        while (listElem->HasElement("value")){
          int val = listElem->Get<int>("value");
          values.push_back(val);

          listElem = listElem->GetNextElement("value");
        }
      }
}

2.连续参数

<plugin name="my_plugin" filename="libmy_plugin.so">
  <param_list>
    <value>1,2,3,4,5,6,7,8</value>
  </param_list>
</plugin>

使用 std::getline 和 std::stringstream 来分割字符串。对于每个分割后的子字符串，将其转换为整数并将其添加到 values 数组中。

    void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
      // 确保 SDF 元素存在
      if (!_sdf)
      {
        gzerr << "No SDF element found. Plugin won't load." << std::endl;
        return;
      }

      // 读取并解析列表参数
      if (_sdf->HasElement("param_list"))
      {
        std::string valueList = _sdf->Get<std::string>("param_list");

        std::vector<int> values;
        std::stringstream ss(valueList);
        std::string item;
        while (std::getline(ss, item, ',')) // 使用逗号作为分隔符
        {
          try
          {
            int value = std::stoi(item);
            values.push_back(value);
          }
          catch (const std::invalid_argument& ia)
          {
            gzerr << "Invalid argument: " << ia.what() << '\n';
          }
        }

        // 使用 values 数组
        for (int val : values)
        {
          gzmsg << "Value: " << val << std::endl;
        }
      }
    }

四、附录：插件源码及URDF、SDF文件

• URDF模型

<?xml version="1.0"?>
<robot name="simple_robot">
  <!-- Base Link -->
  <link name="base_link">
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <visual>
      <geometry>
        <box size="1 1 0.1"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 1"/>
      </material>
    </visual>
  </link>

  <!-- Top Link -->
  <link name="top_link">
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.5" length="0.2"/>
      </geometry>
      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
  </link>

  <!-- Fixed Joint -->
  <joint name="fixed_joint" type="fixed">
    <parent link="base_link"/>
    <child link="top_link"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
  </joint>
  
  <gazebo>
    <plugin name="my_plugin" filename="libmy_gazebo_plugin.so">
      <param1>some_string</param1>
      <param2>42</param2>
      <param3>3.14</param3>
      <param4>true</param4>
      <position>1 2 3</position>
      <param_color>1 0 0 1</param_color>
      <orientation>0 0 0 1</orientation>
    </plugin>
  </gazebo>

</robot>

• SDF模型

<?xml version="1.0"?>
<sdf version="1.6">
  <model name="simple_robot">
    <!-- Base Link -->
    <link name="base_link">
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
      </inertial>
      <visual name="base_visual">
        <geometry>
          <box>
            <size>1 1 0.1</size>
          </box>
        </geometry>
        <material>
          <ambient>0 0 1 1</ambient>
        </material>
      </visual>
    </link>

    <!-- Top Link -->
    <link name="top_link">
      <inertial>
        <mass>1.0</mass>
        <inertia>
          <ixx>0.1</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.1</iyy>
          <iyz>0</iyz>
          <izz>0.1</izz>
        </inertia>
      </inertial>
      <visual name="top_visual">
        <geometry>
          <cylinder>
            <radius>0.5</radius>
            <length>0.2</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>1 0 0 1</ambient>
        </material>
      </visual>
    </link>

    <!-- Fixed Joint -->
    <joint name="fixed_joint" type="fixed">
      <parent>base_link</parent>
      <child>top_link</child>
      <pose>0 0 0.05 0 0 0</pose>
    </joint>

    <plugin name="my_plugin" filename="libmy_gazebo_plugin.so">
      <param1>some_string</param1>
      <param2>42</param2>
      <param3>3.14</param3>
      <param4>true</param4>
      <position>1 2 3</position>
      <param_color>1 0 0 1</param_color>
      <orientation>0 0 0 1</orientation>
    </plugin>
  </model>
</sdf>

在 URDF 中，<mass> 是一个带有 value 属性的自闭合标签；而在 SDF 中，<mass> 是一个包含数据的子标签，但是插件对其处理的方式是一样的，包括inertia标签。