From 3c77a9af1c4bba6d424170b44a726075d57cb515 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Mon, 12 Feb 2024 15:35:36 +0100
Subject: [PATCH] ergoCubV1* regenerate all the models with the imus in the
 feet

---
 .../ERGOCUB_all_options_gazebo.yaml           | 68 +++++++++++++++++++
 .../ERGOCUB_all_options_minContacts.yaml      | 68 +++++++++++++++++++
 .../ERGOCUB_all_options_gazebo.yaml           | 68 +++++++++++++++++++
 .../ERGOCUB_all_options_minContacts.yaml      | 68 +++++++++++++++++++
 .../ergoCub/robots/ergoCubGazeboV1/model.urdf | 56 +++++++++++++++
 .../robots/ergoCubGazeboV1_1/model.urdf       | 56 +++++++++++++++
 .../ergoCubGazeboV1_1_minContacts/model.urdf  | 56 +++++++++++++++
 .../ergoCubGazeboV1_minContacts/model.urdf    | 56 +++++++++++++++
 urdf/ergoCub/robots/ergoCubSN000/model.urdf   | 56 +++++++++++++++
 urdf/ergoCub/robots/ergoCubSN001/model.urdf   | 56 +++++++++++++++
 10 files changed, 608 insertions(+)

diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml
index 874fab86..ca0a7d50 100644
--- a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml
+++ b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml
@@ -1233,6 +1233,74 @@ reverseRotationAxis:
     r_pinkie_dist
 
 XMLBlobs:
+    # left foot IMU (front)
+    - |
+            <gazebo reference="l_ankle_2">
+              <sensor name="l_foot_front_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="l_foot_front_ft_imu" type="accelerometer">
+              <parent link="l_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+            </sensor>
+    # left foot IMU (rear)
+    - |
+            <gazebo reference="l_ankle_2">
+              <sensor name="l_foot_rear_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+              <parent link="l_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+            </sensor>
+    # right foot IMU (front)
+    - |
+            <gazebo reference="r_ankle_2">
+              <sensor name="r_foot_front_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="r_foot_front_ft_imu" type="accelerometer">
+              <parent link="r_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+            </sensor>
+    # right foot IMU (rear)
+    - |
+            <gazebo reference="r_ankle_2">
+              <sensor name="r_foot_rear_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+              <parent link="r_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+            </sensor>
     # imu waist (workaround since simmechanics was not updated)
     - |
             <link name="waist_imu_0"/>
diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml
index ab484604..8acb4648 100644
--- a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml
+++ b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml
@@ -1542,6 +1542,74 @@ reverseRotationAxis:
     r_pinkie_dist
 
 XMLBlobs:
+    # left foot IMU (front)
+    - |
+            <gazebo reference="l_ankle_2">
+              <sensor name="l_foot_front_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="l_foot_front_ft_imu" type="accelerometer">
+              <parent link="l_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+            </sensor>
+    # left foot IMU (rear)
+    - |
+            <gazebo reference="l_ankle_2">
+              <sensor name="l_foot_rear_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+              <parent link="l_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+            </sensor>
+    # right foot IMU (front)
+    - |
+            <gazebo reference="r_ankle_2">
+              <sensor name="r_foot_front_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="r_foot_front_ft_imu" type="accelerometer">
+              <parent link="r_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+            </sensor>
+    # right foot IMU (rear)
+    - |
+            <gazebo reference="r_ankle_2">
+              <sensor name="r_foot_rear_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+              <parent link="r_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+            </sensor>
     # imu waist (workaround since simmechanics was not updated)
     - |
             <link name="waist_imu_0"/>
diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
index dd046544..e6f55b0d 100644
--- a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
+++ b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
@@ -1246,6 +1246,74 @@ reverseRotationAxis:
     r_pinkie_prox
 
 XMLBlobs:
+    # left foot IMU (front)
+    - |
+            <gazebo reference="l_ankle_2">
+              <sensor name="l_foot_front_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="l_foot_front_ft_imu" type="accelerometer">
+              <parent link="l_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+            </sensor>
+    # left foot IMU (rear)
+    - |
+            <gazebo reference="l_ankle_2">
+              <sensor name="l_foot_rear_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+              <parent link="l_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+            </sensor>
+    # right foot IMU (front)
+    - |
+            <gazebo reference="r_ankle_2">
+              <sensor name="r_foot_front_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="r_foot_front_ft_imu" type="accelerometer">
+              <parent link="r_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+            </sensor>
+    # right foot IMU (rear)
+    - |
+            <gazebo reference="r_ankle_2">
+              <sensor name="r_foot_rear_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+              <parent link="r_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+            </sensor>
     # imu waist (workaround since simmechanics was not updated)
     - |
             <link name="waist_imu_0"/>
diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
index e081828f..075febb8 100644
--- a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
+++ b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
@@ -1546,6 +1546,74 @@ reverseRotationAxis:
     r_pinkie_prox
 
 XMLBlobs:
+    # left foot IMU (front)
+    - |
+            <gazebo reference="l_ankle_2">
+              <sensor name="l_foot_front_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="l_foot_front_ft_imu" type="accelerometer">
+              <parent link="l_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+            </sensor>
+    # left foot IMU (rear)
+    - |
+            <gazebo reference="l_ankle_2">
+              <sensor name="l_foot_rear_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+              <parent link="l_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+            </sensor>
+    # right foot IMU (front)
+    - |
+            <gazebo reference="r_ankle_2">
+              <sensor name="r_foot_front_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="r_foot_front_ft_imu" type="accelerometer">
+              <parent link="r_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+            </sensor>
+    # right foot IMU (rear)
+    - |
+            <gazebo reference="r_ankle_2">
+              <sensor name="r_foot_rear_ft_imu" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+              <parent link="r_ankle_2" />
+              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+            </sensor>
     # imu waist (workaround since simmechanics was not updated)
     - |
             <link name="waist_imu_0"/>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf
index 3a2e6f09..ba776969 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf
@@ -2893,6 +2893,62 @@
     <child link="l_hand_pinkie_skin_11"/>
     <dynamics damping="0.1"/>
   </joint>
+    <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_front_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_front_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
   <link name="waist_imu_0"/>
   <joint name="waist_imu_0_fixed_joint" type="fixed">
   <origin xyz="0.0354497 0.00639688 0.060600" rpy="1.5708 0 -1.5708"/>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
index 09175cef..32b029f0 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
@@ -2950,6 +2950,62 @@
     <parent link="r_hand_thumb_3"/>
     <child link="r_hand_thumb_skin_9"/>
   </joint>
+    <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_front_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_front_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
   <link name="waist_imu_0"/>
   <joint name="waist_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.0354497 0.00639688 0.060600" rpy="1.5708 0 -1.5708"/>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
index 87088e73..814da2c6 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
@@ -2764,6 +2764,62 @@
     <parent link="r_hand_thumb_3"/>
     <child link="r_hand_thumb_skin_9"/>
   </joint>
+    <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_front_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_front_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
   <link name="waist_imu_0"/>
   <joint name="waist_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.0354497 0.00639688 0.060600" rpy="1.5708 0 -1.5708"/>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf
index d654f7b5..e3c46d24 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf
@@ -2893,6 +2893,62 @@
     <child link="l_hand_pinkie_skin_11"/>
     <dynamics damping="0.1"/>
   </joint>
+    <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_front_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_front_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
   <link name="waist_imu_0"/>
   <joint name="waist_imu_0_fixed_joint" type="fixed">
   <origin xyz="0.0354497 0.00639688 0.060600" rpy="1.5708 0 -1.5708"/>
diff --git a/urdf/ergoCub/robots/ergoCubSN000/model.urdf b/urdf/ergoCub/robots/ergoCubSN000/model.urdf
index cbf786d5..3537f021 100644
--- a/urdf/ergoCub/robots/ergoCubSN000/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubSN000/model.urdf
@@ -2893,6 +2893,62 @@
     <child link="l_hand_pinkie_skin_11"/>
     <dynamics damping="0.1"/>
   </joint>
+    <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_front_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_front_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
   <link name="waist_imu_0"/>
   <joint name="waist_imu_0_fixed_joint" type="fixed">
   <origin xyz="0.0354497 0.00639688 0.060600" rpy="1.5708 0 -1.5708"/>
diff --git a/urdf/ergoCub/robots/ergoCubSN001/model.urdf b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
index 74a25c8f..d7423496 100644
--- a/urdf/ergoCub/robots/ergoCubSN001/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
@@ -2950,6 +2950,62 @@
     <parent link="r_hand_thumb_3"/>
     <child link="r_hand_thumb_skin_9"/>
   </joint>
+    <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_front_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="l_ankle_2">
+    <sensor name="l_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="l_foot_rear_ft_imu" type="accelerometer">
+    <parent link="l_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_front_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_front_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.13525 0 -0.07019999999999993" />
+  </sensor>
+  <gazebo reference="r_ankle_2">
+    <sensor name="r_foot_rear_ft_imu" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100</update_rate>
+      <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
+      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+      </plugin>
+    </sensor>
+  </gazebo>
+  <sensor name="r_foot_rear_ft_imu" type="accelerometer">
+    <parent link="r_ankle_2" />
+    <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
+  </sensor>
   <link name="waist_imu_0"/>
   <joint name="waist_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.0354497 0.00639688 0.060600" rpy="1.5708 0 -1.5708"/>