diff --git a/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_l_lower_leg.stl b/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_l_lower_leg.stl
index b51b2fca..cb726ebe 100644
Binary files a/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_l_lower_leg.stl and b/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_l_lower_leg.stl differ
diff --git a/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_r_lower_leg.stl b/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_r_lower_leg.stl
index c33b32b4..4669d6d1 100644
Binary files a/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_r_lower_leg.stl and b/urdf/ergoCub/meshes/simmechanics/sim_ecub_1-1_r_lower_leg.stl differ
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
index f6081600..9a4d8ce3 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.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
-    <axis xyz="1.5978340464762954e-11 0.9999999999999999 -3.7677226824487294e-11"/>
+    <origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
+    <axis xyz="1.5978451487065422e-11 1.0000000000000002 -3.767719906891169e-11"/>
     <parent link="l_hand_palm"/>
     <child link="l_hand_index_1"/>
     <limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
@@ -1251,7 +1251,7 @@
   <link name="l_hand_index_1">
     <inertial>
       <mass value="0.01923264"/>
-      <origin xyz="0.0046807100979840005 0.0038510231744863237 -0.018526526203731575" rpy="0 -0 0"/>
+      <origin xyz="0.0046807100979840005 0.0038510231744862677 -0.018526526203731464" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="0.01" iyz="-6.425403568227921e-7" izz="0.01"/>
     </inertial>
     <visual>
@@ -1571,8 +1571,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
-    <axis xyz="-0.9781476007113543 2.3461557522424743e-11 0.2079116909233844"/>
+    <origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
+    <axis xyz="-0.9781476007113546 2.346155062839741e-11 0.20791169092338443"/>
     <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="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
-    <axis xyz="-0.9781476007113552 -1.553317034713362e-8 0.20791169092337955"/>
+    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
+    <axis xyz="-0.9781476007113552 -1.5533170287064815e-8 0.2079116909233797"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
@@ -1614,7 +1614,7 @@
   <link name="l_hand_index_3">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="-0.018787329483201345 0.0027546711703011484 -0.018853294932697662" rpy="0 -0 0"/>
+      <origin xyz="-0.018787329483201373 0.002754671170301093 -0.018853294932697606" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="0.01" iyz="-2.891156821380605e-7" izz="0.01"/>
     </inertial>
     <visual>
@@ -1733,8 +1733,8 @@
     </collision>
   </link>
   <joint name="r_knee" type="revolute">
-    <origin xyz="0.01060000963260986 0.007899987075259199 -0.17970000000000103" rpy="-3.1415926535897274 -8.69101748565412e-14 -1.2193157698906137e-6"/>
-    <axis xyz="-2.0458175022504648e-17 1 2.5382740632965355e-17"/>
+    <origin xyz="0.01060000963260985 0.007899987075259289 -0.17970000000000058" rpy="6.892643261935339e-14 -6.650159361880639e-14 -1.2193157700904425e-6"/>
+    <axis xyz="1.7937066152151813e-16 -1 3.2454535731621664e-15"/>
     <parent link="r_upper_leg"/>
     <child link="r_lower_leg"/>
     <limit lower="-1.8151424220741028" upper="0.10471975511965978" effort="1e+9" velocity="1e+9"/>
@@ -1742,9 +1742,9 @@
   </joint>
   <link name="r_lower_leg">
     <inertial>
-      <mass value="3.7416141515311727"/>
-      <origin xyz="0.0011085261409734988 0.0062001565875166595 0.21425880722970114" rpy="0 -0 0"/>
-      <inertia ixx="0.047253870547411925" ixy="-3.409214993998376e-5" ixz="-0.0018183325903094667" iyy="0.046014005027955385" iyz="8.514638578603628e-4" izz="0.004785149491842782"/>
+      <mass value="3.7621532589747924"/>
+      <origin xyz="0.0012157689164550914 -0.006184734585346893 -0.21372759879868197" rpy="0 -0 0"/>
+      <inertia ixx="0.047738936736850396" ixy="-3.726927487219018e-5" ixz="-0.001804298172118346" iyy="0.04644215616821937" iyz="8.519885904633845e-4" izz="0.004839671863500548"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1766,8 +1766,8 @@
     </collision>
   </link>
   <joint name="r_ankle_pitch" type="revolute">
