From c3478cae9550c9d57abe63aab848be91b6278018 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 11 Nov 2020 08:48:14 +0000
Subject: [PATCH] Automatic build. GitHub Actions build: 357395956
 icub-model-generator commit:04863634ade805233e66476812036ca1d7792b99
 urdf_parser_py commit:31474b9baaf7c3845b40e5a9aa87d5900a2282c3
 simmechanics-to-urdf commit:386e2c9c809d8d95e8e6c169cc23747de58ebae3
 icub-main commit:7baee55743fef19fa163f12231bc0aa65354fb0b idyntree
 commit:40ba1b919b5f730faf597cc4237b492d03f45708

---
 iCub/robots/iCubDarmstadt01/model.urdf     | 374 ++++++++++-----------
 iCub/robots/iCubGazeboV2_5/model.urdf      | 372 ++++++++++----------
 iCub/robots/iCubGazeboV2_5_plus/model.urdf | 346 +++++++++----------
 iCub/robots/iCubGazeboV3/model.urdf        | 130 +++----
 iCub/robots/iCubGenova01/model.urdf        | 374 ++++++++++-----------
 iCub/robots/iCubGenova02/model.urdf        | 348 +++++++++----------
 iCub/robots/iCubGenova04/model.urdf        | 374 ++++++++++-----------
 iCub/robots/iCubGenova04_plus/model.urdf   | 348 +++++++++----------
 iCub/robots/iCubGenova09/model.urdf        | 130 +++----
 9 files changed, 1398 insertions(+), 1398 deletions(-)

diff --git a/iCub/robots/iCubDarmstadt01/model.urdf b/iCub/robots/iCubDarmstadt01/model.urdf
index 6d625da..796e5eb 100644
--- a/iCub/robots/iCubDarmstadt01/model.urdf
+++ b/iCub/robots/iCubDarmstadt01/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -525,7 +525,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.000327222" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.000248285" iyz="-4.29975e-06" izz="0.000150937"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2646,13 +2646,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2694,13 +2694,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2742,13 +2742,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2766,13 +2766,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2802,13 +2802,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2826,13 +2826,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2850,13 +2850,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2992,24 +2992,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3051,13 +3051,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3075,178 +3075,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3264,25 +3264,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3356,45 +3356,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGazeboV2_5/model.urdf b/iCub/robots/iCubGazeboV2_5/model.urdf
index fdead95..9013aac 100644
--- a/iCub/robots/iCubGazeboV2_5/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.01" iyz="1.19785e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="0.01" iyz="3.92498e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="0.01" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="0.01" iyz="-1.59685e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.01" iyz="1.20112e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="0.01" iyz="3.9319e-06" izz="0.01"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="0.01" ixy="3.08362e-08" ixz="9.42153e-07" iyy="0.01" iyz="-1.58924e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="0.01" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="0.01" iyz="3.32338e-08" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.01" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.01" iyz="-4.29975e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2646,13 +2646,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2694,13 +2694,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2742,13 +2742,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2766,13 +2766,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2802,13 +2802,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2826,13 +2826,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2850,13 +2850,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2992,24 +2992,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3051,13 +3051,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3075,178 +3075,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3264,25 +3264,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3356,45 +3356,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGazeboV2_5_plus/model.urdf b/iCub/robots/iCubGazeboV2_5_plus/model.urdf
index e1f46c8..c975f30 100644
--- a/iCub/robots/iCubGazeboV2_5_plus/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5_plus/model.urdf
@@ -247,12 +247,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.01" iyz="1.19785e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -261,15 +261,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -298,7 +298,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -335,12 +335,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="0.01" iyz="3.92498e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -349,7 +349,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -365,7 +365,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -386,7 +386,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -395,35 +395,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -444,14 +444,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -479,7 +479,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -500,35 +500,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -542,7 +542,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -563,19 +563,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="0.01" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="0.01" iyz="-1.59685e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -584,14 +584,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -621,7 +621,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -667,12 +667,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.01" iyz="1.20112e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -681,15 +681,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -718,7 +718,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -755,7 +755,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="0.01" iyz="3.9319e-06" izz="0.01"/>
     </inertial>
@@ -785,7 +785,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -806,7 +806,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -815,35 +815,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -864,7 +864,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -920,35 +920,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -962,7 +962,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -983,19 +983,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="0.01" ixy="3.08362e-08" ixz="9.42153e-07" iyy="0.01" iyz="-1.58924e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1004,14 +1004,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1041,7 +1041,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1057,12 +1057,12 @@
   </joint>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1071,7 +1071,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1247,7 +1247,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1275,7 +1275,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1303,21 +1303,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1352,14 +1352,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1373,21 +1373,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1408,14 +1408,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1436,7 +1436,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1450,7 +1450,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1485,12 +1485,12 @@
   </joint>
   <link name="r_ankle_1">
     <inertial>
-      <origin xyz="0.015143405473968821 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015143405473968807 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6803"/>
       <inertia ixx="0.0004046207392315529" ixy="8.523488844593136e-06" ixz="-2.8913590319053794e-05" iyy="0.0004379396781535379" iyz="3.7330395179426065e-05" izz="0.00031892819418424145"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1499,7 +1499,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1515,12 +1515,12 @@
   </joint>
   <link name="r_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235187 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235048 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.01" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.01" iyz="2.0701e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1529,7 +1529,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1537,7 +1537,7 @@
   </link>
   <joint name="r_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.1979394973216095e-07"/>
+    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.197939497389039e-07"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1545,12 +1545,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <mass value="0.357881"/>
       <inertia ixx="0.00109611" ixy="-2.9193e-07" ixz="-3.70109e-05" iyy="0.00125706" iyz="5.10952e-06" izz="0.000215359"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1566,7 +1566,7 @@
     </collision>
   </link>
   <joint name="r_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot"/>
     <dynamics damping="0.1"/>
@@ -1580,12 +1580,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1594,7 +1594,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1610,12 +1610,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0015761999999999929 -1.940000000000275e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0015761999999999998 -1.939999999998887e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.421106"/>
       <inertia ixx="0.000494284" ixy="1.5483e-07" ixz="-1.86308e-07" iyy="0.000288736" iyz="1.52962e-05" izz="0.000319008"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1624,14 +1624,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1640,12 +1640,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012299999999999117 -0.0002823000000000131 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.0001229999999999981 -0.000282300000000027 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="0.01" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="0.01" iyz="3.32338e-08" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1654,26 +1654,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008574999999999972 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008575000000000041 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868000000000092 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868000000000023 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1682,7 +1682,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1698,7 +1698,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.051256699999999975 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.05125669999999998 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1712,12 +1712,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999700000000013 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997000000000061 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1726,7 +1726,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1742,12 +1742,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015152971856010347 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015152971856010333 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447618553062084" ixy="-8.97867775831874e-06" ixz="2.9134846047326194e-05" iyy="0.00043720150287133953" iyz="3.730206268073148e-05" izz="0.00031828795462876227"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1756,7 +1756,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1772,12 +1772,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235465 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235326 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.01" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.01" iyz="2.0701e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1786,7 +1786,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1794,7 +1794,7 @@
   </link>
   <joint name="l_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.1979394973216095e-07"/>
