diff --git a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options.yaml b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options.yaml
index b95f6dc4..7e62adc9 100644
--- a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options.yaml
+++ b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options.yaml
@@ -1132,6 +1132,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_1">
             <sensor name="waist_imu_0" type="imu">
@@ -1176,6 +1182,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1262,13 +1274,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
-
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="l_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1342,12 +1389,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1374,12 +1457,36 @@ XMLBlobs:
             </gazebo>
 
     # head
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+              <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="neck_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1401,12 +1508,31 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="l_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1443,12 +1569,31 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1483,3 +1628,19 @@ XMLBlobs:
                 <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
               </plugin>
             </gazebo>
+    - |
+            <gazebo>
+              <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+                <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+              </plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"></plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"></plugin>
+            </gazebo>
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 ca0a7d50..45a611a7 100644
--- a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml
+++ b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_gazebo.yaml
@@ -1317,6 +1317,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_1">
             <sensor name="waist_imu_0" type="imu">
@@ -1361,6 +1367,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1447,12 +1459,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
 
     - |
             <gazebo reference="l_hip_pitch">
@@ -1527,12 +1575,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1559,12 +1643,36 @@ XMLBlobs:
             </gazebo>
 
     # head
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+              <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_head_without_eyes" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="neck_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1586,12 +1694,31 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="l_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1628,12 +1755,31 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1668,3 +1814,19 @@ XMLBlobs:
                 <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
               </plugin>
             </gazebo>
+    - |
+            <gazebo>
+              <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+                <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+              </plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"></plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"></plugin>
+            </gazebo>
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 8acb4648..5cd97855 100644
--- a/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml
+++ b/urdf/creo2urdf/data/ergocub1_0/ERGOCUB_all_options_minContacts.yaml
@@ -1626,6 +1626,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_1">
             <sensor name="waist_imu_0" type="imu">
@@ -1670,6 +1676,12 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1756,13 +1768,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
-
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="l_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1836,12 +1883,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1868,12 +1951,36 @@ XMLBlobs:
             </gazebo>
 
     # head
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+              <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_head_without_eyes" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="neck_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1895,12 +2002,31 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_0.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="l_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1937,12 +2063,31 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_0.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1977,3 +2122,19 @@ XMLBlobs:
                 <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
               </plugin>
             </gazebo>
+    - |
+            <gazebo>
+              <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+                <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+              </plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"></plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"></plugin>
+            </gazebo>
diff --git a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml
index 397a8396..0606d5b2 100644
--- a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml
+++ b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml
@@ -1121,6 +1121,12 @@ XMLBlobs:
             </plugin>
             </sensor>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>            
     - |
             <sensor name="waist_imu_0" type="accelerometer">
             <parent link="torso_1"/>
@@ -1148,12 +1154,30 @@ XMLBlobs:
             <!-- For compatibility with SDFormat < 4.4 -->
             <disableFixedJointLumping>true</disableFixedJointLumping>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1240,12 +1264,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
 
     - |
             <gazebo reference="l_hip_pitch">
@@ -1320,12 +1380,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1352,12 +1448,36 @@ XMLBlobs:
             </gazebo>
 
     # head
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+              <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="neck_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1379,6 +1499,13 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
@@ -1421,6 +1548,13 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
@@ -1461,3 +1595,19 @@ XMLBlobs:
                 <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
               </plugin>
             </gazebo>
+    - |
+            <gazebo>
+              <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+                <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+              </plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"></plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"></plugin>
+            </gazebo>
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 06fedb9e..e53e40cf 100644
--- a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
+++ b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
@@ -1018,7 +1018,6 @@ forceTorqueSensors:
             <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
           </plugin>
 
-
 sensors:
   - frameName: SCSYS_HEAD_IMU
     linkName: head
@@ -1194,7 +1193,6 @@ sensors:
         <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>
-
 assignedMasses:
  torso_1   : 2.0645667
 
@@ -1306,16 +1304,23 @@ XMLBlobs:
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+
     - |
             <gazebo reference="torso_1">
