Home | Tutorials | Wiki | Issues
Ask Your Question

Does anyone have a working example or a tutorial for a force_torque plugin?

asked 2018-03-07 18:15:02 -0600

lr101095 gravatar image

updated 2018-03-08 17:04:32 -0600

I'm currently trying to implement a force_torque sensor to the end-effector of a UR5. I've found examples of F/T plugins being used but i cant seem to implement them properly. I would try to check if my plugins are valid using rostopic list but the plugin i'm working on doesnt show.

I've looked at these two examples of using F/T plugins, but i'm not sure if i'm using them correctly. https://github.com/CentroEPiaggio/for... http://docs.ros.org/jade/api/gazebo_p...

I can't seem to find a tutorial for an F/T plugin specifically, nor can i find a tutorial to implement the F/T to a robot end-effector.

I'm using the UR5 model found in the ros-kinetic-universal-robot package, I'm currently modifying the ur5.urdf.xacro and ur.gazebo.xacro files since i'm still learning. later on i'd like to write a separate xacro file that contains the sensors and the plugins.

If someone can point me in the right direction, it would be greatly appreciated.

edit retag flag offensive close merge delete


@josephcoombe@sloretz are there any methods you are aware of that can be inserted into the robot.urdf or ur.gazebo?

lr101095 gravatar imagelr101095 ( 2018-03-08 16:41:54 -0600 )edit

@ir101095 Sorry, I don't understand what you mean by "methods". Could you clarify? Whichever force/torque plugin you end up using - I recommend testing it on a simple .urdf first, and then adding it to a more complicated robot once you've proved to yourself that it works as you expect!

josephcoombe gravatar imagejosephcoombe ( 2018-03-09 11:14:06 -0600 )edit

@josephcoombe what i meant by "methods" was instead of writing up a plugin as a separate file and in a c++ and then calling it later, do either of you know of an example that involves inserting a section of code directly into the urdf of the universal robot that sets up the plugin? not sure if i clarified enough. I've been trying to look for force_torque examples that have been implemented directly into the robot packages (urdf, launch and gazebo files etc)

lr101095 gravatar imagelr101095 ( 2018-03-11 18:14:07 -0600 )edit

@Ir101095 In order to get force-torque joint data, you have to use a Gazebo Plugin defined via C++. However, you do not necessary need to write that Plugin yourself because there are already 2 force/torque plugins (that I know of): http://gazebosim.org/tutorials?tut=force_torque_sensor&cat=sensors and https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp

josephcoombe gravatar imagejosephcoombe ( 2018-03-12 08:45:39 -0600 )edit

@lr101095 in my answer below, I set up the Force/Torque Sensor plugin in my .urdf - have you tried using my answer as a template in your own robot? You should have all the pieces. The Force/Torque sensor is included in Gazebo 7 - you just need to declare it in your .urdf - and I've provided the code for a gazebo_transport_to_ros node. Let me know if that answers your question or not.

josephcoombe gravatar imagejosephcoombe ( 2018-03-12 09:06:29 -0600 )edit

@josephcoombe how do i call your gazebo_transport_to_ros_topic.cpp and make sure that it' actually using that program to setup the plugin? i'm not sure which directory i should be putting the cpp in. i've already included your templates for urdf and the launch and i have edited them as needed. but the ros node still doesnt show up in rostopic list

lr101095 gravatar imagelr101095 ( 2018-03-13 19:48:05 -0600 )edit

@josephcoombe I'm also not sure what to do for the pkg part in <node name="joint_name" pkg="my_pkg_gazebo" type="gazebo_transport_to_ros_topic" output="screen" launch-prefix="xterm -e"> I'm assuming this package is where the cpp may be found, but as mentioned above, i'm not sure where to put the cpp and where to call it from.

lr101095 gravatar imagelr101095 ( 2018-03-13 21:15:55 -0600 )edit

@lr101095 the gazebo_transport_to_ros_topic.cpp should be called via a .launch file as shown in my answer. My gazebo_transport_to_ros_topic.cpp node DOES NOT setup the Gazebo Force/Torque Plugin. The Gazebo Force/Torque Plugin gets set up automatically by Gazebo when the urdf is loaded via the gazebo_ros spawn_urdf node (not shown in my example). You should place the .cpp file in the src directory of a ROS Package - also amend your CMakeLists (I've updated my answer).

josephcoombe gravatar imagejosephcoombe ( 2018-03-14 09:34:55 -0600 )edit

@lr101095 the pkg attribute should be the package where you placed the .cpp file (my_pkg_gazebo/src/gazebo_transport_to_ros_topic.cpp) - make sure you modify that package's CMakeLists.txt to properly compile a C++ node (I've amended my answer slightly).