-    <origin xyz="-1.2462253451417382e-14 0.031899999999999984 0.3210000000000002" rpy="-3.141592653589793 1.2923622733154212e-30 -1.6653283970679406e-16"/>
-    <axis xyz="1.8699102912341645e-16 1 -1.9666186429286112e-16"/>
+    <origin xyz="-5.936223734792634e-15 -0.03190000000000005 -0.3210000000000002" rpy="-2.9932805576414174e-15 -2.0242047360524565e-14 8.880636788937802e-17"/>
+    <axis xyz="-1.342621999194175e-15 1 -3.530978817196621e-14"/>
     <parent link="r_lower_leg"/>
     <child link="r_ankle_1"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -1776,7 +1776,7 @@
   <link name="r_ankle_1">
     <inertial>
       <mass value="0.3481278950635556"/>
-      <origin xyz="-0.00141723097469474 -6.601869590198567e-4 -0.014087432401547195" rpy="0 -0 0"/>
+      <origin xyz="-0.001417230974694688 -6.601869590209392e-4 -0.01408743240154764" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="-8.991873201784187e-6" ixz="5.523926696579657e-5" iyy="0.01" iyz="1.0336975691919178e-5" izz="0.01"/>
     </inertial>
     <visual>
@@ -1799,8 +1799,8 @@
     </collision>
   </link>
   <joint name="r_ankle_roll" type="revolute">
-    <origin xyz="-0.05275000000000156 0.018200000000000008 -0.03499999999999759" rpy="1.1129880739582211e-25 3.883828473168634e-14 1.1834052693462316e-23"/>
-    <axis xyz="-1 5.921958056855112e-17 2.5592852998214428e-15"/>
+    <origin xyz="-0.052750000000001546 0.018200000000000008 -0.0349999999999977" rpy="-1.0176053241861382e-24 3.8838282931741325e-14 -9.278622199115707e-23"/>
+    <axis xyz="-1 -2.1833586821066654e-16 2.39275322255624e-15"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit lower="-0.4363323129985824" upper="0.4363323129985824" effort="1e+9" velocity="1e+9"/>
@@ -1809,8 +1809,8 @@
   <link name="r_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040476468116960304 -8.642493098869053e-5 -0.023871143533700564" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.01" iyz="-0.0013223759105872523" izz="0.01"/>
+      <origin xyz="0.040476468116960346 -8.642493098978687e-5 -0.023871143533700897" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.01" iyz="-0.0013223759105872523" izz="0.010000000000000002"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1832,14 +1832,14 @@
     </collision>
   </link>
   <joint name="r_foot_front_ft_sensor" type="fixed">
-    <origin xyz="0.13524999999999998 4.163336342344337e-17 -0.04789999999999983" rpy="2.6613913647137395e-17 -9.208113692419518e-17 -1.4551885312033586e-15"/>
+    <origin xyz="0.13525000000000004 0 -0.04789999999999994" rpy="2.66139136471374e-17 -9.20811369242055e-17 -1.4551888385804406e-15"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_front"/>
   </joint>
   <link name="r_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385911e-4 -3.747944080090271e-9 -0.020000616615898803" rpy="0 -0 0"/>
+      <origin xyz="5.879202911385634e-4 -3.747945093168781e-9 -0.020000616615899025" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.4706466904026456e-9" izz="0.01"/>
     </inertial>
     <visual>
@@ -1859,14 +1859,14 @@
     </collision>
   </link>
   <joint name="r_foot_rear_ft_sensor" type="fixed">
-    <origin xyz="0.016 2.7755575615628914e-17 -0.04789999999999983" rpy="2.6613913647112164e-17 -9.20811369242145e-17 -1.5662108336658734e-15"/>
+    <origin xyz="0.016 0 -0.04789999999999983" rpy="2.661391364712478e-17 -9.208113692419957e-17 -1.566211141042955e-15"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_rear"/>
   </joint>
   <link name="r_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.87920291138468e-4 -3.747944163356998e-9 -0.0200006166159018" rpy="0 -0 0"/>