+    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.197939497389039e-07"/>
     <parent link="l_ankle_1"/>
     <child link="l_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1823,7 +1823,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2303,13 +2303,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2351,13 +2351,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2399,13 +2399,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2423,13 +2423,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2459,13 +2459,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2483,13 +2483,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2507,13 +2507,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2649,24 +2649,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2684,13 +2684,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2708,238 +2708,238 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b12" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577099999999994 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.0125771 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577099999999994 -0.040407200000000004 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.0125771 -0.040407200000000004 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012575099999999985 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575099999999992 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999985 0.040407 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999992 0.040407 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013080999999999995 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000002 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013080999999999995 -0.040407200000000004 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000002 -0.040407200000000004 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01307899999999998 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013078999999999986 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.01307899999999998 0.040407 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999986 0.040407 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001630000000005 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001630000000005 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352109 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352108999999999 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352109 0.007765700000000028 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352108999999999 0.007765700000000028 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053785 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053785 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 -0.014429399999999995 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429399999999995 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412948999999999 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.034129489999999985 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412948999999999 -0.041499899999999985 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.034129489999999985 -0.041499899999999985 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999998 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043784999999999984 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999998 0.01781110000000001 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043784999999999984 0.01781110000000001 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.043785 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378499999999999 -0.014191799999999997 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785 -0.014191799999999997 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313347 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133469999999995 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313347 0.017184100000000008 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133469999999995 0.017184100000000008 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.054451799999999995 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
+      <pose>0.05445179999999999 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b9" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.054451799999999995 -0.013799100000000002 -0.0043179999999999885"/>
+    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.05445179999999999 -0.013799100000000002 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.02591510000000001 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.02591510000000001 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999994 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999994 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGazeboV3/model.urdf b/iCub/robots/iCubGazeboV3/model.urdf
index 6a11326..036c5b5 100644
--- a/iCub/robots/iCubGazeboV3/model.urdf
+++ b/iCub/robots/iCubGazeboV3/model.urdf
@@ -406,7 +406,7 @@
   </link>
   <joint name="l_shoulder_pitch" type="revolute">
     <origin xyz="-0.020835100000000002 0.0397423 0.1052029" rpy="0.2532616237670649 -1.1982921379466467e-07 -0.26179930185680106"/>
-    <axis xyz="3.323150926732427e-07 0.9999999783534343 1.445651151288363e-07"/>
+    <axis xyz="3.3231509266596455e-07 0.9999999783534343 1.4456511512603564e-07"/>
     <parent link="chest"/>
     <child link="l_shoulder_1"/>
     <limit effort="50000" lower="-1.53588974175501" upper="0.22689280275926285" velocity="120.0"/>
@@ -414,12 +414,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.0032933620254868984 0.0002563804707528261 -0.027865685757097337" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.003293362025486902 0.0002563804707528261 -0.027865685757097337" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.322126"/>
       <inertia ixx="0.01" ixy="-3.5032e-05" ixz="-2.4436e-05" iyy="0.01" iyz="-5.52877e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.026393097869193378 -0.14153112610649202 -0.0072523976446021024" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.02639309786919335 -0.14153112610649204 -0.007252397644602098" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -428,15 +428,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.026393097869193378 -0.14153112610649202 -0.0072523976446021024" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.02639309786919335 -0.14153112610649204 -0.007252397644602098" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="0.004992849332498911 0.11149983845605199 0.00026865023223796025" rpy="-0.1745329613669159 -0.05374759926346645 0"/>
-    <axis xyz="0.9999998262417684 2.0348252649342313e-07 -4.0230228454118944e-08"/>
+    <origin xyz="0.004992849332498911 0.11149983845605199 0.00026865023223796025" rpy="-0.17453296136691587 -0.05374759926346641 0"/>
+    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.792526803190927" velocity="120.0"/>
@@ -444,12 +444,12 @@
   </joint>
   <link name="l_shoulder_3">
     <inertial>
-      <origin xyz="-4.41626402200232e-06 9.587749727052453e-05 -0.007326213473011087" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-4.416264022005789e-06 9.587749727052453e-05 -0.007326213473011087" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.256128"/>
       <inertia ixx="0.01" ixy="-3.76336e-07" ixz="2.93189e-06" iyy="0.01" iyz="-3.52325e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743231026171713 -0.1415314893638154 0.047047641079459614" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.03074323102617168 -0.1415314893638154 0.04704764107945962" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -458,26 +458,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743231026171713 -0.1415314893638154 0.047047641079459614" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.03074323102617168 -0.1415314893638154 0.04704764107945962" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_arm_ft_sensor" type="fixed">
-    <origin xyz="-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172" rpy="0 0 0"/>
+    <origin xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="0 0 0"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_arm">
     <inertial>
-      <origin xyz="0.006736834478024381 -0.02179925575103303 -0.04590612014684704" rpy="-0.07156449487846148 -0.06196998844956253 -1.295357449870532"/>
+      <origin xyz="0.006736834478024374 -0.021799255751033003 -0.04590612014684704" rpy="-0.07156449487846149 -0.06196998844956256 -1.2953574498705325"/>
       <mass value="2.07167"/>
       <inertia ixx="0.0038606" ixy="0.000215061" ixz="-5.23479e-05" iyy="0.018139" iyz="0.000254564" izz="0.0161052"/>
     </inertial>
     <visual>
-      <origin xyz="0.03074314946383008 -0.14153106591257655 0.06954758245615261" rpy="-0.07156449487846148 -0.06196998844956253 -1.295357449870532"/>
+      <origin xyz="0.030743149463830045 -0.14153106591257655 0.06954758245615261" rpy="-0.07156449487846149 -0.06196998844956256 -1.2953574498705325"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -486,15 +486,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03074314946383008 -0.14153106591257655 0.06954758245615261" rpy="-0.07156449487846148 -0.06196998844956253 -1.295357449870532"/>
+      <origin xyz="0.030743149463830045 -0.14153106591257655 0.06954758245615261" rpy="-0.07156449487846149 -0.06196998844956256 -1.2953574498705325"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0 -4.2345248449371375e-07 -0.02249994137312443" rpy="0 0 0"/>
-    <axis xyz="-2.227647404040578e-08 4.393786245948661e-08 1.0000002566182309"/>
+    <origin xyz="0 -4.2345248449371375e-07 -0.022499941373124423" rpy="0 0 0"/>
+    <axis xyz="-2.2276474049102235e-08 4.393786247302041e-08 1.0000002566182309"/>
     <parent link="l_shoulder_3"/>
     <child link="l_upper_arm"/>
     <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
@@ -502,12 +502,12 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="-0.010241946078109292 0.0018224069895867168 -0.01508962025855548" rpy="-0.07156449487846119 -0.0619699884495625 -1.2953574498905749"/>
+      <origin xyz="-0.010241946078109288 0.0018224069895867168 -0.015089620258555453" rpy="-0.07156449487846121 -0.06196998844956255 -1.2953574498905756"/>
       <mass value="0.230254"/>
       <inertia ixx="0.01" ixy="7.51175e-07" ixz="-9.8725e-06" iyy="0.01" iyz="-1.03338e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.02074321922607398 -0.14003141848976391 0.19324791999373642" rpy="-0.07156449487846119 -0.0619699884495625 -1.2953574498905749"/>
