diff --git a/iCub/robots/iCubDarmstadt01/model.urdf b/iCub/robots/iCubDarmstadt01/model.urdf
index 9630b0a..914c582 100644
--- a/iCub/robots/iCubDarmstadt01/model.urdf
+++ b/iCub/robots/iCubDarmstadt01/model.urdf
@@ -220,7 +220,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -255,7 +255,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -283,7 +283,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -311,21 +311,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -360,14 +360,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -381,21 +381,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -416,14 +416,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -444,7 +444,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -458,7 +458,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1022,12 +1022,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -1036,15 +1036,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1073,7 +1073,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1184,7 +1184,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1198,14 +1198,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1226,14 +1226,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1261,7 +1261,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1289,21 +1289,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1345,7 +1345,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -1449,12 +1449,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -1463,15 +1463,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1500,7 +1500,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1611,7 +1611,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1625,7 +1625,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1674,7 +1674,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1716,21 +1716,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1751,7 +1751,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -2036,7 +2036,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2810,13 +2810,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2858,13 +2858,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2966,13 +2966,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -3156,24 +3156,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3215,13 +3215,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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3297,13 +3297,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3381,24 +3381,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3428,25 +3428,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubErzelli02/model.urdf b/iCub/robots/iCubErzelli02/model.urdf
index 1ad6b57..e672384 100644
--- a/iCub/robots/iCubErzelli02/model.urdf
+++ b/iCub/robots/iCubErzelli02/model.urdf
@@ -220,7 +220,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -255,7 +255,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -283,7 +283,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -311,21 +311,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -360,14 +360,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -381,21 +381,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -416,14 +416,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -444,7 +444,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -458,7 +458,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1022,12 +1022,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -1036,15 +1036,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1073,7 +1073,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1184,7 +1184,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1198,14 +1198,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1226,14 +1226,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1261,7 +1261,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1289,21 +1289,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1345,7 +1345,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -1449,12 +1449,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -1463,15 +1463,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1500,7 +1500,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1611,7 +1611,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1625,7 +1625,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1674,7 +1674,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1716,21 +1716,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1751,7 +1751,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -2036,7 +2036,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2810,13 +2810,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2858,13 +2858,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2966,13 +2966,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -3156,24 +3156,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3215,13 +3215,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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3297,13 +3297,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3381,24 +3381,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3428,25 +3428,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGazeboV2_5/model.urdf b/iCub/robots/iCubGazeboV2_5/model.urdf
index 518414d..b56b797 100644
--- a/iCub/robots/iCubGazeboV2_5/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5/model.urdf
@@ -220,7 +220,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -255,7 +255,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -283,7 +283,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -311,21 +311,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -360,14 +360,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -381,21 +381,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -416,14 +416,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -444,7 +444,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -458,7 +458,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1022,12 +1022,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -1036,15 +1036,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1073,7 +1073,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1184,7 +1184,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1198,14 +1198,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1226,14 +1226,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1261,7 +1261,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1289,21 +1289,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1345,7 +1345,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -1449,12 +1449,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -1463,15 +1463,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1500,7 +1500,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1611,7 +1611,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1625,7 +1625,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1674,7 +1674,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1716,21 +1716,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1751,7 +1751,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -2036,7 +2036,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2810,13 +2810,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2858,13 +2858,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2966,13 +2966,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -3156,24 +3156,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3215,13 +3215,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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3297,13 +3297,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3381,24 +3381,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3428,25 +3428,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGazeboV2_5_plus/model.urdf b/iCub/robots/iCubGazeboV2_5_plus/model.urdf
index 0f071a3..8d7b7f9 100644
--- a/iCub/robots/iCubGazeboV2_5_plus/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5_plus/model.urdf
@@ -282,12 +282,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -296,15 +296,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -333,7 +333,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -444,7 +444,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -458,14 +458,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -486,14 +486,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -521,7 +521,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -549,21 +549,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -584,7 +584,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -605,7 +605,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -709,12 +709,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -723,15 +723,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -760,7 +760,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -871,7 +871,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -913,7 +913,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -934,7 +934,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -976,21 +976,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1011,7 +1011,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1289,7 +1289,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.044503292 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.044503292 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -1310,7 +1310,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1352,7 +1352,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1380,21 +1380,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1429,14 +1429,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1450,21 +1450,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1485,14 +1485,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1513,7 +1513,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1527,7 +1527,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1622,12 +1622,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
       <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.832595507822573 0 -1.5707963267948983"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
       <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>
@@ -1847,7 +1847,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2439,13 +2439,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2487,13 +2487,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2595,13 +2595,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2785,24 +2785,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2820,13 +2820,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.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</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.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2844,37 +2844,37 @@
     <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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</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.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" 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.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</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.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
@@ -2926,13 +2926,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3010,24 +3010,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3057,25 +3057,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" 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/iCubGazeboV2_6/model.urdf b/iCub/robots/iCubGazeboV2_6/model.urdf
index 518414d..b56b797 100644
--- a/iCub/robots/iCubGazeboV2_6/model.urdf
+++ b/iCub/robots/iCubGazeboV2_6/model.urdf
@@ -220,7 +220,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -255,7 +255,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -283,7 +283,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -311,21 +311,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -360,14 +360,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -381,21 +381,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -416,14 +416,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -444,7 +444,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -458,7 +458,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1022,12 +1022,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -1036,15 +1036,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1073,7 +1073,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1184,7 +1184,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1198,14 +1198,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1226,14 +1226,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1261,7 +1261,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1289,21 +1289,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1345,7 +1345,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -1449,12 +1449,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -1463,15 +1463,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1500,7 +1500,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1611,7 +1611,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1625,7 +1625,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1674,7 +1674,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1716,21 +1716,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1751,7 +1751,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -2036,7 +2036,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2810,13 +2810,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2858,13 +2858,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2966,13 +2966,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -3156,24 +3156,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3215,13 +3215,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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3297,13 +3297,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3381,24 +3381,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3428,25 +3428,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGazeboV2_7/model.urdf b/iCub/robots/iCubGazeboV2_7/model.urdf
index 07b2690..c872eb9 100644
--- a/iCub/robots/iCubGazeboV2_7/model.urdf
+++ b/iCub/robots/iCubGazeboV2_7/model.urdf
@@ -220,7 +220,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -255,7 +255,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -283,7 +283,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -311,21 +311,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -360,14 +360,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -381,21 +381,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -416,14 +416,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -444,7 +444,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -458,7 +458,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1022,12 +1022,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -1036,15 +1036,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1073,7 +1073,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1184,7 +1184,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1198,14 +1198,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1226,14 +1226,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1261,7 +1261,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1289,21 +1289,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1345,7 +1345,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -1449,12 +1449,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -1463,15 +1463,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1500,7 +1500,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1611,7 +1611,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1625,7 +1625,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1674,7 +1674,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1716,21 +1716,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1751,7 +1751,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -2036,7 +2036,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2826,13 +2826,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2874,13 +2874,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2982,13 +2982,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -3172,24 +3172,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3231,13 +3231,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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3313,13 +3313,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3397,24 +3397,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3444,25 +3444,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999977 -0.041718700000000025 -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 f321b74..3e83ff4 100644
--- a/iCub/robots/iCubGazeboV3/model.urdf
+++ b/iCub/robots/iCubGazeboV3/model.urdf
@@ -252,12 +252,12 @@
   </joint>
   <link name="r_foot_front">
     <inertial>
