diff --git a/iCub/robots/iCubDarmstadt01/model.urdf b/iCub/robots/iCubDarmstadt01/model.urdf
index d7a31d4..59a373d 100644
--- a/iCub/robots/iCubDarmstadt01/model.urdf
+++ b/iCub/robots/iCubDarmstadt01/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubErzelli02/model.urdf b/iCub/robots/iCubErzelli02/model.urdf
index d282267..667753b 100644
--- a/iCub/robots/iCubErzelli02/model.urdf
+++ b/iCub/robots/iCubErzelli02/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubGazeboV2_5/model.urdf b/iCub/robots/iCubGazeboV2_5/model.urdf
index 6ba4416..4420e85 100644
--- a/iCub/robots/iCubGazeboV2_5/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubGazeboV2_5_KIT_007/model.urdf b/iCub/robots/iCubGazeboV2_5_KIT_007/model.urdf
index 96448f1..84da6da 100644
--- a/iCub/robots/iCubGazeboV2_5_KIT_007/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5_KIT_007/model.urdf
@@ -28,6 +28,13 @@
     <child link="root_link_ems_gyro_eb5"/>
     <dynamics damping="0.1"/>
   </joint>
+  <link name="root_link_imu_frame"/>
+  <joint name="root_link_imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.08515499999999998 -0.011000000000000022 -0.11230899999999999" rpy="-2.0943952105869323 0 -1.5707963267948963"/>
+    <parent link="root_link"/>
+    <child link="root_link_imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="root_link_imu_acc"/>
   <joint name="root_link_imu_acc_fixed_joint" type="fixed">
     <origin xyz="0.08515499999999998 -0.011000000000000022 -0.11230899999999999" rpy="-2.0943952105869323 0 -1.5707963267948963"/>
@@ -243,6 +250,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
@@ -340,6 +354,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -767,6 +788,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubGazeboV2_6/model.urdf b/iCub/robots/iCubGazeboV2_6/model.urdf
index c97f3f8..72c1106 100644
--- a/iCub/robots/iCubGazeboV2_6/model.urdf
+++ b/iCub/robots/iCubGazeboV2_6/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="-1.5707963267948997 0 -3.141592653589793"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="-1.5707963267948997 0 -3.141592653589793"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubGazeboV2_7/model.urdf b/iCub/robots/iCubGazeboV2_7/model.urdf
index ce65f88..759dad3 100644
--- a/iCub/robots/iCubGazeboV2_7/model.urdf
+++ b/iCub/robots/iCubGazeboV2_7/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="-1.5707963267948997 0 -3.141592653589793"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="-1.5707963267948997 0 -3.141592653589793"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="1.0" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubGazeboV3/model.urdf b/iCub/robots/iCubGazeboV3/model.urdf
index 2c91507..3cf29bd 100644
--- a/iCub/robots/iCubGazeboV3/model.urdf
+++ b/iCub/robots/iCubGazeboV3/model.urdf
@@ -403,6 +403,13 @@
     <limit effort="50000" lower="-0.7504915783575618" upper="0.7504915783575618" velocity="120.0"/>
     <dynamics damping="2.0" friction="0.0"/>
   </joint>
+  <link name="chest_depth_frame"/>
+  <joint name="chest_depth_frame_fixed_joint" type="fixed">
+    <origin xyz="0.0894115 0.0175 0.09929950000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
+    <parent link="chest"/>
+    <child link="chest_depth_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="chest_imu_0"/>
   <joint name="chest_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.0894115 0.0175 0.09929950000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
@@ -535,6 +542,13 @@
     <limit effort="50000" lower="-0.7853981633974483" upper="0.7853981633974483" velocity="120.0"/>
     <dynamics damping="2.0" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.0035000000000000005 0.0055 0.10133400000000001" rpy="3.141592653589793 0 -1.5707963267948966"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.0035000000000000005 0.0055 0.10133400000000001" rpy="3.141592653589793 0 -1.5707963267948966"/>
diff --git a/iCub/robots/iCubGenova01/model.urdf b/iCub/robots/iCubGenova01/model.urdf
index d7a31d4..59a373d 100644
--- a/iCub/robots/iCubGenova01/model.urdf
+++ b/iCub/robots/iCubGenova01/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubGenova02/model.urdf b/iCub/robots/iCubGenova02/model.urdf
index 717a02c..d65d5c5 100644
--- a/iCub/robots/iCubGenova02/model.urdf
+++ b/iCub/robots/iCubGenova02/model.urdf
@@ -28,6 +28,13 @@
     <child link="root_link_ems_gyro_eb5"/>
     <dynamics damping="0.1"/>
   </joint>
+  <link name="root_link_imu_frame"/>
+  <joint name="root_link_imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.08515499999999998 -0.011000000000000022 -0.11230899999999999" rpy="-2.0943952105869323 0 -1.5707963267948963"/>
+    <parent link="root_link"/>
+    <child link="root_link_imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="root_link_imu_acc"/>
   <joint name="root_link_imu_acc_fixed_joint" type="fixed">
     <origin xyz="0.08515499999999998 -0.011000000000000022 -0.11230899999999999" rpy="-2.0943952105869323 0 -1.5707963267948963"/>