+      <origin xyz="0.020743219226073948 -0.14003141848976391 0.19324791999373644" rpy="-0.07156449487846121 -0.06196998844956255 -1.2953574498905756"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -516,7 +516,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.02074321922607398 -0.14003141848976391 0.19324791999373642" rpy="-0.07156449487846119 -0.0619699884495625 -1.2953574498905749"/>
+      <origin xyz="0.020743219226073948 -0.14003141848976391 0.19324791999373644" rpy="-0.07156449487846121 -0.06196998844956255 -1.2953574498905756"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -524,7 +524,7 @@
   </link>
   <joint name="l_elbow" type="revolute">
     <origin xyz="0.009999930234919404 -0.001499647423428896 -0.12370033753758375" rpy="0 0 0"/>
-    <axis xyz="-8.811824154381087e-08 -1.0000002733514628 4.4047361189014644e-08"/>
+    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
     <parent link="l_upper_arm"/>
     <child link="l_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -532,12 +532,12 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="-0.006269874102298061 -4.68551245806359e-05 -0.030648347755607153" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-0.0062698741022980675 -4.68551245806359e-05 -0.030648347755607208" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.90793"/>
       <inertia ixx="0.00131822" ixy="-7.39737e-07" ixz="-2.69292e-05" iyy="0.00128448" iyz="0.000108551" izz="0.000646975"/>
     </inertial>
     <visual>
-      <origin xyz="0.03574308988793748 -0.14203106199153737 0.2485479835726478" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035743089887937445 -0.14203106199153737 0.2485479835726478" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -546,15 +546,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03574308988793748 -0.14203106199153737 0.2485479835726478" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035743089887937445 -0.14203106199153737 0.2485479835726478" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="-0.014999870654177704 0.0019996435036348514 -0.055300063582479825" rpy="0 0 0"/>
-    <axis xyz="-2.227647404040578e-08 4.393786245948661e-08 1.0000002566182309"/>
+    <origin xyz="-0.0149998706541777 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
+    <axis xyz="-2.2276474049102235e-08 4.393786247302041e-08 1.0000002566182309"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -562,12 +562,12 @@
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.014390150453737368 -0.0005083960996520587 -9.893035257668714e-05" rpy="-0.07156449455958307 -0.06196998853951186 -1.2953574498501934"/>
+      <origin xyz="-0.014390150453737368 -0.0005083960996520864 -9.893035257668714e-05" rpy="-0.0715644945595831 -0.0619699885395119 -1.2953574498501936"/>
       <mass value="0.0344005"/>
       <inertia ixx="0.01" ixy="-9.19776e-07" ixz="-1.4756e-08" iyy="0.01" iyz="-1.11493e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.019143073890582344 -0.14403795720942242 0.3560476247256178" rpy="-0.07156449455958307 -0.06196998853951186 -1.2953574498501934"/>
+      <origin xyz="0.01914307389058231 -0.14403795720942245 0.3560476247256178" rpy="-0.0715644945595831 -0.0619699885395119 -1.2953574498501936"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -576,15 +576,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.019143073890582344 -0.14403795720942242 0.3560476247256178" rpy="-0.07156449455958307 -0.06196998853951186 -1.2953574498501934"/>
+      <origin xyz="0.01914307389058231 -0.14403795720942245 0.3560476247256178" rpy="-0.0715644945595831 -0.0619699885395119 -1.2953574498501936"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.016600015934286182 0.002006895218072541 -0.10749964114254112" rpy="0 0 0"/>
-    <axis xyz="0.9999998262417684 2.0348252649342313e-07 -4.0230228454118944e-08"/>
+    <origin xyz="0.016600015934286182 0.0020068952180725685 -0.10749964114254112" rpy="0 0 0"/>
+    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -592,12 +592,12 @@
   </joint>
   <link name="l_hand">
     <inertial>
-      <origin xyz="0.004786523181334722 -0.0061337378723908265 -0.0672770617484692" rpy="-0.07156449455958207 -0.061969988539511835 -1.295357449850193"/>
+      <origin xyz="0.004786523181334726 -0.0061337378723908265 -0.0672770617484692" rpy="-0.07156449455958207 -0.06196998853951186 -1.2953574498501934"/>
       <mass value="0.263541"/>
       <inertia ixx="0.01" ixy="-4.77094e-05" ixz="-1.03141e-05" iyy="0.01" iyz="1.19895e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.035443195635311536 -0.13703856452785146 0.3560477032643494" rpy="-0.07156449455958207 -0.061969988539511835 -1.295357449850193"/>
+      <origin xyz="0.0354431956353115 -0.13703856452785146 0.3560477032643494" rpy="-0.07156449455958207 -0.06196998853951186 -1.2953574498501934"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -606,15 +606,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.035443195635311536 -0.13703856452785146 0.3560477032643494" rpy="-0.07156449455958207 -0.061969988539511835 -1.295357449850193"/>
+      <origin xyz="0.0354431956353115 -0.13703856452785146 0.3560477032643494" rpy="-0.07156449455958207 -0.06196998853951186 -1.2953574498501934"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.016300121744729387 -0.0069993926815711816 0" rpy="0 0 0"/>
-    <axis xyz="-8.811824154381087e-08 -1.0000002733514628 4.4047361189014644e-08"/>
+    <origin xyz="-0.016300121744729387 -0.006999392681571209 0" rpy="0 0 0"/>
+    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
     <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
@@ -644,7 +644,7 @@
   </link>
   <joint name="r_shoulder_pitch" type="revolute">
     <origin xyz="-0.020835100000000002 -0.0397423 0.1052029" rpy="-0.25326164431680925 2.3014774394592079e-07 0.26179970938509745"/>
-    <axis xyz="1.499044390054707e-07 0.9999999783534812 -1.2401537036188515e-07"/>
+    <axis xyz="1.499044390098182e-07 0.9999999783534812 -1.2401537036323317e-07"/>
     <parent link="chest"/>
     <child link="r_shoulder_1"/>
     <limit effort="50000" lower="-1.53588974175501" upper="0.22689280275926285" velocity="120.0"/>
@@ -652,12 +652,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="0.0007118441514370015 -0.0002899713150672778 -0.027865575116629175" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.000711844151437005 -0.00028997131506725005 -0.027865575116629175" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.322126"/>
       <inertia ixx="0.01" ixy="3.44034e-05" ixz="2.39129e-05" iyy="0.01" iyz="-5.45925e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.023893261066846305 0.14153171676199225 -0.007252350207672735" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.02389326106684631 0.14153171676199228 -0.007252350207672734" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -666,15 +666,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.023893261066846305 0.14153171676199225 -0.007252350207672735" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.02389326106684631 0.14153171676199228 -0.007252350207672734" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069357 -0.053747792579130614 -6.132332688812785e-07"/>
-    <axis xyz="-0.9999998262417886 4.2105014356162707e-08 2.4630481189336084e-08"/>
+    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069352 -0.05374779257913059 -6.132332688804098e-07"/>
+    <axis xyz="-0.9999998262417886 4.210501434612913e-08 2.4630481182686846e-08"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.792526803190927" velocity="120.0"/>
@@ -682,12 +682,12 @@
   </joint>
   <link name="r_shoulder_3">
     <inertial>
-      <origin xyz="-8.928496484206933e-05 -3.376971940446949e-05 -0.00759450025288725" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-8.928496484206933e-05 -3.376971940446949e-05 -0.007594500252887257" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.258699"/>
       <inertia ixx="0.01" ixy="5.87638e-07" ixz="-2.8754e-06" iyy="0.01" iyz="-3.42816e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743207452236184 0.1415314977120317 0.04704763137021825" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743207452236184 0.1415314977120317 0.047047631370218244" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -696,26 +696,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743207452236184 0.1415314977120317 0.04704763137021825" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743207452236184 0.1415314977120317 0.047047631370218244" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_arm_ft_sensor" type="fixed">