-            <sensor name="waist_imu_0" type="imu">
-            <always_on>1</always_on>
-            <update_rate>100</update_rate>
-            <pose>0.0354497 0.00639688 0.060600 1.5708 0 -1.5708</pose>
-            <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
-            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+              <sensor name="waist_imu_0" type="imu">
+                <always_on>1</always_on>
+                <update_rate>100</update_rate>
+                <pose>0.0354497 0.00639688 0.060600 1.5708 0 -1.5708</pose>
+                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
+                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+                </plugin>
+              </sensor>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
             </plugin>
-            </sensor>
             </gazebo>
     - |
             <sensor name="waist_imu_0" type="accelerometer">
@@ -1344,12 +1349,30 @@ XMLBlobs:
             <!-- For compatibility with SDFormat < 4.4 -->
             <disableFixedJointLumping>true</disableFixedJointLumping>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1436,6 +1459,37 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
@@ -1443,6 +1497,13 @@ XMLBlobs:
             </plugin>
             </gazebo>
 
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+
     - |
             <gazebo reference="l_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1516,12 +1577,50 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+
     - |
             <gazebo reference="r_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1548,12 +1647,38 @@ XMLBlobs:
             </gazebo>
 
     # head
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+              <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+
     - |
             <gazebo reference="neck_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1575,12 +1700,22 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
+
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+
     - |
             <gazebo reference="l_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1617,12 +1752,24 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
+
+
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+
+
     - |
             <gazebo reference="r_shoulder_pitch">
               <implicitSpringDamper>1</implicitSpringDamper>
@@ -1657,3 +1804,20 @@ XMLBlobs:
                 <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
               </plugin>
             </gazebo>
+
+    - |
+            <gazebo>
+              <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+                <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+              </plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"></plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"></plugin>
+            </gazebo>
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 6ad9a53f..37538a33 100644
--- a/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
+++ b/urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
@@ -1622,6 +1622,12 @@ XMLBlobs:
             <parent link="torso_1"/>
             <origin rpy="1.5708 0 -1.5708" xyz="0.0354497 0.00639688 0.060600"/>
             </sensor>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     # upperbody
     - |
             <link name="base_link" />
@@ -1644,12 +1650,30 @@ XMLBlobs:
             <!-- For compatibility with SDFormat < 4.4 -->
             <disableFixedJointLumping>true</disableFixedJointLumping>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
               <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="torso_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1736,12 +1760,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
 
     - |
             <gazebo reference="l_hip_pitch">
@@ -1816,12 +1876,48 @@ XMLBlobs:
               <mu2>1</mu2>
               <fdir1>0 0 0</fdir1>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+              <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="r_hip_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1848,12 +1944,36 @@ XMLBlobs:
             </gazebo>
 
     # head
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+              <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
             <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+            </plugin>
+            </gazebo>
     - |
             <gazebo reference="neck_pitch">
                 <implicitSpringDamper>1</implicitSpringDamper>
@@ -1875,6 +1995,13 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
@@ -1917,6 +2044,13 @@ XMLBlobs:
             <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
             </plugin>
             </gazebo>
+    - |
+            <gazebo>
+            <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+              <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+              <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+            </plugin>
+            </gazebo>
     - |
             <gazebo>
             <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
@@ -1957,3 +2091,19 @@ XMLBlobs:
                 <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
               </plugin>
             </gazebo>