-      <origin xyz="-0.0004180000000000017 0.0004849999999999993 0.005581000000000058" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.0004180000000000017 0.0004849999999999993 -0.01671899999999993" rpy="0 0 -1.5707963267948966"/>
       <mass value="0.17964"/>
       <inertia ixx="0.01" ixy="-1.1622018441861841e-05" ixz="2.9686391650024495e-05" iyy="0.01" iyz="7.056362242091155e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10850000000000012 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.10850000000000012 0.0648499999999391 0.9241470000000105" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -273,19 +273,19 @@
     </collision>
   </link>
   <joint name="r_foot_front_ft_sensor" type="fixed">
-    <origin xyz="0.13525 0 -0.06579999999999997" rpy="0 0 0"/>
+    <origin xyz="0.13525 0 -0.04349999999999998" rpy="0 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_front"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_foot_rear">
     <inertial>
-      <origin xyz="0.0006180999999999999 7.079999999998199e-05 0.003514000000000017" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.0006180999999999999 7.079999999998199e-05 -0.01878599999999997" rpy="0 0 -1.5707963267948966"/>
       <mass value="0.17874"/>
       <inertia ixx="0.01" ixy="1.5972601165325875e-07" ixz="2.851962872955676e-06" iyy="0.01" iyz="-1.2898142814635283e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.010749999999999876 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.010749999999999876 0.0648499999999391 0.9241470000000105" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_rear_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -301,14 +301,14 @@
     </collision>
   </link>
   <joint name="r_foot_rear_ft_sensor" type="fixed">
-    <origin xyz="0.016 0 -0.06579999999999997" rpy="0 0 0"/>
+    <origin xyz="0.016 0 -0.04349999999999998" rpy="0 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_rear"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.05963000000000001 0 0" rpy="0 0 0"/>
+    <origin xyz="0.05963000000000001 0 -0.022299999999999986" rpy="0 0 0"/>
     <parent link="r_foot_rear"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
@@ -595,7 +595,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069352 -0.05374779257913059 -6.132332688804098e-07"/>
+    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069357 -0.053747792579130614 -6.132332688812785e-07"/>
     <axis xyz="-0.9999998262417886 4.210501434612913e-08 2.4630481182686846e-08"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
@@ -819,12 +819,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.007668668705576271 0.00018849980855933923 -0.020403454228093053" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.007668668705576271 0.000188499808559367 -0.020403454228093053" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.42492"/>
       <inertia ixx="0.01" ixy="-4.916851307375464e-05" ixz="-6.324300296292925e-05" iyy="0.01" iyz="-7.587195452290467e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.026393096265395596 -0.14064030580326706 0.017434480978924874" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.026393096265395627 -0.14064030580326703 0.01743448097892487" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -833,15 +833,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.026393096265395596 -0.14064030580326706 0.017434480978924874" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.026393096265395627 -0.14064030580326703 0.01743448097892487" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.004992849331690818 0.111499838456099 0.00026865022773668243" rpy="-0.17453296140985003 -0.05374759931933849 0"/>
-    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
+    <origin xyz="0.004992849331690818 0.111499838456099 0.00026865022773668243" rpy="-0.17453296140985 -0.05374759931933852 0"/>
+    <axis xyz="0.9999998262417684 2.0348252651106495e-07 -4.0230228446743775e-08"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.8448866807507573" velocity="120.0"/>
@@ -849,19 +849,19 @@
   </joint>
   <link name="l_arm_ft"/>
   <joint name="l_arm_ft_fixed_joint" type="fixed">
-    <origin xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="3.141592646862139 0 -1.5707962571980942"/>
+    <origin xyz="-0.004350133156978325 3.632573233880443e-07 -0.054300038724061715" rpy="3.141592646862139 0 -1.5707962571980942"/>
     <parent link="l_shoulder_2"/>
     <child link="l_arm_ft"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_shoulder_3">
     <inertial>
-      <origin xyz="-9.561931744893337e-05 3.089115052354319e-05 -0.007966608685190321" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="-9.561931744893337e-05 3.08911505235987e-05 -0.007966608685190321" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.2625"/>
       <inertia ixx="0.01" ixy="-1.6986505676585652e-06" ixz="1.0777869051741286e-05" iyy="0.01" iyz="-5.511652932595611e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743229422373917 -0.14064066906059042 0.0717345197029866" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.030743229422373952 -0.1406406690605904 0.07173451970298658" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -870,26 +870,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743229422373917 -0.14064066906059042 0.0717345197029866" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.030743229422373952 -0.1406406690605904 0.07173451970298658" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="0 0 0"/>
+    <origin xyz="-0.004350133156978325 3.632573233880443e-07 -0.054300038724061715" 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.0010476375729993916 0.0025596225822598698 -0.05065314985074498" rpy="-0.06212860830092193 0.0714268908124578 0.27099931149673545"/>
+      <origin xyz="0.0010476375729993916 0.002559622582259849 -0.05065314985074498" rpy="-0.062128608300921935 0.07142689081245782 0.27099931149673573"/>
       <mass value="1.67729"/>
       <inertia ixx="0.002960829234557446" ixy="-1.0441222117313817e-05" ixz="0.0002229885938828402" iyy="0.0030756376591531418" iyz="0.00020156448677983343" izz="0.001327263106289413"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743147864881363 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.030743147864881398 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -898,15 +898,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743147864881363 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.030743147864881398 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.022499941373124423" rpy="0 0 0"/>