-    <origin xyz="-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099" rpy="0 0 0"/>
+    <origin xyz="-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099" rpy="0 0 0"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_upper_arm">
     <inertial>
-      <origin xyz="-4.8274954764261635e-06 -0.001943485876269635 -0.04747357932833829" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-4.827495476422694e-06 -0.001943485876269635 -0.04747357932833829" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="2.06334"/>
       <inertia ixx="0.00382639" ixy="-5.03894e-06" ixz="0.000205684" iyy="0.00368146" iyz="0.000261155" izz="0.00167002"/>
     </inertial>
     <visual>
-      <origin xyz="0.03074312554381853 0.141531075879301 0.06954757274337192" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743125543818535 0.141531075879301 0.06954757274337192" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -724,15 +724,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03074312554381853 0.141531075879301 0.06954757274337192" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743125543818535 0.141531075879301 0.06954757274337192" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="0 4.2183273077367467e-07 -0.022499941373153573" rpy="0 0 0"/>
-    <axis xyz="-3.787621790513063e-08 2.805197706068352e-08 1.0000002566182309"/>
+    <origin xyz="0 4.2183273077367467e-07 -0.02249994137315358" rpy="0 0 0"/>
+    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
     <parent link="r_shoulder_3"/>
     <child link="r_upper_arm"/>
     <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
@@ -740,12 +740,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.011624818009452066 -0.00180281594701992 0.022300137248866647" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-0.011624818009452066 -0.0018028159470199479 0.022300137248866647" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.397478"/>
       <inertia ixx="0.01" ixy="5.80993e-06" ixz="5.27143e-05" iyy="0.01" iyz="6.49755e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.020743193621214625 0.14003143574727356 0.19324791023291843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.020743193621214625 0.14003143574727353 0.19324791023291843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -754,15 +754,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.020743193621214625 0.14003143574727356 0.19324791023291843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.020743193621214625 0.14003143574727353 0.19324791023291843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_elbow" type="revolute">
-    <origin xyz="0.009999931922603816 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
-    <axis xyz="2.4949582809984105e-07 -1.0000002733514362 2.7942487004772865e-08"/>
+    <origin xyz="0.00999993192260382 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
+    <axis xyz="2.4949582807394525e-07 -1.0000002733514362 2.7942487010105877e-08"/>
     <parent link="r_upper_arm"/>
     <child link="r_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -791,8 +791,8 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.014999869468812953 -0.001999649905318951 -0.05530006367251966" rpy="0 0 0"/>
-    <axis xyz="-3.787621790513063e-08 2.805197706068352e-08 1.0000002566182309"/>
+    <origin xyz="-0.014999869468812953 -0.0019996499053189787 -0.05530006367251966" rpy="0 0 0"/>
+    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -800,12 +800,12 @@
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="-0.014389827079018234 0.0005092753390522065 -9.787315764231419e-05" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-0.014389827079018234 0.0005092753390521787 -9.78731576423697e-05" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.0344005"/>
       <inertia ixx="0.01" ixy="9.19776e-07" ixz="1.47559e-08" iyy="0.01" iyz="-1.11493e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.01944310973607908 0.1440385285434045 0.3560480536175445" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.01944310973607908 0.14403852854340446 0.35604805361754444" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -814,7 +814,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.01944310973607908 0.1440385285434045 0.3560480536175445" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.01944310973607908 0.14403852854340446 0.35604805361754444" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -822,7 +822,7 @@
   </link>
   <joint name="r_wrist_pitch" type="revolute">
     <origin xyz="0.016299953353948633 -0.002007442890811839 -0.10750007971210637" rpy="0 0 0"/>
-    <axis xyz="0.9999998262417886 -4.2105014356162707e-08 -2.4630481189336084e-08"/>
+    <axis xyz="0.9999998262417886 -4.210501434612913e-08 -2.4630481182686846e-08"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -830,12 +830,12 @@
   </joint>
   <link name="r_hand">
     <inertial>
-      <origin xyz="0.004828314385249662 0.00615667237451184 -0.0671378028889843" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
+      <origin xyz="0.004828314385249669 0.00615667237451184 -0.0671378028889843" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
       <mass value="0.264229"/>
       <inertia ixx="0.01" ixy="4.7873e-05" ixz="1.1114e-05" iyy="0.01" iyz="1.14469e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.035743235789782016 0.1370391384923153 0.35604813259525964" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
+      <origin xyz="0.03574323578978202 0.1370391384923153 0.35604813259525964" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -844,15 +844,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.035743235789782016 0.1370391384923153 0.35604813259525964" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
+      <origin xyz="0.03574323578978202 0.1370391384923153 0.35604813259525964" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="continuous">
-    <origin xyz="-0.016300122874272734 0.0069993900510918194 0" rpy="0 0 0"/>
-    <axis xyz="-2.4949582809984105e-07 1.0000002733514362 -2.7942487004772865e-08"/>
+    <origin xyz="-0.016300122874272734 0.006999390051091792 0" rpy="0 0 0"/>
+    <axis xyz="-2.4949582807394525e-07 1.0000002733514362 -2.7942487010105877e-08"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
     <dynamics damping="0.1"/>
diff --git a/iCub/robots/iCubGenova01/model.urdf b/iCub/robots/iCubGenova01/model.urdf
index 6d625da..796e5eb 100644
--- a/iCub/robots/iCubGenova01/model.urdf
+++ b/iCub/robots/iCubGenova01/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -525,7 +525,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.000327222" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.000248285" iyz="-4.29975e-06" izz="0.000150937"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2646,13 +2646,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2694,13 +2694,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2742,13 +2742,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2766,13 +2766,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2802,13 +2802,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2826,13 +2826,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2850,13 +2850,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2992,24 +2992,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3051,13 +3051,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3075,178 +3075,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3264,25 +3264,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3356,45 +3356,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGenova02/model.urdf b/iCub/robots/iCubGenova02/model.urdf
index 4cd589a..21dd648 100644
--- a/iCub/robots/iCubGenova02/model.urdf
+++ b/iCub/robots/iCubGenova02/model.urdf
@@ -247,12 +247,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -261,15 +261,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -298,7 +298,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -335,12 +335,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -349,7 +349,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -365,7 +365,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -386,7 +386,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -395,35 +395,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -444,14 +444,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -479,7 +479,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -500,35 +500,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -542,7 +542,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -563,19 +563,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -584,14 +584,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -621,7 +621,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -667,12 +667,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -681,15 +681,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -718,7 +718,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -755,7 +755,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -785,7 +785,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -806,7 +806,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -815,35 +815,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -864,7 +864,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -920,35 +920,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -962,7 +962,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -983,19 +983,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1004,14 +1004,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1041,7 +1041,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1057,12 +1057,12 @@
   </joint>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1071,7 +1071,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1247,7 +1247,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1275,7 +1275,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1303,21 +1303,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1352,14 +1352,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1373,21 +1373,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1408,14 +1408,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1436,7 +1436,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1450,7 +1450,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1485,12 +1485,12 @@
   </joint>
   <link name="r_ankle_1">
     <inertial>