+    - |
+            <gazebo>
+              <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+                <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+              </plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"></plugin>
+            </gazebo>
+
+    - |
+            <gazebo>
+              <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"></plugin>
+            </gazebo>
diff --git a/urdf/ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini b/urdf/ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini
index 7ed2e1e0..7f240166 100644
--- a/urdf/ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini
+++ b/urdf/ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName ergocub_left_arm_ft
+jointName l_arm_ft_sensor
+sensorName l_arm_ft
diff --git a/urdf/ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini b/urdf/ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini
index 61b6fd1c..0a56e748 100644
--- a/urdf/ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini
+++ b/urdf/ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName ergocub_left_foot_front_ft
+jointName l_foot_front_ft_sensor
+sensorName l_foot_front_ft
diff --git a/urdf/ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini b/urdf/ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini
index 07673c07..d24e7bc6 100644
--- a/urdf/ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini
+++ b/urdf/ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName ergocub_left_foot_rear_ft
+jointName l_foot_rear_ft_sensor
+sensorName l_foot_rear_ft
diff --git a/urdf/ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini b/urdf/ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini
index 8de05340..476f2c3b 100644
--- a/urdf/ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini
+++ b/urdf/ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName ergocub_left_leg_ft
+jointName l_leg_ft_sensor
+sensorName l_leg_ft
diff --git a/urdf/ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini b/urdf/ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini
index 418412f8..5967c7db 100644
--- a/urdf/ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini
+++ b/urdf/ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName ergocub_right_arm_ft
+jointName r_arm_ft_sensor
+sensorName r_arm_ft
diff --git a/urdf/ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini b/urdf/ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini
index d5ea8a80..e830963b 100644
--- a/urdf/ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini
+++ b/urdf/ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName ergocub_right_foot_front_ft
+jointName r_foot_front_ft_sensor
+sensorName r_foot_front_ft
diff --git a/urdf/ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini b/urdf/ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini
index d1473ff1..e3bc0962 100644
--- a/urdf/ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini
+++ b/urdf/ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName ergocub_right_foot_rear_ft
+jointName r_foot_rear_ft_sensor
+sensorName r_foot_rear_ft
diff --git a/urdf/ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini b/urdf/ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini
index 9a4f56e3..2abacaba 100644
--- a/urdf/ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini
+++ b/urdf/ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini
@@ -1,3 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName ergocub_right_leg_ft
-
+jointName r_leg_ft_sensor
+sensorName r_leg_ft
diff --git a/urdf/ergoCub/conf/ergocub_gz_sim.xml b/urdf/ergoCub/conf/ergocub_gz_sim.xml
new file mode 100644
index 00000000..812d7ec2
--- /dev/null
+++ b/urdf/ergoCub/conf/ergocub_gz_sim.xml
@@ -0,0 +1,43 @@
+<?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="ergocubGazebo" build="1" portprefix="/ergocubSim" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <devices>
+
+    <!-- MOTOR CONTROLLERS -->
+    <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/left_leg-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/right_leg-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/head-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/torso-mc_wrapper.xml" />
+
+    <xi:include href="wrappers/motorControl/left_arm-mc_remapper_gz_sim.xml" />
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper_gz_sim.xml" />
+    <xi:include href="wrappers/motorControl/left_leg-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/right_leg-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/head-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/torso-mc_remapper.xml" />
+    <!--  ANALOG SENSORS FT -->
+    <xi:include href="wrappers/FT/left_leg-FT_remapper.xml" />
+    <xi:include href="wrappers/FT/right_leg-FT_remapper.xml" />
+    <xi:include href="wrappers/FT/left_leg-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/right_leg-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" />
+
+    <!-- INERTIAL SENSOR-->
+    <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
+    <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" />
+    <xi:include href="wrappers/inertials/left_foot-IMU_remapper.xml" />
+    <xi:include href="wrappers/inertials/left_foot-IMU_wrapper.xml" />
+    <xi:include href="wrappers/inertials/right_foot-IMU_remapper.xml" />
+    <xi:include href="wrappers/inertials/right_foot-IMU_wrapper.xml" />
+
+    <!--  HEAD SENSORS -->
+    <!-- <xi:include href="sensors/rgbd_camera_wrapper.xml" /> -->
+    <xi:include href="sensors/lidar_wrapper.xml" />
+
+    </devices>
+</robot>
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_inertial.ini
index 5202ca3a..0e60e7b5 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName head_inertial_hardware_device
+parentLinkName head
+sensorName head_imu_0
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini
index 03340fc5..dacd682b 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName l_foot_front_ft_sensor_inertial_hardware_device
+parentLinkName l_foot_front
+sensorName l_foot_front_ft_imu
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini
index 9b839a47..398b49b9 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName l_foot_rear_ft_sensor_inertial_hardware_device
+parentLinkName l_foot_rear
+sensorName l_foot_rear_ft_imu
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini
index 001f24d8..b59777a2 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_r_foot_front_ft_sensor_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName r_foot_front_ft_sensor_inertial_hardware_device
+parentLinkName r_foot_front
+sensorName r_foot_front_ft_imu
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini
index 005d0a80..060a1544 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_r_foot_rear_ft_sensor_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName r_foot_rear_ft_sensor_inertial_hardware_device
+parentLinkName r_foot_rear
+sensorName r_foot_rear_ft_imu
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_realsense_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_realsense_inertial.ini
index 1210d57a..66f7e956 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_realsense_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_realsense_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
 yarpDeviceName realsense_inertial_hardware_device