josephcoombe gravatar imagejosephcoombe ( 2018-03-14 09:38:56 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2018-03-08 15:02:13 -0600

updated 2018-03-14 09:30:28 -0600

An alternative to writing
a Gazebo Plugin that publishes force/torque data to a ROS Topic is to write
a ROS Node that listens to the Gazebo Transport layer and republishes force/torque data to a ROS Topic.

Uses the Gazebo Force/Torque Sensor.

@sloretz's answer is probably better. I did it this way - in my case - because I wanted to access some options that were not available in the gazebo_ros force/torque plugin.



  <gazebo reference="joint_name">
    <sensor name="joint_name_force_torque" type="force_torque">

gazebo_transport_to_ros_topic.cpp (place in src directory of ROS package)

*  Name: gazebo_transport_to_ros_topic.cpp
*  Author: Joseph Coombe
*  Date: 11/22/2017
*  Edited: 11/27/2017
*  Description:
*   Subscribe to a Gazebo transport topic and publish to a ROS topic

// Gazebo dependencies
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>

// ROS dependencies
#include <ros/ros.h>
#include <geometry_msgs/WrenchStamped.h>

#include <iostream>

ros::Publisher pub;

void torquesCb(const ConstWrenchStampedPtr &_msg)
  std::cout << "Received msg: " << std::endl;
  std::cout << _msg->DebugString() << std::endl;
  geometry_msgs::WrenchStamped msgWrenchedStamped;
  // try WrenchStamped msgWrenchedStamped;
  msgWrenchedStamped.header.stamp = ros::Time::now();
  msgWrenchedStamped.wrench.force.x = _msg->wrench().force().x();
  msgWrenchedStamped.wrench.force.y = _msg->wrench().force().y();
  msgWrenchedStamped.wrench.force.z = _msg->wrench().force().z();
  msgWrenchedStamped.wrench.torque.x = _msg->wrench().torque().x();
  msgWrenchedStamped.wrench.torque.y = _msg->wrench().torque().y();
  msgWrenchedStamped.wrench.torque.z = _msg->wrench().torque().z();

int main(int argc, char **argv)
  ROS_INFO("Starting gazebo_transport_to_ros_topic node");

  // Load Gazebo
  gazebo::client::setup(argc, argv);

  // Load ROS
  ros::init(argc, argv, "gazebo_transport_to_ros_topic");

  // Create Gazebo node and init
  gazebo::transport::NodePtr node(new gazebo::transport::Node());

  // Create ROS node and init
  ros::NodeHandle n;

  // Get ROS params
  std::string gazebo_transport_topic_to_sub;
  std::string ros_topic_to_pub;
  double ros_rate;
  ros::param::get("~gazebo_transport_topic_to_sub", gazebo_transport_topic_to_sub);
  ros::param::get("~ros_rate", ros_rate);

  pub = n.advertise<geometry_msgs::WrenchStamped>(ros_topic_to_pub, 100);

  // Listen to Gazebo force_torque sensor topic
  gazebo::transport::SubscriberPtr sub = node->Subscribe(gazebo_transport_topic_to_sub, torquesCb);
  ros::Rate loop_rate(ros_rate); // 100 hz
  return 0;

Make sure to add the following sections to your CMakeLists.txt (sorry this is a bit messy):

## Compile as C++11, supported in ROS Kinetic and newer
# Find Gazebo
find_package(gazebo REQUIRED)

set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")

include_directories( ${catkin_INCLUDE_DIRS}



  <node name="joint_name" pkg="my_pkg_gazebo" type="gazebo_transport_to_ros_topic"
        output="screen" launch-prefix="xterm -e" >
    <param name="gazebo_transport_topic_to_sub" type="str" value="/gazebo/default/robot_name/joint_name/joint_name_force_torque/wrench" />
    <param name="ros_topic_to_pub" type="str" value="force_torque/robot_name/joint_name_force_torque" />
    <param name="ros_rate" type="double" value="100.0" />
edit flag offensive delete link more

answered 2018-03-08 10:47:53 -0600

sloretz gravatar image

Here's a tutorial about using force/torque sensors in gazebo.

The tutorial shows how to get data from a gazebo topic. You'll want a gazebo plugin that publishes force/torque data to a ROS topic instead which is what the plugin you linked does.

It might help to read

edit flag offensive delete link more
Login/Signup to Answer

Question Tools


Asked: 2018-03-07 18:15:02 -0600

Seen: 106 times

Last updated: Mar 14