@@ -243,6 +250,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
@@ -340,6 +354,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -767,6 +788,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubGenova03/model.urdf b/iCub/robots/iCubGenova03/model.urdf
index 06d49b3..0d0df42 100644
--- a/iCub/robots/iCubGenova03/model.urdf
+++ b/iCub/robots/iCubGenova03/model.urdf
@@ -1287,28 +1287,48 @@
             <always_on>1</always_on>
         </sensor>
     </gazebo>
-    <sensor type="force_torque" name="l_arm_ft_sensor">
+    <link name="l_arm_ft"/>
+    <joint name="l_arm_ft_fixed_joint" type="fixed">
+        <parent link="l_upper_arm"/>
+        <child link="l_arm_ft"/>
+    </joint>
+    <link name="r_arm_ft"/>
+    <joint name="r_arm_ft_fixed_joint" type="fixed">
+        <parent link="r_upper_arm"/>
+        <child link="r_arm_ft"/>
+    </joint>
+    <link name="l_leg_ft"/>
+    <joint name="l_leg_ft_fixed_joint" type="fixed">
+        <parent link="l_hip_3"/>
+        <child link="l_leg_ft"/>
+    </joint>
+    <link name="r_leg_ft"/>
+    <joint name="r_leg_ft_fixed_joint" type="fixed">
+        <parent link="r_hip_3"/>
+        <child link="r_leg_ft"/>
+    </joint>
+    <sensor type="force_torque" name="l_arm_ft">
         <parent joint="l_arm_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="r_arm_ft_sensor">
+    <sensor type="force_torque" name="r_arm_ft">
         <parent joint="r_arm_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="l_leg_ft_sensor">
+    <sensor type="force_torque" name="l_leg_ft">
         <parent joint="l_leg_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="r_leg_ft_sensor">
+    <sensor type="force_torque" name="r_leg_ft">
         <parent joint="r_leg_ft_sensor" />
         <force_torque>
             <frame>child</frame>
diff --git a/iCub/robots/iCubGenova04/model.urdf b/iCub/robots/iCubGenova04/model.urdf
index d282267..667753b 100644
--- a/iCub/robots/iCubGenova04/model.urdf
+++ b/iCub/robots/iCubGenova04/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.009500000190735 0.133444 0.009299999999999996" rpy="-1.5707963267948961 1.570796326794896 0"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubGenova09/model.urdf b/iCub/robots/iCubGenova09/model.urdf
index 4cff3ff..6fe2516 100644
--- a/iCub/robots/iCubGenova09/model.urdf
+++ b/iCub/robots/iCubGenova09/model.urdf
@@ -403,6 +403,13 @@
     <limit effort="50000" lower="-0.7504915783575618" upper="0.7504915783575618" velocity="120.0"/>
     <dynamics damping="2.0" friction="0.0"/>
   </joint>
+  <link name="chest_depth_frame"/>
+  <joint name="chest_depth_frame_fixed_joint" type="fixed">
+    <origin xyz="0.0894115 0.0175 0.09929950000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
+    <parent link="chest"/>
+    <child link="chest_depth_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="chest_imu_0"/>
   <joint name="chest_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.0894115 0.0175 0.09929950000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
@@ -535,6 +542,13 @@
     <limit effort="50000" lower="-0.7853981633974483" upper="0.7853981633974483" velocity="120.0"/>
     <dynamics damping="2.0" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.0035000000000000005 0.0055 0.10133400000000001" rpy="3.141592653589793 0 -1.5707963267948966"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.0035000000000000005 0.0055 0.10133400000000001" rpy="3.141592653589793 0 -1.5707963267948966"/>
diff --git a/iCub/robots/iCubGenova11/model.urdf b/iCub/robots/iCubGenova11/model.urdf
index d39597b..c29eb7e 100644
--- a/iCub/robots/iCubGenova11/model.urdf
+++ b/iCub/robots/iCubGenova11/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="-1.5707963267948997 0 -3.141592653589793"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="-1.5707963267948997 0 -3.141592653589793"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
diff --git a/iCub/robots/iCubLisboa01/model.urdf b/iCub/robots/iCubLisboa01/model.urdf
index e5eee95..7ce6378 100644
--- a/iCub/robots/iCubLisboa01/model.urdf
+++ b/iCub/robots/iCubLisboa01/model.urdf
@@ -1241,28 +1241,48 @@
             <always_on>1</always_on>
         </sensor>
     </gazebo>
