Gazebo使用筆記（5） —— 力/力矩傳感器的使用

1. 插件的添加

在相應的<joint>標籤內添加如下代碼：

        <sensor name="force_torque" type="force_torque">
          <update_rate>30</update_rate>
        </sensor>

運行world：

gazebo --verbose force_torque_tutorial.world

查看傳感器輸出：

• 法1：命令查看

  gz topic --view /gazebo/default/model_1/joint_01/force_torque/wrench
  

• 法2：Ctrl+T
  
  添加一個力，測試仿真情況，並觀察力/力矩輸出情況：
  
  傾倒以後，受力情況如下：
  
  已知球體質量爲10kg，距離關節1.5m，開始時只受重力影響：

forceJointZ = mass * g
            = 10 kg * -9.8 m/s^2
            = -98 N

給y方向施加500N，傾倒90°後，關節+Y軸指向地面，作用在球體上的重力會繞X軸施加扭矩：

torqueJoint01_x = r X R
                = ||r|| * ||F|| * sin(theta)
                = distanceZ * (massLink * g) * sin(theta)
                = 1.5 m * (10 kg * 9.8 m/s^2) * sin(-90)
                = -147 Nm

注意：根據物理引擎參數，接近關節極限的測量值可能會發生跳躍！問題敘述參見https://github.com/osrf/gazebo/issues/2209

---

力/力矩的添加：
參見Apping Force/Torque

2. SDF參數說明

傳感器通用參數：

SDF傳感器架構參見-http://sdformat.org/spec?ver=1.6&elem=sensor

• <always_on>：如果爲true，則傳感器始終測量力/力矩；如果爲false，則僅在有訂閱者連接到傳感器主題的情況下傳感器纔會更新；通過代碼訪問傳感器時，此設置很重要；如果沒有訂閱者，則調用ForceTorqueSensor::Torque()或ForceTorqueSensor::Force()將返回舊數據；可以通過檢查是否返回來檢測；代碼可以通過調用來更新沒有訂閱者的傳感器。
• <update_rate>：傳感器更新頻率(Hz)，即傳感器每秒發佈的消息數
• <visualize>：如果爲true，則Gazebo client將對關節處的力/力矩進行可視化
• <topic>：力/扭矩傳感器當前不支持該參數
• <frame>：力/扭矩傳感器當前不支持該參數
• <pose>：浮點數，用空格分隔x y z roll pitch yaw，它描述了傳感器座標系相對於父關節的位置

ForceTorqueSensor Class頭文件爲#include <sensors/sensors.hh>

ForceTorque特定參數：

<sensor name="my_cool_sensor" type="force_torque">
  <force_torque>
    <frame>child</frame>
    <measure_direction>child_to_parent</measure_direction>
  </force_torque>
</sensor>

通過添加屬性類型設置爲force_torque的<sensor>標籤來創建力/扭矩傳感器，可以設置兩個附加參數：

• <frame>：其值有三種，child、parent、sensor；其表示力/力矩座標系，parent和child用於指定關節哪端是父節點哪端是子節點（即可用來設置旋轉方向？？？），sensor值意味着測量值是通過該傳感器的<pose>的旋轉分量描述的，姿勢的平移分量對測量沒有影響。

  無論此設置如何，扭矩分量始終以關節座標系的原點表示。

• <measure_direction>：測量方向，嘗試將上面的例子改爲parent_to_child，傾倒以後會發現，​​傳感器測量值在Y軸上的力爲-98 N，在X軸上的扭矩爲+147 Nm，數值與以前相同，但方向相反。

3. 說明

1. 儘管SDF允許將<sensor>標籤放置在link或joint上，但ForceTorqueSensor僅在關節上起作用。如果將傳感器添加到link，則使用--verbose運行gazebo時會報錯：

[Err] [Link.cc:114] A link cannot load a [force_torque] sensor.

2. 以上示例將力/力矩傳感器放置在旋轉關節上，但是實際情況中力/力矩傳感器通常被剛性地安裝在另一個剛體上，真實傳感器無法準確測量旋轉關節起點處的力/力矩。

   ① 如果實際傳感器距離關節足夠近，以至於偏移誤差可忽略，則這種方式建模是合理的；

   
   ② 如果該誤差不可忽略，可以在實際傳感器的位置處通過固定關節將剛體分成兩個joint
   

4. 示例sdf文件

<?xml version="1.0"?>
<sdf version="1.6">
  <world name="default">
    <physics name="default_physics" default="0" type="ode">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
      <ode>
        <solver>
          <type>quick</type>
          <iters>50</iters>
          <sor>1.0</sor> <!-- Important, see issue #2209 -->
          <use_dynamic_moi_rescaling>false</use_dynamic_moi_rescaling>
        </solver>
      </ode>
    </physics>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
    <model name="model_1">
      <link name="link_1">
        <pose>0 0 2.0 0 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.100000</ixx>
            <ixy>0.000000</ixy>
            <ixz>0.000000</ixz>
            <iyy>0.100000</iyy>
            <iyz>0.000000</iyz>
            <izz>0.100000</izz>
          </inertia>
          <mass>10.000000</mass>
        </inertial>
        <visual name="visual_sphere">
          <geometry>
            <sphere>
              <radius>0.100000</radius>
            </sphere>
          </geometry>
        </visual>
        <visual name="visual_cylinder">
          <pose>0 0 -0.75 0 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.0100000</radius>
              <length>1.5</length>
            </cylinder>
          </geometry>
        </visual>
        <collision name="collision_sphere">
          <max_contacts>250</max_contacts>
          <geometry>
            <sphere>
              <radius>0.100000</radius>
            </sphere>
          </geometry>
        </collision>
      </link>
      <joint name="joint_01" type="revolute">
        <parent>world</parent>
        <child>link_1</child>
        <pose>0 0 -1.5 0 0 0</pose>
        <axis>
          <limit>
            <lower>-1.57079</lower>
            <upper>1.57079</upper>
          </limit>
          <dynamics>
            <damping>0.000000</damping>
            <friction>0.000000</friction>
          </dynamics>
          <xyz>1.000000 0.000000 0.000000</xyz>
        </axis>
        <sensor name="force_torque" type="force_torque">
          <update_rate>30</update_rate>
        </sensor>
      </joint>
    </model>
  </world>
</sdf>

---

參考文獻：

• http://gazebosim.org/tutorials?tut=force_torque_sensor&cat=sensors