+parentLinkName realsense
+sensorName realsense_imu_0
diff --git a/urdf/ergoCub/conf/gazebo_ergocub_waist_inertial.ini b/urdf/ergoCub/conf/gazebo_ergocub_waist_inertial.ini
index 1c03fc88..2077362d 100644
--- a/urdf/ergoCub/conf/gazebo_ergocub_waist_inertial.ini
+++ b/urdf/ergoCub/conf/gazebo_ergocub_waist_inertial.ini
@@ -1,2 +1,4 @@
 disableImplicitNetworkWrapper
-yarpDeviceName waist_inertial_hardware_device
\ No newline at end of file
+yarpDeviceName waist_inertial_hardware_device
+parentLinkName torso_1
+sensorName waist_imu_0
diff --git a/urdf/ergoCub/conf/sensors/gazebo_ergocub_lidar.ini b/urdf/ergoCub/conf/sensors/gazebo_ergocub_lidar.ini
index d7ce67f6..7b72a7d3 100644
--- a/urdf/ergoCub/conf/sensors/gazebo_ergocub_lidar.ini
+++ b/urdf/ergoCub/conf/sensors/gazebo_ergocub_lidar.ini
@@ -1,5 +1,7 @@
 yarpDeviceName las360
 disableImplicitNetworkWrapper
+sensorName lasersensor_head
+parentLinkName head
 period 10
 
 [SENSOR]
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/left_arm-mc_remapper_gz_sim.xml b/urdf/ergoCub/conf/wrappers/motorControl/left_arm-mc_remapper_gz_sim.xml
new file mode 100644
index 00000000..7a9aee81
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/left_arm-mc_remapper_gz_sim.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_arm-mc_remapper"
+    type="controlboardremapper">
+    <paramlist name="networks">
+        <elem name="left_arm_joints1">( 0 6 0 6 )</elem>
+        <!-- <elem name="left_arm_joints2">( 7 12 0 5 )</elem> -->
+    </paramlist>
+    <param name="joints"> 7 </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="left_arm_joints1"> left_arm_hardware_device </elem>
+            <!-- <elem name="left_arm_joints2"> left_hand_hardware_device </elem> -->
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/right_arm-mc_remapper_gz_sim.xml b/urdf/ergoCub/conf/wrappers/motorControl/right_arm-mc_remapper_gz_sim.xml
new file mode 100644
index 00000000..d559bd03
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/right_arm-mc_remapper_gz_sim.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-mc_remapper"
+    type="controlboardremapper">
+    <paramlist name="networks">
+        <elem name="right_arm_joints1">( 0 6 0 6 )</elem>
+        <!-- <elem name="right_arm_joints2">( 7 12 0 5 )</elem> -->
+    </paramlist>
+    <param name="joints"> 7 </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="right_arm_joints1"> right_arm_hardware_device </elem>
+            <!-- <elem name="right_arm_joints2"> right_hand_hardware_device </elem> -->
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
index ffd8141d..96d6497d 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
@@ -1241,8 +1241,8 @@
     </collision>
   </link>
   <joint name="l_index_add" type="revolute">