-    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
+    <origin xyz="0 -4.234524844659582e-07 -0.02249994137312443" rpy="0 0 0"/>
+    <axis xyz="2.2276474042070207e-08 -4.393786245920471e-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"/>
@@ -914,12 +914,12 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="-0.009947433863160948 0.0004713243585720006 -0.014889454628597226" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="-0.009947433863160951 0.00047132435857197286 -0.014889454628597226" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.22161"/>
       <inertia ixx="0.01" ixy="-1.4473906751261063e-06" ixz="-2.4098487110118167e-05" iyy="0.01" iyz="-5.285154621856073e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.020743217629961962 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.020743217629961994 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -928,7 +928,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.020743217629961962 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.020743217629961994 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -936,7 +936,7 @@
   </link>
   <joint name="l_elbow" type="revolute">
     <origin xyz="0.009999930234919404 -0.001499647423428896 -0.12370033753758375" rpy="0 0 0"/>
-    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
+    <axis xyz="-8.811824153288392e-08 -1.0000002733514628 4.404736119110563e-08"/>
     <parent link="l_upper_arm"/>
     <child link="l_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -944,12 +944,12 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="-0.006323186767737077 -4.644477335560837e-05 -0.030456750179120307" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="-0.006323186767737084 -4.644477335560837e-05 -0.030456750179120307" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.87311"/>
       <inertia ixx="0.0012499578358808174" ixy="2.299990880778632e-05" ixz="-0.00013204802784752087" iyy="0.0012325327102048142" iyz="0.00013098820888298704" izz="0.0006619865197576301"/>
     </inertial>
     <visual>
-      <origin xyz="0.03574308828413966 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.0357430882841397 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -958,15 +958,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03574308828413966 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.0357430882841397 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.0149998706541777 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
-    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
+    <origin xyz="-0.014999870654177704 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
+    <axis xyz="2.2276474042070207e-08 -4.393786245920471e-08 -1.0000002566182309"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -974,12 +974,12 @@
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.014340376682819098 -0.0005016202223036004 -0.00011512600676955032" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="-0.014340376682819091 -0.0005016202223036004 -0.00011512600676955032" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.033183"/>
       <inertia ixx="0.01" ixy="-8.77585378818565e-07" ixz="1.8326321501111727e-08" iyy="0.01" iyz="-1.496169319469983e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.01914307234985348 -0.14314713690638445 0.38073450333871567" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.019143072349853515 -0.14314713690638442 0.38073450333871567" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -988,15 +988,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.01914307234985348 -0.14314713690638445 0.38073450333871567" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.019143072349853515 -0.14314713690638442 0.38073450333871567" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.0020068952180725685 -0.10749964114254112" rpy="0 0 0"/>
-    <axis xyz="-0.9999998262417685 -2.0348252625938444e-07 4.023022846062156e-08"/>
+    <origin xyz="0.016600015934286182 0.002006895218072541 -0.10749964114254112" rpy="0 0 0"/>
+    <axis xyz="-0.9999998262417684 -2.0348252651106495e-07 4.0230228446743775e-08"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -1004,12 +1004,12 @@
   </joint>
   <link name="l_hand">
     <inertial>
-      <origin xyz="0.004730263158814873 -0.006201146485818793 -0.06710705725234695" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.004730263158814873 -0.00620114648581882 -0.06710705725234695" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.25639"/>
       <inertia ixx="0.01" ixy="-4.053526663925987e-05" ixz="-5.426688459725203e-05" iyy="0.01" iyz="2.727389831071015e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.03544319409458287 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.035443194094582894 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1018,15 +1018,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03544319409458287 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.035443194094582894 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.006999392681571209 0" rpy="0 0 0"/>
-    <axis xyz="8.811824174789715e-08 1.0000002733514628 -4.404736120498342e-08"/>
+    <origin xyz="-0.016300121744729387 -0.0069993926815711816 0" rpy="0 0 0"/>
+    <axis xyz="8.811824153288392e-08 1.0000002733514628 -4.404736119110563e-08"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
     <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
@@ -1263,12 +1263,12 @@
   </joint>
   <link name="l_foot_rear">
     <inertial>
-      <origin xyz="0.0005056000000000001 3.350000000000575e-05 0.003658999999999968" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.0005056000000000001 3.350000000000575e-05 -0.01864100000000002" rpy="0 0 -1.5707963267948966"/>
       <mass value="0.17851"/>
       <inertia ixx="0.01" ixy="-3.8140209422126887e-07" ixz="3.3779281136245455e-06" iyy="0.01" iyz="8.141588837428499e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.010699999999999342 -0.06475000000000009 0.9464470000000004" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.010699999999999342 -0.06475000000000009 0.9241470000000004" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_foot_rear_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1284,26 +1284,26 @@
     </collision>
   </link>
   <joint name="l_foot_rear_ft_sensor" type="fixed">
-    <origin xyz="0.016 0 -0.06579999999999997" rpy="0 0 0"/>
+    <origin xyz="0.016 0 -0.04349999999999998" rpy="0 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot_rear"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_sole"/>
   <joint name="l_sole_fixed_joint" type="fixed">
-    <origin xyz="0.05963 0 0" rpy="0 0 0"/>
+    <origin xyz="0.05963 0 -0.022299999999999986" rpy="0 0 0"/>
     <parent link="l_foot_rear"/>
     <child link="l_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_foot_front">
     <inertial>
-      <origin xyz="7.700000000000762e-05 -3.990000000000937e-05 0.004256999999999955" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="7.700000000000762e-05 -3.990000000000937e-05 -0.01804300000000003" rpy="0 0 -1.5707963267948966"/>
       <mass value="0.17896"/>
       <inertia ixx="0.01" ixy="3.5467003209527003e-07" ixz="2.7144049104682455e-07" iyy="0.01" iyz="3.4960507170104935e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10855000000000063 -0.06475000000000009 0.9464470000000004" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.10855000000000063 -0.06475000000000009 0.9241470000000004" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1319,7 +1319,7 @@
     </collision>
   </link>
   <joint name="l_foot_front_ft_sensor" type="fixed">
