0
$\begingroup$

Gazebo Answers logo

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/force-torque-sensor/blob/master/model/ft_sensor.gazebo.xacro http://docs.ros.org/jade/api/gazebo_plugins/html/group__GazeboRosFTSensor.html

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.

Originally posted by lr101095 on Gazebo Answers with karma: 1 on 2018-03-07

Post score: 0

Original comments

Comment by lr101095 on 2018-03-08:
@josephcoombe @sloretz

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

Comment by josephcoombe on 2018-03-09:
@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!

Comment by lr101095 on 2018-03-11:
@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)

Comment by josephcoombe on 2018-03-12:
@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

Comment by josephcoombe on 2018-03-12:
@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.

Comment by lr101095 on 2018-03-13:
@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

Comment by lr101095 on 2018-03-13:
@josephcoombe I'm also not sure what to do for the pkg part in 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.

Comment by josephcoombe on 2018-03-14:
@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).

Comment by josephcoombe on 2018-03-14:
@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).

Improve this question
$\endgroup$

2 Answers 2

Reset to default
0
$\begingroup$

Gazebo Answers logo

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

Originally posted by sloretz with karma: 558 on 2018-03-08

This answer was NOT ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$
0
$\begingroup$

Gazebo Answers logo

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.

Example:

my_robot.urdf

...
  <gazebo reference="joint_name">
    <sensor name="joint_name_force_torque" type="force_torque">
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <visualize>true</visualize>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
...

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();
  pub.publish(msgWrenchedStamped);
}

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());
  node->Init();

  // 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
  while(ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
  gazebo::shutdown();
  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
add_compile_options(-std=c++11)
# Find Gazebo
find_package(gazebo REQUIRED)

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

link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(include)
include_directories( ${catkin_INCLUDE_DIRS}
                     ${Boost_INCLUDE_DIR}
                     ${GAZEBO_INCLUDE_DIRS}
)

target_link_libraries(
  ${PROJECT_NAME}_gazebo_transport_to_ros_topic
  ${catkin_LIBRARIES}
  ${GAZEBO_LIBRARIES}
)
...

my_robot.launch

...
  <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" />
  </node>
...

Originally posted by josephcoombe with karma: 609 on 2018-03-08

This answer was NOT ACCEPTED on the original site

Post score: 2

Original comments

Comment by amjack0 on 2020-07-16:
Hi ! Thanks for the answer, works for me with ros melodic with ur5 robot. But the data which is published by the node is very noisy. which looks something like this,

wrench {  force {
x: -85.820636519165689
y: 207.09115865293248
z: 1255.8442285124843 }

torque {
x: 51.407552691283492
y: -18.956931496699944
z: 5.5317985187868786 }

Now, what are these values resembles in regards to force and torque ?

Comment by amjack0 on 2020-08-15:
Here is my working example of the force torque plugin on universal robot5.

https://github.com/amjack0/force-torque-sensor

Comment by marobot.fit on 2021-02-22:
I want to ask if you found how to filter this noisy output, and in which frame of the rigid body these values were computed ? thanks :)

Comment by josephcoombe on 2021-03-01:
Hi, the force/torque data are very noisy - that seems to be an unavoidable consequence of the physics engine. Possible workarounds include a) filtering the data b) decreasing simulator step size (which will also slow things down).

Improve this answer
$\endgroup$

Your Answer

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