From ea7a48e7b20822c8feb0bc4a42ffe40ed130389f Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Mon, 17 Apr 2023 13:58:51 +0200
Subject: [PATCH] Add tutorial for gazebo_yarp_forcetorque plugin (#648)

---
 tutorial/model/forcetorque/README.md          |  36 ++++++
 .../model/forcetorque/forcetorque_nws.xml     |  17 +++
 .../model/forcetorque/forcetorque_world.world |  14 +++
 tutorial/model/forcetorque/model.config       |  16 +++
 tutorial/model/forcetorque/model.sdf          | 103 ++++++++++++++++++
 5 files changed, 186 insertions(+)
 create mode 100644 tutorial/model/forcetorque/README.md
 create mode 100644 tutorial/model/forcetorque/forcetorque_nws.xml
 create mode 100644 tutorial/model/forcetorque/forcetorque_world.world
 create mode 100644 tutorial/model/forcetorque/model.config
 create mode 100644 tutorial/model/forcetorque/model.sdf

diff --git a/tutorial/model/forcetorque/README.md b/tutorial/model/forcetorque/README.md
new file mode 100644
index 000000000..3bf2ee268
--- /dev/null
+++ b/tutorial/model/forcetorque/README.md
@@ -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 .
\ No newline at end of file
diff --git a/tutorial/model/forcetorque/forcetorque_nws.xml b/tutorial/model/forcetorque/forcetorque_nws.xml
new file mode 100644
index 000000000..d56fbd04e
--- /dev/null
+++ b/tutorial/model/forcetorque/forcetorque_nws.xml
@@ -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>
diff --git a/tutorial/model/forcetorque/forcetorque_world.world b/tutorial/model/forcetorque/forcetorque_world.world
new file mode 100644
index 000000000..2e3b04d4a
--- /dev/null
+++ b/tutorial/model/forcetorque/forcetorque_world.world
@@ -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>
diff --git a/tutorial/model/forcetorque/model.config b/tutorial/model/forcetorque/model.config
new file mode 100644
index 000000000..551c7d0b8
--- /dev/null
+++ b/tutorial/model/forcetorque/model.config
@@ -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>silvio.traversaro@iit.it</email>
+  </author>
+
+  <description>
+    A simple model with an forcetorque sensor exposed via YARP plugins.
+  </description>
+</model>
diff --git a/tutorial/model/forcetorque/model.sdf b/tutorial/model/forcetorque/model.sdf
new file mode 100644
index 000000000..81edb0406
--- /dev/null
+++ b/tutorial/model/forcetorque/model.sdf
@@ -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>