-      <origin xyz="0.015143405473968821 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015143405473968807 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6803"/>
       <inertia ixx="0.0004046207392315529" ixy="8.523488844593136e-06" ixz="-2.8913590319053794e-05" iyy="0.0004379396781535379" iyz="3.7330395179426065e-05" izz="0.00031892819418424145"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1499,7 +1499,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1515,12 +1515,12 @@
   </joint>
   <link name="r_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235187 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235048 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.000327646" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.000244632" iyz="2.0701e-05" izz="0.000155777"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1529,7 +1529,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1537,7 +1537,7 @@
   </link>
   <joint name="r_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.1979394973216095e-07"/>
+    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.197939497389039e-07"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1545,12 +1545,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <mass value="0.357881"/>
       <inertia ixx="0.00109611" ixy="-2.9193e-07" ixz="-3.70109e-05" iyy="0.00125706" iyz="5.10952e-06" izz="0.000215359"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1559,14 +1559,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot"/>
     <dynamics damping="0.1"/>
@@ -1580,12 +1580,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1594,7 +1594,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1610,12 +1610,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0015761999999999929 -1.940000000000275e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0015761999999999998 -1.939999999998887e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.421106"/>
       <inertia ixx="0.000494284" ixy="1.5483e-07" ixz="-1.86308e-07" iyy="0.000288736" iyz="1.52962e-05" izz="0.000319008"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1624,14 +1624,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1640,12 +1640,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012299999999999117 -0.0002823000000000131 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.0001229999999999981 -0.000282300000000027 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1654,26 +1654,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008574999999999972 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008575000000000041 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868000000000092 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868000000000023 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1682,7 +1682,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1698,7 +1698,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.051256699999999975 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.05125669999999998 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1712,12 +1712,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999700000000013 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997000000000061 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1726,7 +1726,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1742,12 +1742,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015152971856010347 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015152971856010333 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447618553062084" ixy="-8.97867775831874e-06" ixz="2.9134846047326194e-05" iyy="0.00043720150287133953" iyz="3.730206268073148e-05" izz="0.00031828795462876227"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1756,7 +1756,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1772,12 +1772,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235465 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235326 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.000327646" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.000244632" iyz="2.0701e-05" izz="0.000155777"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1786,7 +1786,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1794,7 +1794,7 @@
   </link>
   <joint name="l_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.1979394973216095e-07"/>
+    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.197939497389039e-07"/>
     <parent link="l_ankle_1"/>
     <child link="l_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1823,7 +1823,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2303,13 +2303,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2351,13 +2351,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2399,13 +2399,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2423,13 +2423,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2459,13 +2459,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2483,13 +2483,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2507,13 +2507,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2649,24 +2649,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2684,13 +2684,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2708,238 +2708,238 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b12" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577099999999994 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.0125771 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577099999999994 -0.040407200000000004 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.0125771 -0.040407200000000004 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012575099999999985 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575099999999992 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999985 0.040407 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999992 0.040407 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013080999999999995 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000002 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013080999999999995 -0.040407200000000004 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000002 -0.040407200000000004 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01307899999999998 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013078999999999986 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.01307899999999998 0.040407 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999986 0.040407 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001630000000005 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001630000000005 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352109 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352108999999999 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352109 0.007765700000000028 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352108999999999 0.007765700000000028 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053785 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053785 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 -0.014429399999999995 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429399999999995 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412948999999999 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.034129489999999985 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412948999999999 -0.041499899999999985 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.034129489999999985 -0.041499899999999985 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999998 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043784999999999984 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999998 0.01781110000000001 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043784999999999984 0.01781110000000001 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.043785 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378499999999999 -0.014191799999999997 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785 -0.014191799999999997 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313347 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133469999999995 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313347 0.017184100000000008 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133469999999995 0.017184100000000008 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.054451799999999995 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
+      <pose>0.05445179999999999 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b9" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.054451799999999995 -0.013799100000000002 -0.0043179999999999885"/>
+    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.05445179999999999 -0.013799100000000002 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.02591510000000001 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.02591510000000001 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999994 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999994 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGenova04/model.urdf b/iCub/robots/iCubGenova04/model.urdf
index 3077afd..ff43722 100644
--- a/iCub/robots/iCubGenova04/model.urdf
+++ b/iCub/robots/iCubGenova04/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -525,7 +525,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.000327222" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.000248285" iyz="-4.29975e-06" izz="0.000150937"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2646,13 +2646,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2694,13 +2694,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2742,13 +2742,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2766,13 +2766,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2802,13 +2802,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2826,13 +2826,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2850,13 +2850,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2992,24 +2992,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3051,13 +3051,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3075,178 +3075,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3264,25 +3264,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3356,45 +3356,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGenova04_plus/model.urdf b/iCub/robots/iCubGenova04_plus/model.urdf
index 4cd589a..21dd648 100644
--- a/iCub/robots/iCubGenova04_plus/model.urdf
+++ b/iCub/robots/iCubGenova04_plus/model.urdf
@@ -247,12 +247,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -261,15 +261,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -298,7 +298,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -335,12 +335,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -349,7 +349,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -365,7 +365,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -386,7 +386,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -395,35 +395,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -444,14 +444,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -479,7 +479,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -500,35 +500,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -542,7 +542,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -563,19 +563,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -584,14 +584,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -621,7 +621,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -667,12 +667,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -681,15 +681,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -718,7 +718,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -755,7 +755,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -785,7 +785,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -806,7 +806,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -815,35 +815,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -864,7 +864,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -920,35 +920,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -962,7 +962,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -983,19 +983,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1004,14 +1004,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1041,7 +1041,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1057,12 +1057,12 @@
   </joint>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1071,7 +1071,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1247,7 +1247,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1275,7 +1275,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1303,21 +1303,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1352,14 +1352,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1373,21 +1373,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1408,14 +1408,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1436,7 +1436,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1450,7 +1450,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1485,12 +1485,12 @@
   </joint>
   <link name="r_ankle_1">
     <inertial>
-      <origin xyz="0.015143405473968821 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015143405473968807 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6803"/>
       <inertia ixx="0.0004046207392315529" ixy="8.523488844593136e-06" ixz="-2.8913590319053794e-05" iyy="0.0004379396781535379" iyz="3.7330395179426065e-05" izz="0.00031892819418424145"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1499,7 +1499,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1515,12 +1515,12 @@
   </joint>
   <link name="r_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235187 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235048 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.000327646" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.000244632" iyz="2.0701e-05" izz="0.000155777"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1529,7 +1529,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1537,7 +1537,7 @@
   </link>
   <joint name="r_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.1979394973216095e-07"/>
+    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.197939497389039e-07"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1545,12 +1545,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <mass value="0.357881"/>
       <inertia ixx="0.00109611" ixy="-2.9193e-07" ixz="-3.70109e-05" iyy="0.00125706" iyz="5.10952e-06" izz="0.000215359"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1559,14 +1559,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot"/>
     <dynamics damping="0.1"/>