+      <origin xyz="5.879202911385132e-4 -3.747945231946659e-9 -0.020000616615902134" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.470646669939015e-9" izz="0.01"/>
     </inertial>
     <visual>
@@ -1982,8 +1982,8 @@
     </collision>
   </link>
   <joint name="l_knee" type="revolute">
-    <origin xyz="0.010600000000000049 -0.00790000000000024 -0.17969999999999958" rpy="-3.141592653589793 -1.1419428146632273e-14 2.6734444562115447e-15"/>
-    <axis xyz="-2.4058767102531225e-16 1 6.181058845623608e-17"/>
+    <origin xyz="0.010600000000000822 -0.007900000000000185 -0.17969999999999958" rpy="1.970486418692682e-16 -1.1419428146632273e-14 2.8602839935211434e-15"/>
+    <axis xyz="-4.27427208334911e-16 -1 1.3523805341303425e-16"/>
     <parent link="l_upper_leg"/>
     <child link="l_lower_leg"/>
     <limit lower="-1.8151424220741028" upper="0.10471975511965978" effort="1e+9" velocity="1e+9"/>
@@ -1991,9 +1991,9 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <mass value="3.741614150537795"/>
-      <origin xyz="0.0013845327980559936 -0.006187154814159421 0.21425998171590055" rpy="0 -0 0"/>
-      <inertia ixx="0.047251812520766406" ixy="1.9125290134772514e-5" ixz="0.0018224985829850483" iyy="0.04601293081939889" iyz="8.051365783012876e-4" izz="0.004782596450589537"/>
+      <mass value="3.767581145212888"/>
+      <origin xyz="0.001420176808809015 0.006186333743956975 -0.21383298861124966" rpy="0 -0 0"/>
+      <inertia ixx="0.04776726641499918" ixy="2.1189289797885975e-5" ixz="0.0018123762001582072" iyy="0.04647110900067475" iyz="8.110710215724027e-4" izz="0.0048404912519552645"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2015,8 +2015,8 @@
     </collision>
   </link>
   <joint name="l_ankle_pitch" type="revolute">
-    <origin xyz="-3.701899897734506e-15 -0.03189999999999994 0.3209999999999994" rpy="-3.141592653589793 8.673617379885829e-19 -3.1554436208840472e-30"/>
-    <axis xyz="2.405876710253171e-16 1 6.181058845623607e-17"/>
+    <origin xyz="-4.468647674116255e-15 0.03189999999999981 -0.3209999999999995" rpy="-1.9704864186927032e-16 -8.673617379880112e-19 -1.8683953730959558e-16"/>
+    <axis xyz="-7.24267500143743e-17 1 6.181058845623533e-17"/>
     <parent link="l_lower_leg"/>
     <child link="l_ankle_1"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
index 5ffb0f21..4b19ced4 100644
--- a/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
@@ -1133,8 +1133,8 @@
     </collision>
   </link>
   <joint name="l_index_add" type="revolute">
-    <origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
-    <axis xyz="1.5978340464762954e-11 0.9999999999999999 -3.7677226824487294e-11"/>
+    <origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
+    <axis xyz="1.5978451487065422e-11 1.0000000000000002 -3.767719906891169e-11"/>
     <parent link="l_hand_palm"/>
     <child link="l_hand_index_1"/>
     <limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
@@ -1143,7 +1143,7 @@
   <link name="l_hand_index_1">
     <inertial>
       <mass value="0.01923264"/>
-      <origin xyz="0.0046807100979840005 0.0038510231744863237 -0.018526526203731575" rpy="0 -0 0"/>
+      <origin xyz="0.0046807100979840005 0.0038510231744862677 -0.018526526203731464" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="0.01" iyz="-6.425403568227921e-7" izz="0.01"/>
     </inertial>
     <visual>
@@ -1433,8 +1433,8 @@
     </collision>
   </link>
   <joint name="l_index_prox" type="revolute">