-    <origin xyz="0.13524999999999998 0 -0.06579999999999997" rpy="0 0 0"/>
+    <origin xyz="0.13524999999999998 0 -0.04349999999999998" rpy="0 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot_front"/>
     <dynamics damping="0.1"/>
@@ -1586,7 +1586,7 @@
         <frame>sensor</frame>
         <measure_direction>child_to_parent</measure_direction>
       </force_torque>
-      <pose>3.469446951953614e-18 0.0 0.0 3.141592646862139 -3.10468332187885e-08 -1.5707962571980942</pose>
+      <pose>0.0 2.7755575615628914e-17 0.0 3.141592646862139 -3.104683320142575e-08 -1.5707962571980942</pose>
       <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1598,7 +1598,7 @@
       <frame>sensor</frame>
       <measure_direction>child_to_parent</measure_direction>
     </force_torque>
-    <origin rpy="3.141592646862139 -3.10468332187885e-08 -1.5707962571980942" xyz="3.469446951953614e-18 0.0 0.0"/>
+    <origin rpy="3.141592646862139 -3.104683320142575e-08 -1.5707962571980942" xyz="0.0 2.7755575615628914e-17 0.0"/>
   </sensor>
   <gazebo reference="r_arm_ft_sensor">
     <sensor name="r_arm_ft" type="force_torque">
@@ -1652,7 +1652,7 @@
         <frame>sensor</frame>
         <measure_direction>parent_to_child</measure_direction>
       </force_torque>
-      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
+      <pose>0.0 0.0 0.0 0.0 -0.0 -2.094395210586932</pose>
       <plugin name="left_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_front_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1664,7 +1664,7 @@
       <frame>sensor</frame>
       <measure_direction>parent_to_child</measure_direction>
     </force_torque>
-    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
+    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.0"/>
   </sensor>
   <gazebo reference="l_foot_rear_ft_sensor">
     <sensor name="l_foot_rear_ft" type="force_torque">
@@ -1674,7 +1674,7 @@
         <frame>sensor</frame>
         <measure_direction>parent_to_child</measure_direction>
       </force_torque>
-      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
+      <pose>0.0 0.0 0.0 0.0 -0.0 -2.094395210586932</pose>
       <plugin name="left_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_rear_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1686,7 +1686,7 @@
       <frame>sensor</frame>
       <measure_direction>parent_to_child</measure_direction>
     </force_torque>
-    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
+    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.0"/>
   </sensor>
   <gazebo reference="r_leg_ft_sensor">
     <sensor name="r_leg_ft" type="force_torque">
@@ -1718,7 +1718,7 @@
         <frame>sensor</frame>
         <measure_direction>parent_to_child</measure_direction>
       </force_torque>
-      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
+      <pose>0.0 0.0 0.0 0.0 -0.0 -2.094395210586932</pose>
       <plugin name="right_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_foot_front_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1730,7 +1730,7 @@
       <frame>sensor</frame>
       <measure_direction>parent_to_child</measure_direction>
     </force_torque>
-    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
+    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.0"/>
   </sensor>
   <gazebo reference="r_foot_rear_ft_sensor">
     <sensor name="r_foot_rear_ft" type="force_torque">
@@ -1740,7 +1740,7 @@
         <frame>sensor</frame>
         <measure_direction>parent_to_child</measure_direction>
       </force_torque>
-      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
+      <pose>0.0 0.0 0.0 0.0 -0.0 -2.094395210586932</pose>
       <plugin name="right_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_foot_rear_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1752,7 +1752,7 @@
       <frame>sensor</frame>
       <measure_direction>parent_to_child</measure_direction>
     </force_torque>
-    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
+    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.0"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="head_imu_0" type="imu">
diff --git a/iCub/robots/iCubGenova01/model.urdf b/iCub/robots/iCubGenova01/model.urdf
index 9630b0a..914c582 100644
--- a/iCub/robots/iCubGenova01/model.urdf
+++ b/iCub/robots/iCubGenova01/model.urdf
@@ -220,7 +220,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -255,7 +255,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -283,7 +283,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -311,21 +311,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -360,14 +360,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -381,21 +381,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -416,14 +416,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -444,7 +444,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -458,7 +458,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1022,12 +1022,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -1036,15 +1036,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1073,7 +1073,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1184,7 +1184,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1198,14 +1198,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1226,14 +1226,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1261,7 +1261,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1289,21 +1289,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1345,7 +1345,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -1449,12 +1449,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -1463,15 +1463,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1500,7 +1500,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1611,7 +1611,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1625,7 +1625,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1674,7 +1674,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1716,21 +1716,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1751,7 +1751,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -2036,7 +2036,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2810,13 +2810,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2858,13 +2858,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2966,13 +2966,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -3156,24 +3156,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3215,13 +3215,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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3297,13 +3297,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3381,24 +3381,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3428,25 +3428,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGenova02/model.urdf b/iCub/robots/iCubGenova02/model.urdf
index ab815a7..6f8ffdc 100644
--- a/iCub/robots/iCubGenova02/model.urdf
+++ b/iCub/robots/iCubGenova02/model.urdf
@@ -282,12 +282,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -296,15 +296,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -333,7 +333,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -444,7 +444,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -458,14 +458,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -486,14 +486,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -521,7 +521,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -549,21 +549,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -584,7 +584,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -605,7 +605,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -709,12 +709,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -723,15 +723,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -760,7 +760,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -871,7 +871,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -913,7 +913,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -934,7 +934,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -976,21 +976,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1011,7 +1011,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1289,7 +1289,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.044503292 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.044503292 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -1310,7 +1310,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1352,7 +1352,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1380,21 +1380,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1429,14 +1429,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1450,21 +1450,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1485,14 +1485,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1513,7 +1513,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1527,7 +1527,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1622,12 +1622,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
       <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.832595507822573 0 -1.5707963267948983"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
       <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>
@@ -1636,7 +1636,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
       <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>
@@ -1847,7 +1847,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2439,13 +2439,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2487,13 +2487,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2595,13 +2595,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2785,24 +2785,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2820,13 +2820,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.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</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.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2844,37 +2844,37 @@
     <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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</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.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" 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.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</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.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