@@ -1580,12 +1580,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1594,7 +1594,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1610,12 +1610,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0015761999999999929 -1.940000000000275e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0015761999999999998 -1.939999999998887e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.421106"/>
       <inertia ixx="0.000494284" ixy="1.5483e-07" ixz="-1.86308e-07" iyy="0.000288736" iyz="1.52962e-05" izz="0.000319008"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1624,14 +1624,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1640,12 +1640,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012299999999999117 -0.0002823000000000131 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.0001229999999999981 -0.000282300000000027 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1654,26 +1654,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008574999999999972 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008575000000000041 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868000000000092 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868000000000023 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1682,7 +1682,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1698,7 +1698,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.051256699999999975 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.05125669999999998 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1712,12 +1712,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999700000000013 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997000000000061 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1726,7 +1726,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1742,12 +1742,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015152971856010347 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015152971856010333 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447618553062084" ixy="-8.97867775831874e-06" ixz="2.9134846047326194e-05" iyy="0.00043720150287133953" iyz="3.730206268073148e-05" izz="0.00031828795462876227"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1756,7 +1756,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1772,12 +1772,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235465 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235326 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.000327646" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.000244632" iyz="2.0701e-05" izz="0.000155777"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1786,7 +1786,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1794,7 +1794,7 @@
   </link>
   <joint name="l_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.1979394973216095e-07"/>
+    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.197939497389039e-07"/>
     <parent link="l_ankle_1"/>
     <child link="l_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1823,7 +1823,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2303,13 +2303,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2351,13 +2351,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2399,13 +2399,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2423,13 +2423,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2459,13 +2459,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2483,13 +2483,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2507,13 +2507,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2649,24 +2649,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2684,13 +2684,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2708,238 +2708,238 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b12" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577099999999994 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.0125771 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577099999999994 -0.040407200000000004 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.0125771 -0.040407200000000004 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012575099999999985 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575099999999992 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999985 0.040407 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999992 0.040407 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013080999999999995 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000002 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013080999999999995 -0.040407200000000004 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000002 -0.040407200000000004 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01307899999999998 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013078999999999986 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.01307899999999998 0.040407 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999986 0.040407 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001630000000005 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001630000000005 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352109 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352108999999999 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352109 0.007765700000000028 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352108999999999 0.007765700000000028 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053785 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053785 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 -0.014429399999999995 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429399999999995 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412948999999999 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.034129489999999985 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412948999999999 -0.041499899999999985 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.034129489999999985 -0.041499899999999985 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999998 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043784999999999984 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999998 0.01781110000000001 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043784999999999984 0.01781110000000001 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.043785 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378499999999999 -0.014191799999999997 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785 -0.014191799999999997 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313347 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133469999999995 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313347 0.017184100000000008 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133469999999995 0.017184100000000008 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.054451799999999995 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
+      <pose>0.05445179999999999 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b9" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.054451799999999995 -0.013799100000000002 -0.0043179999999999885"/>
+    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.05445179999999999 -0.013799100000000002 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.02591510000000001 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.02591510000000001 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999994 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999994 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGenova09/model.urdf b/iCub/robots/iCubGenova09/model.urdf
index 8edb534..76a10b0 100644
--- a/iCub/robots/iCubGenova09/model.urdf
+++ b/iCub/robots/iCubGenova09/model.urdf
@@ -406,7 +406,7 @@
   </link>
   <joint name="l_shoulder_pitch" type="revolute">
     <origin xyz="-0.020835100000000002 0.0397423 0.1052029" rpy="0.2532616237670649 -1.1982921379466467e-07 -0.26179930185680106"/>
-    <axis xyz="3.323150926732427e-07 0.9999999783534343 1.445651151288363e-07"/>
+    <axis xyz="3.3231509266596455e-07 0.9999999783534343 1.4456511512603564e-07"/>
     <parent link="chest"/>
     <child link="l_shoulder_1"/>
     <limit effort="50000" lower="-1.53588974175501" upper="0.22689280275926285" velocity="120.0"/>
@@ -414,12 +414,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.0032933620254868984 0.0002563804707528261 -0.027865685757097337" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.003293362025486902 0.0002563804707528261 -0.027865685757097337" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.322126"/>
       <inertia ixx="0.000311039" ixy="-3.5032e-05" ixz="-2.4436e-05" iyy="0.000216027" iyz="-5.52877e-05" izz="0.00022365"/>
     </inertial>
     <visual>
-      <origin xyz="0.026393097869193378 -0.14153112610649202 -0.0072523976446021024" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.02639309786919335 -0.14153112610649204 -0.007252397644602098" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -428,15 +428,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.026393097869193378 -0.14153112610649202 -0.0072523976446021024" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.02639309786919335 -0.14153112610649204 -0.007252397644602098" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="0.004992849332498911 0.11149983845605199 0.00026865023223796025" rpy="-0.1745329613669159 -0.05374759926346645 0"/>
-    <axis xyz="0.9999998262417684 2.0348252649342313e-07 -4.0230228454118944e-08"/>
+    <origin xyz="0.004992849332498911 0.11149983845605199 0.00026865023223796025" rpy="-0.17453296136691587 -0.05374759926346641 0"/>
+    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.792526803190927" velocity="120.0"/>
@@ -444,12 +444,12 @@
   </joint>
   <link name="l_shoulder_3">
     <inertial>
-      <origin xyz="-4.41626402200232e-06 9.587749727052453e-05 -0.007326213473011087" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-4.416264022005789e-06 9.587749727052453e-05 -0.007326213473011087" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.256128"/>
       <inertia ixx="7.10321e-05" ixy="-3.76336e-07" ixz="2.93189e-06" iyy="7.06105e-05" iyz="-3.52325e-06" izz="0.000119948"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743231026171713 -0.1415314893638154 0.047047641079459614" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.03074323102617168 -0.1415314893638154 0.04704764107945962" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -458,26 +458,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743231026171713 -0.1415314893638154 0.047047641079459614" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.03074323102617168 -0.1415314893638154 0.04704764107945962" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_arm_ft_sensor" type="fixed">
-    <origin xyz="-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172" rpy="0 0 0"/>
+    <origin xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="0 0 0"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_arm">
     <inertial>
-      <origin xyz="0.006736834478024381 -0.02179925575103303 -0.04590612014684704" rpy="-0.07156449487846148 -0.06196998844956253 -1.295357449870532"/>
+      <origin xyz="0.006736834478024374 -0.021799255751033003 -0.04590612014684704" rpy="-0.07156449487846149 -0.06196998844956256 -1.2953574498705325"/>
       <mass value="2.07167"/>
       <inertia ixx="0.00382639" ixy="0.000215061" ixz="-5.23479e-05" iyy="0.00368146" iyz="0.000254564" izz="0.00167002"/>
     </inertial>
     <visual>
-      <origin xyz="0.03074314946383008 -0.14153106591257655 0.06954758245615261" rpy="-0.07156449487846148 -0.06196998844956253 -1.295357449870532"/>
+      <origin xyz="0.030743149463830045 -0.14153106591257655 0.06954758245615261" rpy="-0.07156449487846149 -0.06196998844956256 -1.2953574498705325"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -486,15 +486,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03074314946383008 -0.14153106591257655 0.06954758245615261" rpy="-0.07156449487846148 -0.06196998844956253 -1.295357449870532"/>
+      <origin xyz="0.030743149463830045 -0.14153106591257655 0.06954758245615261" rpy="-0.07156449487846149 -0.06196998844956256 -1.2953574498705325"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0 -4.2345248449371375e-07 -0.02249994137312443" rpy="0 0 0"/>
-    <axis xyz="-2.227647404040578e-08 4.393786245948661e-08 1.0000002566182309"/>
+    <origin xyz="0 -4.2345248449371375e-07 -0.022499941373124423" rpy="0 0 0"/>
+    <axis xyz="-2.2276474049102235e-08 4.393786247302041e-08 1.0000002566182309"/>
     <parent link="l_shoulder_3"/>
     <child link="l_upper_arm"/>
     <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