-    <origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
-    <axis xyz="1.3005559878087045e-15 1.0000000000000002 -1.953448495705706e-15"/>
+    <origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
+    <axis xyz="1.1895336853461886e-15 0.9999999999999999 -1.9812040713213337e-15"/>
     <parent link="l_hand_palm"/>
     <child link="l_hand_index_1"/>
     <limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
@@ -1571,8 +1571,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
-    <axis xyz="-1.0000000000000002 2.7755575615627613e-17 7.023654366444809e-10"/>
+    <origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
+    <axis xyz="-1 8.326672684688522e-17 7.023654435833748e-10"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1604,8 +1604,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
-    <axis xyz="-1 -1.804112664227406e-16 9.020562801946248e-17"/>
+    <origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
+    <axis xyz="-1 -2.359224176539984e-16 4.163337069211178e-17"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
@@ -2972,6 +2972,11 @@
       </plugin>
     </sensor>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <sensor name="waist_imu_0" type="accelerometer">
     <parent link="torso_1"/>
     <origin rpy="1.5708 0 -1.5708" xyz="0.0354497 0.00639688 0.060600"/>
@@ -2993,11 +2998,26 @@
     <!-- For compatibility with SDFormat < 4.4 -->
     <disableFixedJointLumping>true</disableFixedJointLumping>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="torso_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3076,11 +3096,41 @@
     <mu2>1</mu2>
     <fdir1>0 0 0</fdir1>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="l_hip_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3150,11 +3200,41 @@
     <mu2>1</mu2>
     <fdir1>0 0 0</fdir1>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="r_hip_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3173,11 +3253,31 @@
   <gazebo reference="r_ankle_roll">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+      <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="neck_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3193,6 +3293,12 @@
       <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini</yarpConfigurationFile>
@@ -3225,6 +3331,12 @@
       <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini</yarpConfigurationFile>
@@ -3256,6 +3368,17 @@
       <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+      <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
+  </gazebo>
+  <gazebo>
+    <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
+  </gazebo>
   <gazebo>
     <pose>0.000000 0.000000 0.800000 0.000000 0.000000 0.000000</pose>
   </gazebo>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
index 5288b4a1..c0e94795 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
@@ -1127,8 +1127,8 @@
     </collision>
   </link>
   <joint name="l_index_add" type="revolute">
-    <origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
-    <axis xyz="1.3005559878087045e-15 1.0000000000000002 -1.953448495705706e-15"/>
+    <origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
+    <axis xyz="1.1895336853461886e-15 0.9999999999999999 -1.9812040713213337e-15"/>
     <parent link="l_hand_palm"/>
     <child link="l_hand_index_1"/>
     <limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
@@ -1427,8 +1427,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
-    <axis xyz="-1.0000000000000002 2.7755575615627613e-17 7.023654366444809e-10"/>
+    <origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
+    <axis xyz="-1 8.326672684688522e-17 7.023654435833748e-10"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1457,8 +1457,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
-    <axis xyz="-1 -1.804112664227406e-16 9.020562801946248e-17"/>
+    <origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
+    <axis xyz="-1 -2.359224176539984e-16 4.163337069211178e-17"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
@@ -2790,6 +2790,11 @@
     <parent link="torso_1"/>
     <origin rpy="1.5708 0 -1.5708" xyz="0.0354497 0.00639688 0.060600"/>
   </sensor>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <link name="base_link"/>
   <joint name="base_link_fixed_joint" type="fixed">
     <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2807,11 +2812,26 @@
     <!-- For compatibility with SDFormat < 4.4 -->
     <disableFixedJointLumping>true</disableFixedJointLumping>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="torso_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -2890,11 +2910,41 @@
     <mu2>1</mu2>
     <fdir1>0 0 0</fdir1>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="l_hip_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -2964,11 +3014,41 @@
     <mu2>1</mu2>
     <fdir1>0 0 0</fdir1>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="r_hip_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -2987,11 +3067,31 @@
   <gazebo reference="r_ankle_roll">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+      <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="neck_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3007,6 +3107,12 @@
       <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini</yarpConfigurationFile>