-    <origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
-    <axis xyz="-0.9781476007113543 2.3461557522424743e-11 0.2079116909233844"/>
+    <origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
+    <axis xyz="-0.9781476007113546 2.346155062839741e-11 0.20791169092338443"/>
     <parent link="l_hand_index_1"/>
     <child link="l_hand_index_2"/>
     <limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
@@ -1463,8 +1463,8 @@
     </collision>
   </link>
   <joint name="l_index_dist" type="revolute">
-    <origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
-    <axis xyz="-0.9781476007113552 -1.553317034713362e-8 0.20791169092337955"/>
+    <origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
+    <axis xyz="-0.9781476007113552 -1.5533170287064815e-8 0.2079116909233797"/>
     <parent link="l_hand_index_2"/>
     <child link="l_hand_index_3"/>
     <limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
@@ -1473,7 +1473,7 @@
   <link name="l_hand_index_3">
     <inertial>
       <mass value="0.007251413"/>
-      <origin xyz="-0.018787329483201345 0.0027546711703011484 -0.018853294932697662" rpy="0 -0 0"/>
+      <origin xyz="-0.018787329483201373 0.002754671170301093 -0.018853294932697606" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="0.01" iyz="-2.891156821380605e-7" izz="0.01"/>
     </inertial>
     <visual>
@@ -1580,8 +1580,8 @@
     </collision>
   </link>
   <joint name="r_knee" type="revolute">
-    <origin xyz="0.01060000963260986 0.007899987075259199 -0.17970000000000103" rpy="-3.1415926535897274 -8.69101748565412e-14 -1.2193157698906137e-6"/>
-    <axis xyz="-2.0458175022504648e-17 1 2.5382740632965355e-17"/>
+    <origin xyz="0.01060000963260985 0.007899987075259289 -0.17970000000000058" rpy="6.892643261935339e-14 -6.650159361880639e-14 -1.2193157700904425e-6"/>
+    <axis xyz="1.7937066152151813e-16 -1 3.2454535731621664e-15"/>
     <parent link="r_upper_leg"/>
     <child link="r_lower_leg"/>
     <limit lower="-1.8151424220741028" upper="0.10471975511965978" effort="1e+9" velocity="1e+9"/>
@@ -1589,9 +1589,9 @@
   </joint>
   <link name="r_lower_leg">
     <inertial>
-      <mass value="3.7416141515311727"/>
-      <origin xyz="0.0011085261409734988 0.0062001565875166595 0.21425880722970114" rpy="0 -0 0"/>
-      <inertia ixx="0.047253870547411925" ixy="-3.409214993998376e-5" ixz="-0.0018183325903094667" iyy="0.046014005027955385" iyz="8.514638578603628e-4" izz="0.004785149491842782"/>
+      <mass value="3.7621532589747924"/>
+      <origin xyz="0.0012157689164550914 -0.006184734585346893 -0.21372759879868197" rpy="0 -0 0"/>
+      <inertia ixx="0.047738936736850396" ixy="-3.726927487219018e-5" ixz="-0.001804298172118346" iyy="0.04644215616821937" iyz="8.519885904633845e-4" izz="0.004839671863500548"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1610,8 +1610,8 @@
     </collision>
   </link>
   <joint name="r_ankle_pitch" type="revolute">
-    <origin xyz="-1.2462253451417382e-14 0.031899999999999984 0.3210000000000002" rpy="-3.141592653589793 1.2923622733154212e-30 -1.6653283970679406e-16"/>
-    <axis xyz="1.8699102912341645e-16 1 -1.9666186429286112e-16"/>
+    <origin xyz="-5.936223734792634e-15 -0.03190000000000005 -0.3210000000000002" rpy="-2.9932805576414174e-15 -2.0242047360524565e-14 8.880636788937802e-17"/>
+    <axis xyz="-1.342621999194175e-15 1 -3.530978817196621e-14"/>
     <parent link="r_lower_leg"/>
     <child link="r_ankle_1"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -1620,7 +1620,7 @@
   <link name="r_ankle_1">
     <inertial>
       <mass value="0.3481278950635556"/>
