From b29370289b87cfc190f5c11598a204d13f71ee21 Mon Sep 17 00:00:00 2001
From: Alessandro Croci <alessandro.croci@iit.it>
Date: Tue, 6 Aug 2024 18:06:42 +0200
Subject: [PATCH 1/3] Update arm and leg imu ini files

---
 urdf/ergoCub/conf/gazebo_ergocub_l_arm_ft_sensor_inertial.ini | 4 +++-
 urdf/ergoCub/conf/gazebo_ergocub_l_leg_ft_sensor_inertial.ini | 2 ++
 urdf/ergoCub/conf/gazebo_ergocub_r_arm_ft_sensor_inertial.ini | 2 ++
 urdf/ergoCub/conf/gazebo_ergocub_r_leg_ft_sensor_inertial.ini | 2 ++
 4 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/urdf/ergoCub/conf/gazebo_ergocub_l_arm_ft_sensor_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_l_arm_ft_sensor_inertial.ini
index e84c4a64..cea3121e 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_l_arm_ft_sensor_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_l_arm_ft_sensor_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
-yarpDeviceName l_arm_ft_sensor_inertial_hardware_device
\ No newline at end of file
+yarpDeviceName l_arm_ft_sensor_inertial_hardware_device
+parentLinkName l_arm
+sensorName l_arm_ft_imu
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_l_leg_ft_sensor_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_l_leg_ft_sensor_inertial.ini
index adb2fdb8..156d0359 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_l_leg_ft_sensor_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_l_leg_ft_sensor_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName l_leg_ft_sensor_inertial_hardware_device
+parentLinkName l_leg
+sensorName l_leg_ft_imu
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_r_arm_ft_sensor_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_r_arm_ft_sensor_inertial.ini
index 69ec0ef8..4f3aefa8 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_r_arm_ft_sensor_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_r_arm_ft_sensor_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName r_arm_ft_sensor_inertial_hardware_device
+parentLinkName r_arm
+sensorName r_arm_ft_imu
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_r_leg_ft_sensor_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_r_leg_ft_sensor_inertial.ini
index 0240158b..69fa26a8 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_r_leg_ft_sensor_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_r_leg_ft_sensor_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName r_leg_ft_sensor_inertial_hardware_device
+parentLinkName r_leg
+sensorName r_leg_ft_imu

From c6050dce844fddc2e06d519977c747a30024521c Mon Sep 17 00:00:00 2001
From: Alessandro Croci <alessandro.croci@iit.it>
Date: Tue, 6 Aug 2024 18:14:21 +0200
Subject: [PATCH 2/3] Add missing IMU `gz-sim-yarp-plugins` for `ergocub1_1`

---
 urdf/creo2urdf/data/ergocub1_1/base_1_1.yaml | 26 +++++++++++++++++++-
 1 file changed, 25 insertions(+), 1 deletion(-)

diff --git a/urdf/creo2urdf/data/ergocub1_1/base_1_1.yaml b/urdf/creo2urdf/data/ergocub1_1/base_1_1.yaml
index 715e435a..c584baec 100644
--- a/urdf/creo2urdf/data/ergocub1_1/base_1_1.yaml
+++ b/urdf/creo2urdf/data/ergocub1_1/base_1_1.yaml
@@ -1170,7 +1170,7 @@ XMLBlobs:
             <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
             </plugin>
-            </gazebo>            
+            </gazebo>
     - |
             <sensor name="waist_imu_0" type="accelerometer">
             <parent link="torso_1"/>
@@ -1210,6 +1210,18 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_arm_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_arm_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
@@ -1338,6 +1350,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_leg_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
@@ -1454,6 +1472,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_leg_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">

From 716da7cb87beebb022aa66398f9bc544d8c74e97 Mon Sep 17 00:00:00 2001
From: Alessandro Croci <alessandro.croci@iit.it>
Date: Wed, 7 Aug 2024 11:30:07 +0200
Subject: [PATCH 3/3] Add missing IMU `gz-sim-yarp-plugin` for `ergocub1_0`

---
 urdf/creo2urdf/data/ergocub1_0/base_1_0.yaml | 24 ++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/urdf/creo2urdf/data/ergocub1_0/base_1_0.yaml b/urdf/creo2urdf/data/ergocub1_0/base_1_0.yaml
index 3050ba29..a726a870 100644
--- a/urdf/creo2urdf/data/ergocub1_0/base_1_0.yaml
+++ b/urdf/creo2urdf/data/ergocub1_0/base_1_0.yaml
@@ -1377,6 +1377,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_leg_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
@@ -1492,6 +1498,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_leg_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
@@ -1606,6 +1618,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_arm_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="l_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1667,6 +1685,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_arm_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>