Inaccurate FT Sensor Values in Gazebo Classic 11 (22.04 ROS2 Humble)

[jpmartin42]

I am working on a project that uses ROS2 and Gazebo 11; however, after integrating the F/T sensor into the URDF, I'm getting wildly inaccurate data.

I've made the modifications listed in this reference page to get data from the sensor. Without these minor syntax changes, nothing is published, but the data is successfully published over the specified topic post-changes.

To test behavior in isolation, I made the simple test URDF below:

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

  <link name="base_link">
    <inertial>
      <mass value="1"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="0.014" ixy="0" ixz="0" iyy="0.014" iyz="0" izz="0.014"/>
    </inertial>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="0.5 0.5 0.5"/>
        </geometry>
      </collision>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="0.5 0.5 0.5"/>
        </geometry>
      </visual>
  </link>

  <link name="distal_link">
    <inertial>
      <mass value="1"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="0.014" ixy="0" ixz="0" iyy="0.014" iyz="0" izz="0.014"/>
    </inertial>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 1.1"/>
        <geometry>
          <box size="0.5 0.5 0.5"/>
        </geometry>
      </collision>
      <visual>
        <origin rpy="0 0 0" xyz="0 0 1.1"/>
        <geometry>
          <box size="0.5 0.5 0.5"/>
        </geometry>
      </visual>
  </link>

  <joint name="jt" type="continuous">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="distal_link"/>
    <axis xyz= "0 0 1"/>
  </joint>    

<!-- Force-torque plugin at steering joint-->
    <gazebo reference="jt">
      <provideFeedback>true</provideFeedback>
    </gazebo>

    <gazebo>
      <plugin name="ft_sensor_jt" filename="libgazebo_ros_ft_sensor.so">
        <!-- <updateRate>100.0</updateRate>
        <topicName>/ft_sensors/limb_${limb_number}</topicName>
        <jointName>jt</jointName> -->
        <ros>
          <namespace>ft_sensors</namespace>
          <remapping>wrench:=/jt</remapping>
        </ros>
        <joint_name>jt</joint_name>
        <body_name>base_link</body_name>
        <frame_name>distal_link</frame_name>
        <update_rate>50.0</update_rate>
        <gaussian_noise>0.001</gaussian_noise>
      </plugin>
    </gazebo>
</robot>

... and spawned it with a launch file in the default Gazebo world. Given the mass of 1kg on the top joint, I'd expect to be seeing about -9.8 N in the z-direction of the ft sensor; instead, it's about -8, with an additional -7 N in the x-direction. The specific values end up being dependent on what height the robot is spawned at; if spawned inside the ground, for instance, it pops up to ground level as expected but then can give very low magnitudes on the order of 2-4 N in the z-direction.

This same configuration gives accurate results if implemented properly with a ROS1 sensor in a ROS1 workspace (which I implemented on a Ubuntu 20.04 machine), and gives accurate results in ROS2 if I use Gazebo Fortress instead of Gazebo 11. This seems to be an issue only with ROS2 and Gazebo 11 paired together.

Is there a way to fix this or something obvious missing from the URDF? Thanks!

[Related RoboticsStackExchange post]