-      <origin xyz="-0.00141723097469474 -6.601869590198567e-4 -0.014087432401547195" rpy="0 -0 0"/>
+      <origin xyz="-0.001417230974694688 -6.601869590209392e-4 -0.01408743240154764" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="-8.991873201784187e-6" ixz="5.523926696579657e-5" iyy="0.01" iyz="1.0336975691919178e-5" izz="0.01"/>
     </inertial>
     <visual>
@@ -1640,8 +1640,8 @@
     </collision>
   </link>
   <joint name="r_ankle_roll" type="revolute">
-    <origin xyz="-0.05275000000000156 0.018200000000000008 -0.03499999999999759" rpy="1.1129880739582211e-25 3.883828473168634e-14 1.1834052693462316e-23"/>
-    <axis xyz="-1 5.921958056855112e-17 2.5592852998214428e-15"/>
+    <origin xyz="-0.052750000000001546 0.018200000000000008 -0.0349999999999977" rpy="-1.0176053241861382e-24 3.8838282931741325e-14 -9.278622199115707e-23"/>
+    <axis xyz="-1 -2.1833586821066654e-16 2.39275322255624e-15"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit lower="-0.4363323129985824" upper="0.4363323129985824" effort="1e+9" velocity="1e+9"/>
@@ -1650,8 +1650,8 @@
   <link name="r_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040476468116960304 -8.642493098869053e-5 -0.023871143533700564" rpy="0 -0 0"/>
-      <inertia ixx="0.01" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.01" iyz="-0.0013223759105872523" izz="0.01"/>
+      <origin xyz="0.040476468116960346 -8.642493098978687e-5 -0.023871143533700897" rpy="0 -0 0"/>
+      <inertia ixx="0.01" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.01" iyz="-0.0013223759105872523" izz="0.010000000000000002"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1670,14 +1670,14 @@
     </collision>
   </link>
   <joint name="r_foot_front_ft_sensor" type="fixed">
-    <origin xyz="0.13524999999999998 4.163336342344337e-17 -0.04789999999999983" rpy="2.6613913647137395e-17 -9.208113692419518e-17 -1.4551885312033586e-15"/>
+    <origin xyz="0.13525000000000004 0 -0.04789999999999994" rpy="2.66139136471374e-17 -9.20811369242055e-17 -1.4551888385804406e-15"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_front"/>
   </joint>
   <link name="r_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385911e-4 -3.747944080090271e-9 -0.020000616615898803" rpy="0 -0 0"/>
+      <origin xyz="5.879202911385634e-4 -3.747945093168781e-9 -0.020000616615899025" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.4706466904026456e-9" izz="0.01"/>
     </inertial>
     <visual>
@@ -1697,14 +1697,14 @@
     </collision>
   </link>
   <joint name="r_foot_rear_ft_sensor" type="fixed">
-    <origin xyz="0.016 2.7755575615628914e-17 -0.04789999999999983" rpy="2.6613913647112164e-17 -9.20811369242145e-17 -1.5662108336658734e-15"/>
+    <origin xyz="0.016 0 -0.04789999999999983" rpy="2.661391364712478e-17 -9.208113692419957e-17 -1.566211141042955e-15"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_rear"/>
   </joint>
   <link name="r_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.87920291138468e-4 -3.747944163356998e-9 -0.0200006166159018" rpy="0 -0 0"/>
+      <origin xyz="5.879202911385132e-4 -3.747945231946659e-9 -0.020000616615902134" rpy="0 -0 0"/>
       <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="3.470646669939015e-9" izz="0.01"/>
     </inertial>
     <visual>
@@ -1811,8 +1811,8 @@
     </collision>
   </link>
   <joint name="l_knee" type="revolute">
-    <origin xyz="0.010600000000000049 -0.00790000000000024 -0.17969999999999958" rpy="-3.141592653589793 -1.1419428146632273e-14 2.6734444562115447e-15"/>
-    <axis xyz="-2.4058767102531225e-16 1 6.181058845623608e-17"/>
+    <origin xyz="0.010600000000000822 -0.007900000000000185 -0.17969999999999958" rpy="1.970486418692682e-16 -1.1419428146632273e-14 2.8602839935211434e-15"/>
+    <axis xyz="-4.27427208334911e-16 -1 1.3523805341303425e-16"/>
     <parent link="l_upper_leg"/>
     <child link="l_lower_leg"/>
     <limit lower="-1.8151424220741028" upper="0.10471975511965978" effort="1e+9" velocity="1e+9"/>
