Skip to content

Commit

Permalink
Add tutorial for gazebo_yarp_forcetorque plugin (#648)
Browse files Browse the repository at this point in the history
  • Loading branch information
@traversaro
traversaro authored Apr 17, 2023
1 parent 4b35f02 commit ea7a48e
Show file tree
Hide file tree
Showing 5 changed files with 186 additions and 0 deletions.
36 changes: 36 additions & 0 deletions tutorial/model/forcetorque/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
# Example of a model using a gazebo_yarp_forcetorque plugin

This directory contains the example of a model using the `gazebo_yarp_forcetorque` plugin.

For more information on simulation of Force/Torque sensors in Gazebo Classic, see [https://classic.gazebosim.org/tutorials?tut=force_torque_sensor&cat=sensors](https://classic.gazebosim.org/tutorials?tut=force_torque_sensor&cat=sensors).


To run this example, first of all run `yarpserver` in a terminal, then make sure to have added `gazebo-yarp-plugins/tutorial/model` directory to `GAZEBO_MODEL_PATH` as documented in the "Example models" section of the `doc/embed_plugins.md` documentation and then run the world:
~~~
gazebo --verbose forcetorque_world.world
~~~

If the simulation starts correctly, you should see a model composed by a cylinder and a box. The FT sensor measures the force exchanged between the cylinder and the box. To check if the YARP plugin works as expected, open a new terminal and type:
~~~
yarp name list
~~~

To list all the YARP ports open. You should see the port `/forcetorque/measures:o`, that is the one opened by the plugin.

You can then read the measures from the the `/forcetorque/measures:o` with the command:
~~~
traversaro@IITICUBLAP257:~$ yarp read ... /forcetorque/measures:o
[INFO] |yarp.os.Port|/tmp/port/1| Port /tmp/port/1 active at tcp://172.22.196.221:10004/
[INFO] |yarp.os.impl.PortCoreInputUnit|/tmp/port/1| Receiving input from /forcetorque/measures:o to /tmp/port/1 using tcp
() () () () () (((0.0 0.0 -98.0000000000000568434 0.0 0.0 0.0) 59.0670000000000001705)) () () () ()
() () () () () (((0.0 0.0 -97.9999999999999573674 0.0 0.0 0.0) 59.542999999999999261)) () () () ()
() () () () () (((0.0 0.0 -98.0000000000000568434 0.0 0.0 0.0) 60.0240000000000009095)) () () () ()
() () () () () (((0.0 0.0 -98.0000000000000568434 0.0 0.0 0.0) 60.5)) () () () ()
() () () () () (((0.0 0.0 -97.9999999999999573674 0.0 0.0 0.0) 60.9759999999999990905)) () () () ()
() () () () () (((0.0 0.0 -97.9999999999999573674 0.0 0.0 0.0) 61.4530000000000029559)) () () () ()
() () () () () (((0.0 0.0 -98.0000000000000568434 0.0 0.0 0.0) 61.93200000000000216)) () () () ()
~~~

There is a parenthesis with six numbers, the first three are force and the last three are torques. In this case, it is possible
to see that the force measure make sense as the weight of the link is 10 Kg, and the acceleration of gravity is 9.8, so the measure
norm on the Z axis is correctly approximately 9.8*10 = 98.0 .
17 changes: 17 additions & 0 deletions tutorial/model/forcetorque/forcetorque_nws.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<robot name="forcetorque" portprefix="forcetorque" build="0" xmlns:xi="http://www.w3.org/2001/XInclude">
<devices>
<device name="forcetorque_nws_yarp" type="multipleanalogsensorsserver">
<!-- See https://www.yarp.it/latest/classMultipleAnalogSensorsServer.html for parameter documentation -->
<param name="name"> /forcetorque </param>
<param name="period"> 100 </param>
<action phase="startup" level="5" type="attach">
<!-- This is the same name that we passed with the yarpDeviceName to the gazebo_yarp_forcetorque plugin -->
<param name="device"> forcetorque_plugin_device </param>
</action>
<action phase="shutdown" level="5" type="detach" />
</device>
</devices>
</robot>
14 changes: 14 additions & 0 deletions tutorial/model/forcetorque/forcetorque_world.world
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://forcetorque</uri>
</include>
</world>
</sdf>
16 changes: 16 additions & 0 deletions tutorial/model/forcetorque/model.config
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
<?xml version="1.0"?>

<model>
<name>ForceTorque</name>
<version>1.0</version>
<sdf>model.sdf</sdf>

<author>
<name>Silvio Traversaro</name>
<email>[email protected]</email>
</author>

<description>
A simple model with an forcetorque sensor exposed via YARP plugins.
</description>
</model>
103 changes: 103 additions & 0 deletions tutorial/model/forcetorque/model.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,103 @@
<?xml version="1.0" ?>
<sdf version="1.7">
<!-- Model inspired from https://github.com/gazebosim/gazebo-classic/blob/gazebo11_11.12.0/worlds/force_torque_demo.world -->
<!-- to demonstrate force torque, we'll construct a model with
two bodies stacked vertically, with a fixed joint connecting them. -->
<model name="model_1">
<link name="link_1">
<inertial>
<pose>0 0 0.05 0 0 0</pose>
<inertia>
<ixx>0.020000</ixx>
<ixy>0.000000</ixy>
<ixz>0.000000</ixz>
<iyy>0.020000</iyy>
<iyz>0.000000</iyz>
<izz>0.020000</izz>
</inertia>
<mass>10.000000</mass>
</inertial>
<visual name="visual_cylinder">
<pose>0 0 0.05 0 0 0</pose>
<geometry>
<cylinder>
<radius>0.100000</radius>
<length>0.100000</length>
</cylinder>
</geometry>
</visual>
<collision name="collision_cylinder">
<pose>0 0 0.05 0 0 0</pose>
<max_contacts>250</max_contacts>
<geometry>
<cylinder>
<radius>0.100000</radius>
<length>0.100000</length>
</cylinder>
</geometry>
</collision>
</link>
<link name="link_2">
<pose>0 0 0.15 0 0 0</pose>
<inertial>
<pose>0 0 0.0 0 0 0</pose>
<inertia>
<ixx>0.020000</ixx>
<ixy>0.000000</ixy>
<ixz>0.000000</ixz>
<iyy>0.020000</iyy>
<iyz>0.000000</iyz>
<izz>0.020000</izz>
</inertia>
<mass>10.000000</mass>
</inertial>
<visual name="visual_box">
<pose>0 0 0.0 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<collision name="collision_box">
<pose>0 0 0.0 0 0 0</pose>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
</link>
<joint name="joint_01" type="fixed">
<parent>world</parent>
<child>link_1</child>
<pose>0 0 0.0 0 0 0</pose>
<sensor name="force_torque" type="force_torque">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>30</update_rate>
</sensor>
</joint>
<joint name="joint_12" type="fixed">
<parent>link_1</parent>
<child>link_2</child>
<!-- joint_1 at origin of link_2 link frame -->
<!-- moement arm from link_2 inertial frame to joint_01 is 2m -->
<!-- moement arm from link_2 inertial frame to joint_12 is 0.5m -->
<pose>0 0 0 0 0 0</pose>
<sensor name="force_torque" type="force_torque">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>30</update_rate>
<!-- Create an instance the gazebo_yarp_forcetorque YARP device called "forcetorque_plugin_device"-->
<plugin name="forcetorque_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationString>(yarpDeviceName forcetorque_plugin_device) (disableImplicitNetworkWrapper)</yarpConfigurationString>
</plugin>
</sensor>
</joint>
<!-- Launch the other YARP devices, in this case a multipleanalogsensorsclient to expose the forcetorque measured on a YARP port -->
<plugin name="robotinterface" filename="libgazebo_yarp_robotinterface.so">
<yarpRobotInterfaceConfigurationFile>model://forcetorque/forcetorque_nws.xml</yarpRobotInterfaceConfigurationFile>
</plugin>
</model>
</sdf>

0 comments on commit ea7a48e

Please sign in to comment.