@@ -502,12 +502,12 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="-0.010241946078109292 0.0018224069895867168 -0.01508962025855548" rpy="-0.07156449487846119 -0.0619699884495625 -1.2953574498905749"/>
+      <origin xyz="-0.010241946078109288 0.0018224069895867168 -0.015089620258555453" rpy="-0.07156449487846121 -0.06196998844956255 -1.2953574498905756"/>
       <mass value="0.230254"/>
       <inertia ixx="0.0001165" ixy="7.51175e-07" ixz="-9.8725e-06" iyy="0.00012549" iyz="-1.03338e-05" izz="4.38981e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.02074321922607398 -0.14003141848976391 0.19324791999373642" rpy="-0.07156449487846119 -0.0619699884495625 -1.2953574498905749"/>
+      <origin xyz="0.020743219226073948 -0.14003141848976391 0.19324791999373644" rpy="-0.07156449487846121 -0.06196998844956255 -1.2953574498905756"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -516,7 +516,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.02074321922607398 -0.14003141848976391 0.19324791999373642" rpy="-0.07156449487846119 -0.0619699884495625 -1.2953574498905749"/>
+      <origin xyz="0.020743219226073948 -0.14003141848976391 0.19324791999373644" rpy="-0.07156449487846121 -0.06196998844956255 -1.2953574498905756"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -524,7 +524,7 @@
   </link>
   <joint name="l_elbow" type="revolute">
     <origin xyz="0.009999930234919404 -0.001499647423428896 -0.12370033753758375" rpy="0 0 0"/>
-    <axis xyz="-8.811824154381087e-08 -1.0000002733514628 4.4047361189014644e-08"/>
+    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
     <parent link="l_upper_arm"/>
     <child link="l_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -532,12 +532,12 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="-0.006269874102298061 -4.68551245806359e-05 -0.030648347755607153" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-0.0062698741022980675 -4.68551245806359e-05 -0.030648347755607208" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.90793"/>
       <inertia ixx="0.00131822" ixy="-7.39737e-07" ixz="-2.69292e-05" iyy="0.00128448" iyz="0.000108551" izz="0.000646975"/>
     </inertial>
     <visual>
-      <origin xyz="0.03574308988793748 -0.14203106199153737 0.2485479835726478" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035743089887937445 -0.14203106199153737 0.2485479835726478" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -546,15 +546,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03574308988793748 -0.14203106199153737 0.2485479835726478" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035743089887937445 -0.14203106199153737 0.2485479835726478" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="-0.014999870654177704 0.0019996435036348514 -0.055300063582479825" rpy="0 0 0"/>
-    <axis xyz="-2.227647404040578e-08 4.393786245948661e-08 1.0000002566182309"/>
+    <origin xyz="-0.0149998706541777 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
+    <axis xyz="-2.2276474049102235e-08 4.393786247302041e-08 1.0000002566182309"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -562,12 +562,12 @@
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.014390150453737368 -0.0005083960996520587 -9.893035257668714e-05" rpy="-0.07156449455958307 -0.06196998853951186 -1.2953574498501934"/>
+      <origin xyz="-0.014390150453737368 -0.0005083960996520864 -9.893035257668714e-05" rpy="-0.0715644945595831 -0.0619699885395119 -1.2953574498501936"/>
       <mass value="0.0344005"/>
       <inertia ixx="5.11755e-06" ixy="-9.19776e-07" ixz="-1.4756e-08" iyy="1.60849e-06" iyz="-1.11493e-07" izz="5.58546e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.019143073890582344 -0.14403795720942242 0.3560476247256178" rpy="-0.07156449455958307 -0.06196998853951186 -1.2953574498501934"/>
+      <origin xyz="0.01914307389058231 -0.14403795720942245 0.3560476247256178" rpy="-0.0715644945595831 -0.0619699885395119 -1.2953574498501936"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -576,15 +576,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.019143073890582344 -0.14403795720942242 0.3560476247256178" rpy="-0.07156449455958307 -0.06196998853951186 -1.2953574498501934"/>
+      <origin xyz="0.01914307389058231 -0.14403795720942245 0.3560476247256178" rpy="-0.0715644945595831 -0.0619699885395119 -1.2953574498501936"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.016600015934286182 0.002006895218072541 -0.10749964114254112" rpy="0 0 0"/>
-    <axis xyz="0.9999998262417684 2.0348252649342313e-07 -4.0230228454118944e-08"/>
+    <origin xyz="0.016600015934286182 0.0020068952180725685 -0.10749964114254112" rpy="0 0 0"/>
+    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -592,12 +592,12 @@
   </joint>
   <link name="l_hand">
     <inertial>
-      <origin xyz="0.004786523181334722 -0.0061337378723908265 -0.0672770617484692" rpy="-0.07156449455958207 -0.061969988539511835 -1.295357449850193"/>
+      <origin xyz="0.004786523181334726 -0.0061337378723908265 -0.0672770617484692" rpy="-0.07156449455958207 -0.06196998853951186 -1.2953574498501934"/>
       <mass value="0.263541"/>
       <inertia ixx="0.000427427" ixy="-4.77094e-05" ixz="-1.03141e-05" iyy="0.000346557" iyz="1.19895e-05" izz="0.000161212"/>
     </inertial>
     <visual>
-      <origin xyz="0.035443195635311536 -0.13703856452785146 0.3560477032643494" rpy="-0.07156449455958207 -0.061969988539511835 -1.295357449850193"/>
+      <origin xyz="0.0354431956353115 -0.13703856452785146 0.3560477032643494" rpy="-0.07156449455958207 -0.06196998853951186 -1.2953574498501934"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -606,15 +606,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.035443195635311536 -0.13703856452785146 0.3560477032643494" rpy="-0.07156449455958207 -0.061969988539511835 -1.295357449850193"/>
+      <origin xyz="0.0354431956353115 -0.13703856452785146 0.3560477032643494" rpy="-0.07156449455958207 -0.06196998853951186 -1.2953574498501934"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.016300121744729387 -0.0069993926815711816 0" rpy="0 0 0"/>
-    <axis xyz="-8.811824154381087e-08 -1.0000002733514628 4.4047361189014644e-08"/>
+    <origin xyz="-0.016300121744729387 -0.006999392681571209 0" rpy="0 0 0"/>
+    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
     <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
@@ -644,7 +644,7 @@
   </link>
   <joint name="r_shoulder_pitch" type="revolute">
     <origin xyz="-0.020835100000000002 -0.0397423 0.1052029" rpy="-0.25326164431680925 2.3014774394592079e-07 0.26179970938509745"/>
-    <axis xyz="1.499044390054707e-07 0.9999999783534812 -1.2401537036188515e-07"/>
+    <axis xyz="1.499044390098182e-07 0.9999999783534812 -1.2401537036323317e-07"/>
     <parent link="chest"/>
     <child link="r_shoulder_1"/>
     <limit effort="50000" lower="-1.53588974175501" upper="0.22689280275926285" velocity="120.0"/>