@@ -1820,9 +1820,9 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <mass value="3.741614150537795"/>
-      <origin xyz="0.0013845327980559936 -0.006187154814159421 0.21425998171590055" rpy="0 -0 0"/>
-      <inertia ixx="0.047251812520766406" ixy="1.9125290134772514e-5" ixz="0.0018224985829850483" iyy="0.04601293081939889" iyz="8.051365783012876e-4" izz="0.004782596450589537"/>
+      <mass value="3.767581145212888"/>
+      <origin xyz="0.001420176808809015 0.006186333743956975 -0.21383298861124966" rpy="0 -0 0"/>
+      <inertia ixx="0.04776726641499918" ixy="2.1189289797885975e-5" ixz="0.0018123762001582072" iyy="0.04647110900067475" iyz="8.110710215724027e-4" izz="0.0048404912519552645"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1841,8 +1841,8 @@
     </collision>
   </link>
   <joint name="l_ankle_pitch" type="revolute">
-    <origin xyz="-3.701899897734506e-15 -0.03189999999999994 0.3209999999999994" rpy="-3.141592653589793 8.673617379885829e-19 -3.1554436208840472e-30"/>
-    <axis xyz="2.405876710253171e-16 1 6.181058845623607e-17"/>
+    <origin xyz="-4.468647674116255e-15 0.03189999999999981 -0.3209999999999995" rpy="-1.9704864186927032e-16 -8.673617379880112e-19 -1.8683953730959558e-16"/>
+    <axis xyz="-7.24267500143743e-17 1 6.181058845623533e-17"/>
     <parent link="l_lower_leg"/>
     <child link="l_ankle_1"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
diff --git a/urdf/ergoCub/robots/ergoCubSN001/model.urdf b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
index 9fce97b3..bf228e7b 100644
--- a/urdf/ergoCub/robots/ergoCubSN001/model.urdf
+++ b/urdf/ergoCub/robots/ergoCubSN001/model.urdf
@@ -1733,8 +1733,8 @@
     </collision>
   </link>
   <joint name="r_knee" type="revolute">
-    <origin xyz="0.01060000963260986 0.007899987075259199 -0.17970000000000103" rpy="-3.1415926535897274 -8.69101748565412e-14 -1.2193157698906137e-6"/>
-    <axis xyz="-2.0458175022504648e-17 1 2.5382740632965355e-17"/>
+    <origin xyz="0.01060000963260985 0.007899987075259289 -0.17970000000000058" rpy="6.892643261935339e-14 -6.650159361880639e-14 -1.2193157700904425e-6"/>
+    <axis xyz="1.7937066152151813e-16 -1 3.2454535731621664e-15"/>
     <parent link="r_upper_leg"/>
     <child link="r_lower_leg"/>
     <limit lower="-1.8151424220741028" upper="0.10471975511965978" effort="1e+9" velocity="1e+9"/>
@@ -1742,9 +1742,9 @@
   </joint>
   <link name="r_lower_leg">
     <inertial>
-      <mass value="3.7416141515311727"/>
-      <origin xyz="0.0011085261409734988 0.0062001565875166595 0.21425880722970114" rpy="0 -0 0"/>
-      <inertia ixx="0.047253870547411925" ixy="-3.409214993998376e-5" ixz="-0.0018183325903094667" iyy="0.046014005027955385" iyz="8.514638578603628e-4" izz="0.004785149491842782"/>
+      <mass value="3.7621532589747924"/>
+      <origin xyz="0.0012157689164550914 -0.006184734585346893 -0.21372759879868197" rpy="0 -0 0"/>
+      <inertia ixx="0.047738936736850396" ixy="-3.726927487219018e-5" ixz="-0.001804298172118346" iyy="0.04644215616821937" iyz="8.519885904633845e-4" izz="0.004839671863500548"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1766,8 +1766,8 @@
     </collision>
   </link>
   <joint name="r_ankle_pitch" type="revolute">