-    <sensor type="force_torque" name="l_arm_ft_sensor">
+    <link name="l_arm_ft"/>
+    <joint name="l_arm_ft_fixed_joint" type="fixed">
+        <parent link="l_upper_arm"/>
+        <child link="l_arm_ft"/>
+    </joint>
+    <link name="r_arm_ft"/>
+    <joint name="r_arm_ft_fixed_joint" type="fixed">
+        <parent link="r_upper_arm"/>
+        <child link="r_arm_ft"/>
+    </joint>
+    <link name="l_leg_ft"/>
+    <joint name="l_leg_ft_fixed_joint" type="fixed">
+        <parent link="l_hip_3"/>
+        <child link="l_leg_ft"/>
+    </joint>
+    <link name="r_leg_ft"/>
+    <joint name="r_leg_ft_fixed_joint" type="fixed">
+        <parent link="r_hip_3"/>
+        <child link="r_leg_ft"/>
+    </joint>
+    <sensor type="force_torque" name="l_arm_ft">
         <parent joint="l_arm_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="r_arm_ft_sensor">
+    <sensor type="force_torque" name="r_arm_ft">
         <parent joint="r_arm_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="l_leg_ft_sensor">
+    <sensor type="force_torque" name="l_leg_ft">
         <parent joint="l_leg_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="r_leg_ft_sensor">
+    <sensor type="force_torque" name="r_leg_ft">
         <parent joint="r_leg_ft_sensor" />
         <force_torque>
             <frame>child</frame>
diff --git a/iCub/robots/iCubNancy01/model.urdf b/iCub/robots/iCubNancy01/model.urdf
index ae83f02..0ea84e2 100644
--- a/iCub/robots/iCubNancy01/model.urdf
+++ b/iCub/robots/iCubNancy01/model.urdf
@@ -1287,28 +1287,48 @@
             <always_on>1</always_on>
         </sensor>
     </gazebo>
-    <sensor type="force_torque" name="l_arm_ft_sensor">
+    <link name="l_arm_ft"/>
+    <joint name="l_arm_ft_fixed_joint" type="fixed">
+        <parent link="l_upper_arm"/>
+        <child link="l_arm_ft"/>
+    </joint>
+    <link name="r_arm_ft"/>
+    <joint name="r_arm_ft_fixed_joint" type="fixed">
+        <parent link="r_upper_arm"/>
+        <child link="r_arm_ft"/>
+    </joint>
+    <link name="l_leg_ft"/>
+    <joint name="l_leg_ft_fixed_joint" type="fixed">
+        <parent link="l_hip_3"/>
+        <child link="l_leg_ft"/>
+    </joint>
+    <link name="r_leg_ft"/>
+    <joint name="r_leg_ft_fixed_joint" type="fixed">
+        <parent link="r_hip_3"/>
+        <child link="r_leg_ft"/>
+    </joint>
+    <sensor type="force_torque" name="l_arm_ft">
         <parent joint="l_arm_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="r_arm_ft_sensor">
+    <sensor type="force_torque" name="r_arm_ft">
         <parent joint="r_arm_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="l_leg_ft_sensor">
+    <sensor type="force_torque" name="l_leg_ft">
         <parent joint="l_leg_ft_sensor" />
         <force_torque>
             <frame>child</frame>
             <measure_direction>child_to_parent</measure_direction>
         </force_torque>
     </sensor>
-    <sensor type="force_torque" name="r_leg_ft_sensor">
+    <sensor type="force_torque" name="r_leg_ft">
         <parent joint="r_leg_ft_sensor" />
         <force_torque>
             <frame>child</frame>
diff --git a/iCub/robots/iCubWaterloo01/model.urdf b/iCub/robots/iCubWaterloo01/model.urdf
index d39597b..c29eb7e 100644
--- a/iCub/robots/iCubWaterloo01/model.urdf
+++ b/iCub/robots/iCubWaterloo01/model.urdf
@@ -983,6 +983,13 @@
     <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="imu_frame"/>
+  <joint name="imu_frame_fixed_joint" type="fixed">
+    <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="-1.5707963267948997 0 -3.141592653589793"/>
+    <parent link="head"/>
+    <child link="imu_frame"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="head_imu_0"/>
   <joint name="head_imu_0_fixed_joint" type="fixed">
     <origin xyz="0.023453700190734995 0.20544400000000002 -0.023077900000000023" rpy="-1.5707963267948997 0 -3.141592653589793"/>
@@ -1080,6 +1087,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="r_arm_ft"/>
+  <joint name="r_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
+    <parent link="r_shoulder_3"/>
+    <child link="r_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="r_arm_ft_imu"/>
   <joint name="r_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="-0.05089730176650909 0 0.029167029620599215" rpy="0 -1.3089970069677397 0"/>
@@ -1507,6 +1521,13 @@
     <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
     <dynamics damping="0.06" friction="0.0"/>
   </joint>
+  <link name="l_arm_ft"/>
+  <joint name="l_arm_ft_fixed_joint" type="fixed">
+    <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>
+    <parent link="l_shoulder_3"/>
+    <child link="l_arm_ft"/>
+    <dynamics damping="0.1"/>
+  </joint>
   <link name="l_arm_ft_imu"/>
   <joint name="l_arm_ft_imu_fixed_joint" type="fixed">
     <origin xyz="0.05093014324520106 0 0.029175829465900395" rpy="0 -1.3089970069677395 3.1415926535897927"/>