@@ -652,12 +652,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="0.0007118441514370015 -0.0002899713150672778 -0.027865575116629175" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.000711844151437005 -0.00028997131506725005 -0.027865575116629175" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.322126"/>
       <inertia ixx="0.000309774" ixy="3.44034e-05" ixz="2.39129e-05" iyy="0.00021568" iyz="-5.45925e-05" izz="0.000222102"/>
     </inertial>
     <visual>
-      <origin xyz="0.023893261066846305 0.14153171676199225 -0.007252350207672735" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.02389326106684631 0.14153171676199228 -0.007252350207672734" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -666,15 +666,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.023893261066846305 0.14153171676199225 -0.007252350207672735" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.02389326106684631 0.14153171676199228 -0.007252350207672734" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069357 -0.053747792579130614 -6.132332688812785e-07"/>
-    <axis xyz="-0.9999998262417886 4.2105014356162707e-08 2.4630481189336084e-08"/>
+    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069352 -0.05374779257913059 -6.132332688804098e-07"/>
+    <axis xyz="-0.9999998262417886 4.210501434612913e-08 2.4630481182686846e-08"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.792526803190927" velocity="120.0"/>
@@ -682,12 +682,12 @@
   </joint>
   <link name="r_shoulder_3">
     <inertial>
-      <origin xyz="-8.928496484206933e-05 -3.376971940446949e-05 -0.00759450025288725" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-8.928496484206933e-05 -3.376971940446949e-05 -0.007594500252887257" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.258699"/>
       <inertia ixx="7.20362e-05" ixy="5.87638e-07" ixz="-2.8754e-06" iyy="7.22746e-05" iyz="-3.42816e-06" izz="0.000119776"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743207452236184 0.1415314977120317 0.04704763137021825" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743207452236184 0.1415314977120317 0.047047631370218244" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -696,26 +696,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743207452236184 0.1415314977120317 0.04704763137021825" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743207452236184 0.1415314977120317 0.047047631370218244" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_arm_ft_sensor" type="fixed">
-    <origin xyz="-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099" rpy="0 0 0"/>
+    <origin xyz="-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099" rpy="0 0 0"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_upper_arm">
     <inertial>
-      <origin xyz="-4.8274954764261635e-06 -0.001943485876269635 -0.04747357932833829" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-4.827495476422694e-06 -0.001943485876269635 -0.04747357932833829" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="2.06334"/>
       <inertia ixx="0.00382639" ixy="-5.03894e-06" ixz="0.000205684" iyy="0.00368146" iyz="0.000261155" izz="0.00167002"/>
     </inertial>
     <visual>
-      <origin xyz="0.03074312554381853 0.141531075879301 0.06954757274337192" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743125543818535 0.141531075879301 0.06954757274337192" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -724,15 +724,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03074312554381853 0.141531075879301 0.06954757274337192" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743125543818535 0.141531075879301 0.06954757274337192" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="0 4.2183273077367467e-07 -0.022499941373153573" rpy="0 0 0"/>
-    <axis xyz="-3.787621790513063e-08 2.805197706068352e-08 1.0000002566182309"/>
+    <origin xyz="0 4.2183273077367467e-07 -0.02249994137315358" rpy="0 0 0"/>
+    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
     <parent link="r_shoulder_3"/>
     <child link="r_upper_arm"/>
     <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
@@ -740,12 +740,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.011624818009452066 -0.00180281594701992 0.022300137248866647" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-0.011624818009452066 -0.0018028159470199479 0.022300137248866647" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.397478"/>
       <inertia ixx="0.00139406" ixy="5.80993e-06" ixz="5.27143e-05" iyy="0.00139077" iyz="6.49755e-05" izz="0.000397127"/>
     </inertial>
     <visual>
-      <origin xyz="0.020743193621214625 0.14003143574727356 0.19324791023291843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.020743193621214625 0.14003143574727353 0.19324791023291843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -754,15 +754,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.020743193621214625 0.14003143574727356 0.19324791023291843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.020743193621214625 0.14003143574727353 0.19324791023291843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_elbow" type="revolute">
-    <origin xyz="0.009999931922603816 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
-    <axis xyz="2.4949582809984105e-07 -1.0000002733514362 2.7942487004772865e-08"/>
+    <origin xyz="0.00999993192260382 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
+    <axis xyz="2.4949582807394525e-07 -1.0000002733514362 2.7942487010105877e-08"/>
     <parent link="r_upper_arm"/>
     <child link="r_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -791,8 +791,8 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.014999869468812953 -0.001999649905318951 -0.05530006367251966" rpy="0 0 0"/>
-    <axis xyz="-3.787621790513063e-08 2.805197706068352e-08 1.0000002566182309"/>
+    <origin xyz="-0.014999869468812953 -0.0019996499053189787 -0.05530006367251966" rpy="0 0 0"/>
+    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -800,12 +800,12 @@
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="-0.014389827079018234 0.0005092753390522065 -9.787315764231419e-05" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-0.014389827079018234 0.0005092753390521787 -9.78731576423697e-05" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.0344005"/>
       <inertia ixx="5.11755e-06" ixy="9.19776e-07" ixz="1.47559e-08" iyy="1.60849e-06" iyz="-1.11493e-07" izz="5.58546e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.01944310973607908 0.1440385285434045 0.3560480536175445" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.01944310973607908 0.14403852854340446 0.35604805361754444" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -814,7 +814,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.01944310973607908 0.1440385285434045 0.3560480536175445" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.01944310973607908 0.14403852854340446 0.35604805361754444" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -822,7 +822,7 @@
   </link>
   <joint name="r_wrist_pitch" type="revolute">
     <origin xyz="0.016299953353948633 -0.002007442890811839 -0.10750007971210637" rpy="0 0 0"/>
-    <axis xyz="0.9999998262417886 -4.2105014356162707e-08 -2.4630481189336084e-08"/>
+    <axis xyz="0.9999998262417886 -4.210501434612913e-08 -2.4630481182686846e-08"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -830,12 +830,12 @@
   </joint>
   <link name="r_hand">
     <inertial>
-      <origin xyz="0.004828314385249662 0.00615667237451184 -0.0671378028889843" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
+      <origin xyz="0.004828314385249669 0.00615667237451184 -0.0671378028889843" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
       <mass value="0.264229"/>
       <inertia ixx="0.000428764" ixy="4.7873e-05" ixz="1.1114e-05" iyy="0.000348073" iyz="1.14469e-05" izz="0.00016164"/>
     </inertial>
     <visual>
-      <origin xyz="0.035743235789782016 0.1370391384923153 0.35604813259525964" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
+      <origin xyz="0.03574323578978202 0.1370391384923153 0.35604813259525964" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -844,15 +844,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.035743235789782016 0.1370391384923153 0.35604813259525964" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
+      <origin xyz="0.03574323578978202 0.1370391384923153 0.35604813259525964" rpy="-0.07156446894052497 0.061969920956429156 -1.8462350407754615"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="continuous">
-    <origin xyz="-0.016300122874272734 0.0069993900510918194 0" rpy="0 0 0"/>
-    <axis xyz="-2.4949582809984105e-07 1.0000002733514362 -2.7942487004772865e-08"/>
+    <origin xyz="-0.016300122874272734 0.006999390051091792 0" rpy="0 0 0"/>
+    <axis xyz="-2.4949582807394525e-07 1.0000002733514362 -2.7942487010105877e-08"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
     <dynamics damping="0.1"/>