@@ -2926,13 +2926,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3010,24 +3010,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3057,25 +3057,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" 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 1ad6b57..e672384 100644
--- a/iCub/robots/iCubGenova04/model.urdf
+++ b/iCub/robots/iCubGenova04/model.urdf
@@ -220,7 +220,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -255,7 +255,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -283,7 +283,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -311,21 +311,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -360,14 +360,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -381,21 +381,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -416,14 +416,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -444,7 +444,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -458,7 +458,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1022,12 +1022,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -1036,15 +1036,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1073,7 +1073,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1184,7 +1184,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1198,14 +1198,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1226,14 +1226,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1261,7 +1261,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1289,21 +1289,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1345,7 +1345,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -1449,12 +1449,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -1463,15 +1463,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1500,7 +1500,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1611,7 +1611,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -1625,7 +1625,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1674,7 +1674,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1716,21 +1716,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1751,7 +1751,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -2036,7 +2036,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2810,13 +2810,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2858,13 +2858,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2966,13 +2966,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -3156,24 +3156,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3215,13 +3215,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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3297,13 +3297,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3381,24 +3381,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3428,25 +3428,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGenova04_plus/model.urdf b/iCub/robots/iCubGenova04_plus/model.urdf
index ab815a7..6f8ffdc 100644
--- a/iCub/robots/iCubGenova04_plus/model.urdf
+++ b/iCub/robots/iCubGenova04_plus/model.urdf
@@ -282,12 +282,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
+      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" 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.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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>
@@ -296,15 +296,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471541783582e-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.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
-    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
+    <axis xyz="-0.258819 -2.0285226262510233e-17 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -333,7 +333,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
+    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -444,7 +444,7 @@
   </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.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -458,14 +458,14 @@
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.694872808587699 -0.26975772710448953 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.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -486,14 +486,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.725646408570657 -0.26399290072226783 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 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.2706050905123207 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -521,7 +521,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.2948305020697443 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -549,21 +549,21 @@
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 -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.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -584,7 +584,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.5642955277146757 -0.1369802721974407 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -605,7 +605,7 @@
   </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.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
@@ -709,12 +709,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
+      <origin xyz="0.021474595127247476 0.00026895789968721384 -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.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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>
@@ -723,15 +723,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.040743380770989907" 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.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
-    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
+    <axis xyz="-0.258819 2.0285226262510233e-17 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -760,7 +760,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
+    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.0054006165626487995" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -871,7 +871,7 @@
   </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.27444253376399047 0.04212287374727479"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
@@ -913,7 +913,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.2639929007222678 -0.014380110925326664"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -934,7 +934,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.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -976,21 +976,21 @@
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
     <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.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
     <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.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
@@ -1011,7 +1011,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.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1289,7 +1289,7 @@
   </joint>
   <link name="r_lower_leg_ems_gyro_eb9"/>
   <joint name="r_lower_leg_ems_gyro_eb9_fixed_joint" type="fixed">
-    <origin xyz="0.044503292 0.014343499999999974 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.044503292 0.014343499999999974 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_ems_gyro_eb9"/>
     <dynamics damping="0.1"/>
@@ -1310,7 +1310,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.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1324,7 +1324,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.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1352,7 +1352,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.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1380,21 +1380,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.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
     <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.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
     <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.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1429,14 +1429,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.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
     <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.515872652806505 -0.2074073356601009"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1450,21 +1450,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.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
     <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.40681343597041 -0.24823308941290148"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
     <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.141589702392633 -1.4352475041611767 0.006879436515037955"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1485,14 +1485,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.141589635185034 -1.5169055940579301 0.26264042505774415"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
     <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.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1513,7 +1513,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.141591640420276 -1.5031867442105085 0.010871726099408735"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1527,7 +1527,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.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1622,12 +1622,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
       <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.832595507822573 0 -1.5707963267948983"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
       <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>
@@ -1636,7 +1636,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
       <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>
@@ -1847,7 +1847,7 @@
   </joint>
   <link name="l_lower_leg_ems_gyro_eb7"/>
   <joint name="l_lower_leg_ems_gyro_eb7_fixed_joint" type="fixed">
-    <origin xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996" rpy="3.1415926535897922 -1.3526297834539558 0"/>
+    <origin xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996" rpy="-3.1415926535897905 -1.3526297834539558 0"/>
     <parent link="l_lower_leg"/>
     <child link="l_lower_leg_ems_gyro_eb7"/>
     <dynamics damping="0.1"/>
@@ -2439,13 +2439,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.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</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.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2487,13 +2487,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.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</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.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2595,13 +2595,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.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</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.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2785,24 +2785,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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2820,13 +2820,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.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</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.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2844,37 +2844,37 @@
     <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.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</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.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" 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.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</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.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
@@ -2926,13 +2926,13 @@
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</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.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
@@ -3010,24 +3010,24 @@
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</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.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" 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.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
@@ -3057,25 +3057,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</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.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" 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.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</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.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" 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 5c34832..4cff3ff 100644
--- a/iCub/robots/iCubGenova09/model.urdf
+++ b/iCub/robots/iCubGenova09/model.urdf
@@ -252,12 +252,12 @@
   </joint>
   <link name="r_foot_front">
     <inertial>
-      <origin xyz="-0.0004180000000000017 0.0004849999999999993 0.005581000000000058" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.0004180000000000017 0.0004849999999999993 -0.01671899999999993" rpy="0 0 -1.5707963267948966"/>
       <mass value="0.17964"/>
       <inertia ixx="0.0004475629748133475" ixy="-1.1622018441861841e-05" ixz="2.9686391650024495e-05" iyy="0.0003761336092973683" iyz="7.056362242091155e-05" izz="0.00040019707061844293"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10850000000000012 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.10850000000000012 0.0648499999999391 0.9241470000000105" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -266,26 +266,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10850000000000012 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.10850000000000012 0.0648499999999391 0.9241470000000105" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_foot_front_ft_sensor" type="fixed">
-    <origin xyz="0.13525 0 -0.06579999999999997" rpy="0 0 0"/>
+    <origin xyz="0.13525 0 -0.04349999999999998" rpy="0 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_front"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_foot_rear">
     <inertial>
-      <origin xyz="0.0006180999999999999 7.079999999998199e-05 0.003514000000000017" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.0006180999999999999 7.079999999998199e-05 -0.01878599999999997" rpy="0 0 -1.5707963267948966"/>
       <mass value="0.17874"/>
       <inertia ixx="0.00023837060598027492" ixy="1.5972601165325875e-07" ixz="2.851962872955676e-06" iyy="0.00018229756818021977" iyz="-1.2898142814635283e-06" izz="0.0003645245458529223"/>
     </inertial>
     <visual>