-    <origin xyz="-1.2462253451417382e-14 0.031899999999999984 0.3210000000000002" rpy="-3.141592653589793 1.2923622733154212e-30 -1.6653283970679406e-16"/>
-    <axis xyz="1.8699102912341645e-16 1 -1.9666186429286112e-16"/>
+    <origin xyz="-5.936223734792634e-15 -0.03190000000000005 -0.3210000000000002" rpy="-2.9932805576414174e-15 -2.0242047360524565e-14 8.880636788937802e-17"/>
+    <axis xyz="-1.342621999194175e-15 1 -3.530978817196621e-14"/>
     <parent link="r_lower_leg"/>
     <child link="r_ankle_1"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>
@@ -1776,7 +1776,7 @@
   <link name="r_ankle_1">
     <inertial>
       <mass value="0.3481278950635556"/>
-      <origin xyz="-0.00141723097469474 -6.601869590198567e-4 -0.014087432401547195" rpy="0 -0 0"/>
+      <origin xyz="-0.001417230974694688 -6.601869590209392e-4 -0.01408743240154764" rpy="0 -0 0"/>
       <inertia ixx="5.753493395636923e-4" ixy="-8.991873201784187e-6" ixz="5.523926696579657e-5" iyy="2.47340125952152e-4" iyz="1.0336975691919178e-5" izz="4.626760389351452e-4"/>
     </inertial>
     <visual>
@@ -1799,8 +1799,8 @@
     </collision>
   </link>
   <joint name="r_ankle_roll" type="revolute">
-    <origin xyz="-0.05275000000000156 0.018200000000000008 -0.03499999999999759" rpy="1.1129880739582211e-25 3.883828473168634e-14 1.1834052693462316e-23"/>
-    <axis xyz="-1 5.921958056855112e-17 2.5592852998214428e-15"/>
+    <origin xyz="-0.052750000000001546 0.018200000000000008 -0.0349999999999977" rpy="-1.0176053241861382e-24 3.8838282931741325e-14 -9.278622199115707e-23"/>
+    <axis xyz="-1 -2.1833586821066654e-16 2.39275322255624e-15"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit lower="-0.4363323129985824" upper="0.4363323129985824" effort="1e+9" velocity="1e+9"/>
@@ -1809,8 +1809,8 @@
   <link name="r_ankle_2">
     <inertial>
       <mass value="1.5418216247038539"/>
-      <origin xyz="0.040476468116960304 -8.642493098869053e-5 -0.023871143533700564" rpy="0 -0 0"/>
-      <inertia ixx="0.009438348902378935" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.0021440882932298143" iyz="-0.0013223759105872523" izz="0.009024460701601934"/>
+      <origin xyz="0.040476468116960346 -8.642493098978687e-5 -0.023871143533700897" rpy="0 -0 0"/>
+      <inertia ixx="0.009438348902378935" ixy="-2.3071453411386873e-5" ixz="-3.077717707144611e-5" iyy="0.002144088293229814" iyz="-0.0013223759105872523" izz="0.009024460701601934"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1832,14 +1832,14 @@
     </collision>
   </link>
   <joint name="r_foot_front_ft_sensor" type="fixed">
-    <origin xyz="0.13524999999999998 4.163336342344337e-17 -0.04789999999999983" rpy="2.6613913647137395e-17 -9.208113692419518e-17 -1.4551885312033586e-15"/>
+    <origin xyz="0.13525000000000004 0 -0.04789999999999994" rpy="2.66139136471374e-17 -9.20811369242055e-17 -1.4551888385804406e-15"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_front"/>
   </joint>
   <link name="r_foot_front">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.879202911385911e-4 -3.747944080090271e-9 -0.020000616615898803" rpy="0 -0 0"/>
+      <origin xyz="5.879202911385634e-4 -3.747945093168781e-9 -0.020000616615899025" rpy="0 -0 0"/>
       <inertia ixx="2.6091266876367765e-4" ixy="0" ixz="0" iyy="1.912506447891574e-4" iyz="3.4706466904026456e-9" izz="4.500881026346283e-4"/>
     </inertial>
     <visual>