@@ -3039,6 +3145,12 @@
       <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini</yarpConfigurationFile>
@@ -3070,6 +3182,17 @@
       <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+      <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
+  </gazebo>
+  <gazebo>
+    <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
+  </gazebo>
   <gazebo>
     <pose>0.000000 0.000000 0.800000 0.000000 0.000000 0.000000</pose>
   </gazebo>
diff --git a/urdf/ergoCub/robots/ergoCubSN001/model.urdf b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
index 4de41b4c..b5287a34 100644
--- a/urdf/ergoCub/robots/ergoCubSN001/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
@@ -1241,8 +1241,8 @@
     </collision>
   </link>
   <joint name="l_index_add" type="revolute">
-    <origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
-    <axis xyz="1.3005559878087045e-15 1.0000000000000002 -1.953448495705706e-15"/>
+    <origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
+    <axis xyz="1.1895336853461886e-15 0.9999999999999999 -1.9812040713213337e-15"/>
     <parent link="l_hand_palm"/>
     <child link="l_hand_index_1"/>
     <limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
@@ -1571,8 +1571,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
-    <axis xyz="-1.0000000000000002 2.7755575615627613e-17 7.023654366444809e-10"/>
+    <origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
+    <axis xyz="-1 8.326672684688522e-17 7.023654435833748e-10"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1604,8 +1604,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
-    <axis xyz="-1 -1.804112664227406e-16 9.020562801946248e-17"/>
+    <origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
+    <axis xyz="-1 -2.359224176539984e-16 4.163337069211178e-17"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
@@ -2972,6 +2972,11 @@
       </plugin>
     </sensor>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_waist_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <sensor name="waist_imu_0" type="accelerometer">
     <parent link="torso_1"/>
     <origin rpy="1.5708 0 -1.5708" xyz="0.0354497 0.00639688 0.060600"/>
@@ -2993,11 +2998,26 @@
     <!-- For compatibility with SDFormat < 4.4 -->
     <disableFixedJointLumping>true</disableFixedJointLumping>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_torso.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="torso_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3076,11 +3096,41 @@
     <mu2>1</mu2>
     <fdir1>0 0 0</fdir1>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_leg.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="l_hip_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3150,11 +3200,41 @@
     <mu2>1</mu2>
     <fdir1>0 0 0</fdir1>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ForceTorque" filename="gz-sim-yarp-forcetorque-system">
+      <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_r_foot_front_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_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_leg.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="r_hip_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3173,11 +3253,31 @@
   <gazebo reference="r_ankle_roll">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Imu" filename="gz-sim-yarp-imu-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::Laser" filename="gz-sim-yarp-laser-system">
+      <yarpConfigurationFile>model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_head.ini</yarpConfigurationFile>
+    </plugin>
+  </gazebo>
   <gazebo reference="neck_pitch">
     <implicitSpringDamper>1</implicitSpringDamper>
   </gazebo>
@@ -3193,6 +3293,12 @@
       <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_arm.ini</yarpConfigurationFile>
+      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_left_hand" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_left_hand_mk5_1.ini</yarpConfigurationFile>
@@ -3225,6 +3331,12 @@
       <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_arm.ini</yarpConfigurationFile>
+      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
+    </plugin>
+  </gazebo>
   <gazebo>
     <plugin name="controlboard_right_hand" filename="libgazebo_yarp_controlboard.so">
       <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_right_hand_mk5_1.ini</yarpConfigurationFile>
@@ -3256,6 +3368,17 @@
       <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
     </plugin>
   </gazebo>
+  <gazebo>
+    <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
+      <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub_gz_sim.xml</yarpRobotInterfaceConfigurationFile>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
+  </gazebo>
+  <gazebo>
+    <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu"/>
+  </gazebo>
   <gazebo>
     <pose>0.000000 0.000000 0.800000 0.000000 0.000000 0.000000</pose>
   </gazebo>