-      <origin xyz="0.010749999999999876 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.010749999999999876 0.0648499999999391 0.9241470000000105" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_rear_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -294,21 +294,21 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.010749999999999876 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.010749999999999876 0.0648499999999391 0.9241470000000105" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_rear_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_foot_rear_ft_sensor" type="fixed">
-    <origin xyz="0.016 0 -0.06579999999999997" rpy="0 0 0"/>
+    <origin xyz="0.016 0 -0.04349999999999998" rpy="0 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot_rear"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.05963000000000001 0 0" rpy="0 0 0"/>
+    <origin xyz="0.05963000000000001 0 -0.022299999999999986" rpy="0 0 0"/>
     <parent link="r_foot_rear"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
@@ -595,7 +595,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069352 -0.05374779257913059 -6.132332688804098e-07"/>
+    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069357 -0.053747792579130614 -6.132332688812785e-07"/>
     <axis xyz="-0.9999998262417886 4.210501434612913e-08 2.4630481182686846e-08"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
@@ -819,12 +819,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.007668668705576271 0.00018849980855933923 -0.020403454228093053" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.007668668705576271 0.000188499808559367 -0.020403454228093053" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.42492"/>
       <inertia ixx="0.00038388233784675605" ixy="-4.916851307375464e-05" ixz="-6.324300296292925e-05" iyy="0.00031068388949320714" iyz="-7.587195452290467e-05" izz="0.00026013597951892223"/>
     </inertial>
     <visual>
-      <origin xyz="0.026393096265395596 -0.14064030580326706 0.017434480978924874" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.026393096265395627 -0.14064030580326703 0.01743448097892487" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -833,15 +833,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.026393096265395596 -0.14064030580326706 0.017434480978924874" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.026393096265395627 -0.14064030580326703 0.01743448097892487" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.004992849331690818 0.111499838456099 0.00026865022773668243" rpy="-0.17453296140985003 -0.05374759931933849 0"/>
-    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
+    <origin xyz="0.004992849331690818 0.111499838456099 0.00026865022773668243" rpy="-0.17453296140985 -0.05374759931933852 0"/>
+    <axis xyz="0.9999998262417684 2.0348252651106495e-07 -4.0230228446743775e-08"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.8448866807507573" velocity="120.0"/>
@@ -849,19 +849,19 @@
   </joint>
   <link name="l_arm_ft"/>
   <joint name="l_arm_ft_fixed_joint" type="fixed">
-    <origin xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="3.141592646862139 0 -1.5707962571980942"/>
+    <origin xyz="-0.004350133156978325 3.632573233880443e-07 -0.054300038724061715" rpy="3.141592646862139 0 -1.5707962571980942"/>
     <parent link="l_shoulder_2"/>
     <child link="l_arm_ft"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_shoulder_3">
     <inertial>
-      <origin xyz="-9.561931744893337e-05 3.089115052354319e-05 -0.007966608685190321" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="-9.561931744893337e-05 3.08911505235987e-05 -0.007966608685190321" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.2625"/>
       <inertia ixx="7.626797646850803e-05" ixy="-1.6986505676585652e-06" ixz="1.0777869051741286e-05" iyy="7.457103744115285e-05" iyz="-5.511652932595611e-06" izz="0.00012000121562184585"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743229422373917 -0.14064066906059042 0.0717345197029866" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.030743229422373952 -0.1406406690605904 0.07173451970298658" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -870,26 +870,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743229422373917 -0.14064066906059042 0.0717345197029866" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.030743229422373952 -0.1406406690605904 0.07173451970298658" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="0 0 0"/>
+    <origin xyz="-0.004350133156978325 3.632573233880443e-07 -0.054300038724061715" 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.0010548304233867177 0.00246637035549363 -0.05059691565201817" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.0010548304233867212 0.0024663703554936023 -0.050596915652018154" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="1.6203"/>
       <inertia ixx="0.002863606589878253" ixy="6.96396353067558e-05" ixz="-0.0004505518970518024" iyy="0.002841423681069468" iyz="0.0002762768153497255" izz="0.00142003494270709"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743147864881363 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.030743147864881398 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -898,15 +898,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743147864881363 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.030743147864881398 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.022499941373124423" rpy="0 0 0"/>
-    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
+    <origin xyz="0 -4.234524844659582e-07 -0.02249994137312443" rpy="0 0 0"/>
+    <axis xyz="2.2276474042070207e-08 -4.393786245920471e-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"/>
@@ -914,12 +914,12 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="-0.009947433863160948 0.0004713243585720006 -0.014889454628597226" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="-0.009947433863160951 0.00047132435857197286 -0.014889454628597226" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.22161"/>
       <inertia ixx="0.00010487340390623636" ixy="-1.4473906751261063e-06" ixz="-2.4098487110118167e-05" iyy="0.00012419799085403103" iyz="-5.285154621856073e-06" izz="4.922634886024471e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.020743217629961962 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.020743217629961994 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -928,7 +928,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.020743217629961962 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.020743217629961994 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -936,7 +936,7 @@
   </link>
   <joint name="l_elbow" type="revolute">
     <origin xyz="0.009999930234919404 -0.001499647423428896 -0.12370033753758375" rpy="0 0 0"/>
-    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
+    <axis xyz="-8.811824153288392e-08 -1.0000002733514628 4.404736119110563e-08"/>
     <parent link="l_upper_arm"/>
     <child link="l_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -944,12 +944,12 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="-0.006323186767737077 -4.644477335560837e-05 -0.030456750179120307" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="-0.006323186767737084 -4.644477335560837e-05 -0.030456750179120307" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.87311"/>
       <inertia ixx="0.0012499578358808174" ixy="2.299990880778632e-05" ixz="-0.00013204802784752087" iyy="0.0012325327102048142" iyz="0.00013098820888298704" izz="0.0006619865197576301"/>
     </inertial>
     <visual>
