0
$\begingroup$

ROS2 Humble | Gazebo Classic 11 | Ubuntu 22.04

Attempting to add a force-torque sensor & plugin to the URDF of a UR5e results in the following launch error, despite the sensor being present in gazebo's auto-generated .world file.

[controller_manager]: Can't activate controller 'robot_admittance_controller': State interface with key 'tcp_fts_sensor/force.x' does not exist

I'm trying to model a UR5e with the F/T end effector and have added the following to my xacro:

    <gazebo>
        <sensor name="tcp_fts_sensor" type="ft">
          <always_on>1</always_on>
          <update_rate>1</update_rate>
          <visualize>true</visualize>
          <topic>wrench</topic>
          <plugin name="tcp_fts_sensor" filename="libgazebo_ros_ft_sensor.so">
            <body_name>wrist_3_link</body_name>
            <update_rate>200</update_rate>
            <gaussian_noise>0.01</gaussian_noise>
          </plugin>
        </sensor>
    </gazebo>

I'm converting the robot description into an SDF and launching gazebo with the following launch commands from a python launch file:

     # Gazebo nodes
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [FindPackageShare("gazebo_ros"), "/launch", "/gazebo.launch.py"]
        ),
    )

    # Spawn robot
    gazebo_spawn_robot = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        name="spawn_ur",
        arguments=["-entity", "my_project", "-topic", "robot_description"],
        output="screen",
    ) 

The launch file brings up a gazebo UI and from there I was able to save off the .world file to assess how well it managed to replicate my URDF and found the following:

      <sensor name='tcp_fts_sensor' type='ft'>
        <always_on>1</always_on>
        <update_rate>1</update_rate>
        <visualize>1</visualize>
        <topic>wrench</topic>
        <plugin filename='libgazebo_ros_ft_sensor.so' name='tcp_fts_sensor'>
          <body_name>wrist_3_link</body_name>
          <update_rate>200</update_rate>
          <gaussian_noise>0.01</gaussian_noise>
        </plugin>
      </sensor>
$\endgroup$

1 Answer 1

0
$\begingroup$

The library you're trying to use was written for the old Gazebo. Thus you cannot use the tcp_fts_sensor (libgazebo_ros_ft_sensor.so) with (Ignition) Gazebo Fortress.
Luckily Gazebo Fortress already comes with an inbuilt ForceTorqueSensor that may fit your needs. The related PR also shows an example (the full SDF specification can be found here):

<gazebo reference="${prefix}_joint">
  <sensor name="force_torque_sensor" type="force_torque">
    <always_on>true</always_on>
    <update_rate>50</update_rate>
    <visualize>true</visualize>
    <topic>${prefix}_joint/force_torque</topic>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
  </sensor>
</gazebo>

Please notice the difference in how the sensors are named in the new Gazebo:

<plugin
  filename="ignition-gazebo-forcetorque-system"
  name="ignition::gazebo::systems::ForceTorque">
</plugin>
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.