@@ -1859,15 +1859,15 @@
     </collision>
   </link>
   <joint name="r_foot_rear_ft_sensor" type="fixed">
-    <origin xyz="0.016 2.7755575615628914e-17 -0.04789999999999983" rpy="2.6613913647112164e-17 -9.20811369242145e-17 -1.5662108336658734e-15"/>
+    <origin xyz="0.016 0 -0.04789999999999983" rpy="2.661391364712478e-17 -9.208113692419957e-17 -1.566211141042955e-15"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_rear"/>
   </joint>
   <link name="r_foot_rear">
     <inertial>
       <mass value="0.22110467939552797"/>
-      <origin xyz="5.87920291138468e-4 -3.747944163356998e-9 -0.0200006166159018" rpy="0 -0 0"/>
-      <inertia ixx="2.609126687634939e-4" ixy="0" ixz="0" iyy="1.9125064478893545e-4" iyz="3.470646669939015e-9" izz="4.5008810263463014e-4"/>
+      <origin xyz="5.879202911385132e-4 -3.747945231946659e-9 -0.020000616615902134" rpy="0 -0 0"/>
+      <inertia ixx="2.609126687634939e-4" ixy="0" ixz="0" iyy="1.912506447889355e-4" iyz="3.470646669939015e-9" izz="4.5008810263463014e-4"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -1982,8 +1982,8 @@
     </collision>
   </link>
   <joint name="l_knee" type="revolute">
-    <origin xyz="0.010600000000000049 -0.00790000000000024 -0.17969999999999958" rpy="-3.141592653589793 -1.1419428146632273e-14 2.6734444562115447e-15"/>
-    <axis xyz="-2.4058767102531225e-16 1 6.181058845623608e-17"/>
+    <origin xyz="0.010600000000000822 -0.007900000000000185 -0.17969999999999958" rpy="1.970486418692682e-16 -1.1419428146632273e-14 2.8602839935211434e-15"/>
+    <axis xyz="-4.27427208334911e-16 -1 1.3523805341303425e-16"/>
     <parent link="l_upper_leg"/>
     <child link="l_lower_leg"/>
     <limit lower="-1.8151424220741028" upper="0.10471975511965978" effort="1e+9" velocity="1e+9"/>
@@ -1991,9 +1991,9 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <mass value="3.741614150537795"/>
-      <origin xyz="0.0013845327980559936 -0.006187154814159421 0.21425998171590055" rpy="0 -0 0"/>
-      <inertia ixx="0.047251812520766406" ixy="1.9125290134772514e-5" ixz="0.0018224985829850483" iyy="0.04601293081939889" iyz="8.051365783012876e-4" izz="0.004782596450589537"/>
+      <mass value="3.767581145212888"/>
+      <origin xyz="0.001420176808809015 0.006186333743956975 -0.21383298861124966" rpy="0 -0 0"/>
+      <inertia ixx="0.04776726641499918" ixy="2.1189289797885975e-5" ixz="0.0018123762001582072" iyy="0.04647110900067475" iyz="8.110710215724027e-4" izz="0.0048404912519552645"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="0 -0 0"/>
@@ -2015,8 +2015,8 @@
     </collision>
   </link>
   <joint name="l_ankle_pitch" type="revolute">
-    <origin xyz="-3.701899897734506e-15 -0.03189999999999994 0.3209999999999994" rpy="-3.141592653589793 8.673617379885829e-19 -3.1554436208840472e-30"/>
-    <axis xyz="2.405876710253171e-16 1 6.181058845623607e-17"/>
+    <origin xyz="-4.468647674116255e-15 0.03189999999999981 -0.3209999999999995" rpy="-1.9704864186927032e-16 -8.673617379880112e-19 -1.8683953730959558e-16"/>
+    <axis xyz="-7.24267500143743e-17 1 6.181058845623533e-17"/>
     <parent link="l_lower_leg"/>
     <child link="l_ankle_1"/>
     <limit lower="-0.7853981633974483" upper="0.7853981633974483" effort="1e+9" velocity="1e+9"/>