-      <origin xyz="0.03574308828413966 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.0357430882841397 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -958,15 +958,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03574308828413966 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.0357430882841397 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.0149998706541777 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
-    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
+    <origin xyz="-0.014999870654177704 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
+    <axis xyz="2.2276474042070207e-08 -4.393786245920471e-08 -1.0000002566182309"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -974,12 +974,12 @@
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.014340376682819098 -0.0005016202223036004 -0.00011512600676955032" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="-0.014340376682819091 -0.0005016202223036004 -0.00011512600676955032" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.033183"/>
       <inertia ixx="4.9277807564423474e-06" ixy="-8.77585378818565e-07" ixz="1.8326321501111727e-08" iyy="1.5491988398963742e-06" iyz="-1.496169319469983e-07" izz="5.352345695144233e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.01914307234985348 -0.14314713690638445 0.38073450333871567" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.019143072349853515 -0.14314713690638442 0.38073450333871567" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -988,15 +988,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.01914307234985348 -0.14314713690638445 0.38073450333871567" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.019143072349853515 -0.14314713690638442 0.38073450333871567" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.0020068952180725685 -0.10749964114254112" rpy="0 0 0"/>
-    <axis xyz="-0.9999998262417685 -2.0348252625938444e-07 4.023022846062156e-08"/>
+    <origin xyz="0.016600015934286182 0.002006895218072541 -0.10749964114254112" rpy="0 0 0"/>
+    <axis xyz="-0.9999998262417684 -2.0348252651106495e-07 4.0230228446743775e-08"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -1004,12 +1004,12 @@
   </joint>
   <link name="l_hand">
     <inertial>
-      <origin xyz="0.004730263158814873 -0.006201146485818793 -0.06710705725234695" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.004730263158814873 -0.00620114648581882 -0.06710705725234695" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <mass value="0.25639"/>
       <inertia ixx="0.0004049513635009297" ixy="-4.053526663925987e-05" ixz="-5.426688459725203e-05" iyy="0.00033344402318011924" iyz="2.727389831071015e-05" izz="0.0001698381527187421"/>
     </inertial>
     <visual>
-      <origin xyz="0.03544319409458287 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.035443194094582894 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1018,15 +1018,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03544319409458287 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <origin xyz="0.035443194094582894 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729362 -0.22978873231718935 -1.2882780461440753"/>
       <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.006999392681571209 0" rpy="0 0 0"/>
-    <axis xyz="8.811824174789715e-08 1.0000002733514628 -4.404736120498342e-08"/>
+    <origin xyz="-0.016300121744729387 -0.0069993926815711816 0" rpy="0 0 0"/>
+    <axis xyz="8.811824153288392e-08 1.0000002733514628 -4.404736119110563e-08"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
     <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
@@ -1263,12 +1263,12 @@
   </joint>
   <link name="l_foot_rear">
     <inertial>
-      <origin xyz="0.0005056000000000001 3.350000000000575e-05 0.003658999999999968" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.0005056000000000001 3.350000000000575e-05 -0.01864100000000002" rpy="0 0 -1.5707963267948966"/>
       <mass value="0.17851"/>
       <inertia ixx="0.00028386984946481193" ixy="-3.8140209422126887e-07" ixz="3.3779281136245455e-06" iyy="0.00022675202580603227" iyz="8.141588837428499e-06" izz="0.00036516477016163983"/>
     </inertial>
     <visual>
-      <origin xyz="0.010699999999999342 -0.06475000000000009 0.9464470000000004" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.010699999999999342 -0.06475000000000009 0.9241470000000004" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_foot_rear_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1277,33 +1277,33 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.010699999999999342 -0.06475000000000009 0.9464470000000004" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0.010699999999999342 -0.06475000000000009 0.9241470000000004" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_foot_rear_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_foot_rear_ft_sensor" type="fixed">
-    <origin xyz="0.016 0 -0.06579999999999997" rpy="0 0 0"/>
+    <origin xyz="0.016 0 -0.04349999999999998" rpy="0 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot_rear"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_sole"/>
   <joint name="l_sole_fixed_joint" type="fixed">
-    <origin xyz="0.05963 0 0" rpy="0 0 0"/>
+    <origin xyz="0.05963 0 -0.022299999999999986" rpy="0 0 0"/>
     <parent link="l_foot_rear"/>
     <child link="l_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_foot_front">
     <inertial>
-      <origin xyz="7.700000000000762e-05 -3.990000000000937e-05 0.004256999999999955" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="7.700000000000762e-05 -3.990000000000937e-05 -0.01804300000000003" rpy="0 0 -1.5707963267948966"/>
       <mass value="0.17896"/>
       <inertia ixx="0.00032486721722121964" ixy="3.5467003209527003e-07" ixz="2.7144049104682455e-07" iyy="0.00025600096782908466" iyz="3.4960507170104935e-05" izz="0.00037767895393194076"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10855000000000063 -0.06475000000000009 0.9464470000000004" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.10855000000000063 -0.06475000000000009 0.9241470000000004" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1312,14 +1312,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10855000000000063 -0.06475000000000009 0.9464470000000004" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.10855000000000063 -0.06475000000000009 0.9241470000000004" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_foot_front_ft_sensor" type="fixed">
-    <origin xyz="0.13524999999999998 0 -0.06579999999999997" rpy="0 0 0"/>
+    <origin xyz="0.13524999999999998 0 -0.04349999999999998" rpy="0 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot_front"/>
     <dynamics damping="0.1"/>
@@ -1586,7 +1586,7 @@
         <frame>sensor</frame>
         <measure_direction>child_to_parent</measure_direction>
       </force_torque>
-      <pose>3.469446951953614e-18 0.0 0.0 3.141592646862139 -3.10468332187885e-08 -1.5707962571980942</pose>
+      <pose>0.0 2.7755575615628914e-17 0.0 3.141592646862139 -3.104683320142575e-08 -1.5707962571980942</pose>
       <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1598,7 +1598,7 @@
       <frame>sensor</frame>
       <measure_direction>child_to_parent</measure_direction>
     </force_torque>
-    <origin rpy="3.141592646862139 -3.10468332187885e-08 -1.5707962571980942" xyz="3.469446951953614e-18 0.0 0.0"/>
+    <origin rpy="3.141592646862139 -3.104683320142575e-08 -1.5707962571980942" xyz="0.0 2.7755575615628914e-17 0.0"/>
   </sensor>
   <gazebo reference="r_arm_ft_sensor">
     <sensor name="r_arm_ft" type="force_torque">
@@ -1652,7 +1652,7 @@
         <frame>sensor</frame>
         <measure_direction>parent_to_child</measure_direction>
       </force_torque>
-      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
+      <pose>0.0 0.0 0.0 0.0 -0.0 -2.094395210586932</pose>
       <plugin name="left_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_front_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1664,7 +1664,7 @@
       <frame>sensor</frame>
       <measure_direction>parent_to_child</measure_direction>
     </force_torque>
-    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
+    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.0"/>
   </sensor>
   <gazebo reference="l_foot_rear_ft_sensor">
     <sensor name="l_foot_rear_ft" type="force_torque">
@@ -1674,7 +1674,7 @@
         <frame>sensor</frame>
         <measure_direction>parent_to_child</measure_direction>
       </force_torque>
-      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
+      <pose>0.0 0.0 0.0 0.0 -0.0 -2.094395210586932</pose>
       <plugin name="left_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_rear_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1686,7 +1686,7 @@
       <frame>sensor</frame>
       <measure_direction>parent_to_child</measure_direction>
     </force_torque>
-    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
+    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.0"/>
   </sensor>
   <gazebo reference="r_leg_ft_sensor">
     <sensor name="r_leg_ft" type="force_torque">
@@ -1718,7 +1718,7 @@
         <frame>sensor</frame>
         <measure_direction>parent_to_child</measure_direction>
       </force_torque>
-      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
+      <pose>0.0 0.0 0.0 0.0 -0.0 -2.094395210586932</pose>
       <plugin name="right_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_foot_front_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1730,7 +1730,7 @@
       <frame>sensor</frame>
       <measure_direction>parent_to_child</measure_direction>
     </force_torque>
-    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
+    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.0"/>
   </sensor>
   <gazebo reference="r_foot_rear_ft_sensor">
     <sensor name="r_foot_rear_ft" type="force_torque">
@@ -1740,7 +1740,7 @@
         <frame>sensor</frame>
         <measure_direction>parent_to_child</measure_direction>
       </force_torque>
-      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
+      <pose>0.0 0.0 0.0 0.0 -0.0 -2.094395210586932</pose>
       <plugin name="right_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_foot_rear_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1752,7 +1752,7 @@
       <frame>sensor</frame>
       <measure_direction>parent_to_child</measure_direction>
     </force_torque>
-    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
+    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.0"/>
   </sensor>
   <gazebo reference="head">
     <sensor name="head_imu_0" type="imu">
diff --git a/iCub/robots/left_hand_mk3/model.urdf b/iCub/robots/left_hand_mk3/model.urdf
index c60aa07..1310afa 100644
--- a/iCub/robots/left_hand_mk3/model.urdf
+++ b/iCub/robots/left_hand_mk3/model.urdf
@@ -203,7 +203,7 @@
   </joint>
   <link name="l_hand_thumb_proximal_link">
     <inertial>
-      <origin xyz="0.007942237973736329 -5.9058138708734456e-05 -0.001045447354816556" rpy="0 0 0"/>
+      <origin xyz="0.007942237973736326 -5.9058138708734456e-05 -0.001045447354816556" rpy="0 0 0"/>
       <mass value="0.00972892"/>
       <inertia ixx="7.30894e-07" ixy="-1.72738e-09" ixz="-2.79201e-07" iyy="1.01435e-06" iyz="6.0045e-10" izz="8.85583e-07"/>
     </inertial>
@@ -224,8 +224,8 @@
     </collision>
   </link>
   <joint name="l_hand_thumb_rotation_joint" type="revolute">
-    <origin xyz="0.04079150000000001 -0.048141699999999996 -4.558000000000036e-05" rpy="-1.919861924123965 0.17453290528622997 -1.4311699319115785"/>
-    <axis xyz="1.000000323780413 -2.554693535849738e-07 6.571824263261195e-08"/>
+    <origin xyz="0.04079150000000001 -0.048141699999999996 -4.558000000000036e-05" rpy="-1.9198619241239645 0.17453290528623008 -1.4311699319115785"/>
+    <axis xyz="1.0000003237804127 -2.554693537237517e-07 6.571824259405435e-08"/>
     <parent link="l_hand_metacarpus_link"/>
     <child link="l_hand_thumb_proximal_link"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.0" velocity="50000"/>
@@ -255,7 +255,7 @@
   </link>
   <joint name="l_hand_thumb_proximal_joint" type="revolute">
     <origin xyz="0.021999958515447667 0 0.003500025233931968" rpy="0 0 0"/>
-    <axis xyz="2.628557918787494e-08 -1.0000002854169352 -2.183550824779987e-07"/>
+    <axis xyz="2.6285579160119366e-08 -1.0000002854169352 -2.1835508227251483e-07"/>
     <parent link="l_hand_thumb_proximal_link"/>
     <child link="l_hand_thumb_medial_link"/>
     <limit effort="50000" lower="0.0" upper="1.4835298641951802" velocity="50000"/>
@@ -263,12 +263,12 @@
   </joint>
   <link name="l_hand_thumb_fingertip_link">
     <inertial>
-      <origin xyz="0.010517869126825619 8.912381595140711e-06 -0.003989284736807208" rpy="0 0 0"/>
+      <origin xyz="0.010517869126825605 8.912381595133773e-06 -0.003989284736807201" rpy="0 0 0"/>
       <mass value="0.0169365"/>
       <inertia ixx="0.01" ixy="1.68813e-09" ixz="-2.86966e-08" iyy="0.01" iyz="-1.92473e-09" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.0029999461796957755 0 0.0030000193982093123" rpy="0 0 0"/>
+      <origin xyz="-0.0029999461796957824 0 0.0030000193982093193" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_l_hand_distal_thumb_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -277,15 +277,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.0029999461796957755 0 0.0030000193982093123" rpy="0 0 0"/>
+      <origin xyz="-0.0029999461796957824 0 0.0030000193982093193" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_l_hand_distal_thumb_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hand_thumb_distal_joint" type="revolute">
-    <origin xyz="0.020000016253208935 0 0.0014999980937911578" rpy="0 0 0"/>
-    <axis xyz="2.628557918787494e-08 -1.0000002854169352 -2.183550824779987e-07"/>
+    <origin xyz="0.020000016253208942 0 0.0014999980937911508" rpy="0 0 0"/>
+    <axis xyz="2.6285579160119366e-08 -1.0000002854169352 -2.1835508227251483e-07"/>
     <parent link="l_hand_thumb_medial_link"/>
     <child link="l_hand_thumb_fingertip_link"/>
     <limit effort="50000" lower="0.0" upper="0.9599310885968813" velocity="50000"/>