diff --git a/iCub/conf_icub3/gazebo_icub_head.ini b/iCub/conf_icub3/gazebo_icub_head.ini
index 9939c14..b12c4eb 100644
--- a/iCub/conf_icub3/gazebo_icub_head.ini
+++ b/iCub/conf_icub3/gazebo_icub_head.ini
@@ -51,6 +51,7 @@ stictionUp    0.0   0.0   0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 4.363 4.363 4.363
diff --git a/iCub/conf_icub3/gazebo_icub_head_without_eyes.ini b/iCub/conf_icub3/gazebo_icub_head_without_eyes.ini
index c670dd2..a76edd9 100644
--- a/iCub/conf_icub3/gazebo_icub_head_without_eyes.ini
+++ b/iCub/conf_icub3/gazebo_icub_head_without_eyes.ini
@@ -27,10 +27,10 @@ device gazebo_controlboard
 jointNames neck_pitch neck_roll neck_yaw
 name head
 
-min_stiffness 0.0    0.0    0.0   
+min_stiffness 0.0    0.0    0.0
 max_stiffness 1000.0 1000.0 1000.0
-min_damping   0.0    0.0    0.0   
-max_damping   100.0  100.0  100.0 
+min_damping   0.0    0.0    0.0
+max_damping   100.0  100.0  100.0
 
 #PIDs:
 # this information is used to set the PID values in simulation for GAZEBO, we need only the first three values
@@ -40,25 +40,26 @@ controlLaw    joint_pid_gazebo_v1
 kp            1.745 1.745 1.745
 kd            0.122 0.122 0.122
 ki            0.003 0.003 0.003
-maxInt        9999  9999  9999 
-maxOutput     9999  9999  9999 
-shift         0.0   0.0   0.0  
-ko            0.0   0.0   0.0  
-stictionUp    0.0   0.0   0.0  
-stictionDwn   0.0   0.0   0.0  
+maxInt        9999  9999  9999
+maxOutput     9999  9999  9999
+shift         0.0   0.0   0.0
+ko            0.0   0.0   0.0
+stictionUp    0.0   0.0   0.0
+stictionDwn   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726
 kd            0.035 0.035 0.035
 ki            0.003 0.003 0.003
-maxInt        9999  9999  9999 
-maxOutput     9999  9999  9999 
-shift         0.0   0.0   0.0  
-ko            0.0   0.0   0.0  
-stictionUp    0.0   0.0   0.0  
-stictionDwn   0.0   0.0   0.0  
+maxInt        9999  9999  9999
+maxOutput     9999  9999  9999
+shift         0.0   0.0   0.0
+ko            0.0   0.0   0.0
+stictionUp    0.0   0.0   0.0
+stictionDwn   0.0   0.0   0.0
 
 [IMPEDANCE_CONTROL]
 controlUnits  metric_units
@@ -67,6 +68,6 @@ stiffness     0.0   0.0   0.0
 damping       0.0   0.0   0.0
 
 [LIMITS]
-jntPosMax 22.0 20.0 45.0 
+jntPosMax 22.0 20.0 45.0
 jntPosMin -30.0 -20.0 -45.0
 jntVelMax 100.0 100.0 100.0
diff --git a/iCub/conf_icub3/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini b/iCub/conf_icub3/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini
index 4e049cd..5389288 100644
--- a/iCub/conf_icub3/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini
+++ b/iCub/conf_icub3/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini
@@ -49,6 +49,7 @@ stictionUp    0.0   0.0   0.0   0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 5.236 5.236 5.236 5.236
diff --git a/iCub/conf_icub3/gazebo_icub_left_hand_finger.ini b/iCub/conf_icub3/gazebo_icub_left_hand_finger.ini
index 2e8b870..e7844b9 100644
--- a/iCub/conf_icub3/gazebo_icub_left_hand_finger.ini
+++ b/iCub/conf_icub3/gazebo_icub_left_hand_finger.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 5.235
diff --git a/iCub/conf_icub3/gazebo_icub_left_hand_fingers.ini b/iCub/conf_icub3/gazebo_icub_left_hand_fingers.ini
index 4ed8880..e433182 100644
--- a/iCub/conf_icub3/gazebo_icub_left_hand_fingers.ini
+++ b/iCub/conf_icub3/gazebo_icub_left_hand_fingers.ini
@@ -53,12 +53,13 @@ kd            0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 ki            0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174 0.174
 maxInt        9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
 maxOutput     9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999  9999
-shift         0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0  
+shift         0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 ko            0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 stictionUp    0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236
diff --git a/iCub/conf_icub3/gazebo_icub_left_hand_index.ini b/iCub/conf_icub3/gazebo_icub_left_hand_index.ini
index c36ea47..2f12791 100644
--- a/iCub/conf_icub3/gazebo_icub_left_hand_index.ini
+++ b/iCub/conf_icub3/gazebo_icub_left_hand_index.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726
diff --git a/iCub/conf_icub3/gazebo_icub_left_hand_middle.ini b/iCub/conf_icub3/gazebo_icub_left_hand_middle.ini
index 1ab6c0c..81f628c 100644
--- a/iCub/conf_icub3/gazebo_icub_left_hand_middle.ini
+++ b/iCub/conf_icub3/gazebo_icub_left_hand_middle.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726
diff --git a/iCub/conf_icub3/gazebo_icub_left_hand_pinky.ini b/iCub/conf_icub3/gazebo_icub_left_hand_pinky.ini
index 754b25f..bf3258f 100644
--- a/iCub/conf_icub3/gazebo_icub_left_hand_pinky.ini
+++ b/iCub/conf_icub3/gazebo_icub_left_hand_pinky.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 8.726 8.726 8.726
diff --git a/iCub/conf_icub3/gazebo_icub_left_hand_thumb.ini b/iCub/conf_icub3/gazebo_icub_left_hand_thumb.ini
index 86cc73c..fb22964 100644
--- a/iCub/conf_icub3/gazebo_icub_left_hand_thumb.ini
+++ b/iCub/conf_icub3/gazebo_icub_left_hand_thumb.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 5.236
diff --git a/iCub/conf_icub3/gazebo_icub_left_leg.ini b/iCub/conf_icub3/gazebo_icub_left_leg.ini
index 0a149e9..246b879 100644
--- a/iCub/conf_icub3/gazebo_icub_left_leg.ini
+++ b/iCub/conf_icub3/gazebo_icub_left_leg.ini
@@ -48,6 +48,7 @@ stictionUp    0.0    0.0    0.0    0.0    0.0    0.0
 stictionDwn   0.0    0.0    0.0    0.0    0.0    0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 8.726 8.726 8.726
diff --git a/iCub/conf_icub3/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini b/iCub/conf_icub3/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini
index 37187a5..73ced13 100644
--- a/iCub/conf_icub3/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini
+++ b/iCub/conf_icub3/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini
@@ -49,6 +49,7 @@ stictionUp    0.0   0.0   0.0   0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 5.236 5.236 5.236 5.236
diff --git a/iCub/conf_icub3/gazebo_icub_right_hand_finger.ini b/iCub/conf_icub3/gazebo_icub_right_hand_finger.ini
index a2869d0..628a774 100644
--- a/iCub/conf_icub3/gazebo_icub_right_hand_finger.ini
+++ b/iCub/conf_icub3/gazebo_icub_right_hand_finger.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 5.236
diff --git a/iCub/conf_icub3/gazebo_icub_right_hand_fingers.ini b/iCub/conf_icub3/gazebo_icub_right_hand_fingers.ini
index 31f500d..7d51ba4 100644
--- a/iCub/conf_icub3/gazebo_icub_right_hand_fingers.ini
+++ b/iCub/conf_icub3/gazebo_icub_right_hand_fingers.ini
@@ -59,6 +59,7 @@ stictionUp    0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236 5.236
diff --git a/iCub/conf_icub3/gazebo_icub_right_hand_index.ini b/iCub/conf_icub3/gazebo_icub_right_hand_index.ini
index 1c244a2..5d60b69 100644
--- a/iCub/conf_icub3/gazebo_icub_right_hand_index.ini
+++ b/iCub/conf_icub3/gazebo_icub_right_hand_index.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726
diff --git a/iCub/conf_icub3/gazebo_icub_right_hand_middle.ini b/iCub/conf_icub3/gazebo_icub_right_hand_middle.ini
index a473953..00c1294 100644
--- a/iCub/conf_icub3/gazebo_icub_right_hand_middle.ini
+++ b/iCub/conf_icub3/gazebo_icub_right_hand_middle.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726
diff --git a/iCub/conf_icub3/gazebo_icub_right_hand_pinky.ini b/iCub/conf_icub3/gazebo_icub_right_hand_pinky.ini
index 81b5d3f..c1367ac 100644
--- a/iCub/conf_icub3/gazebo_icub_right_hand_pinky.ini
+++ b/iCub/conf_icub3/gazebo_icub_right_hand_pinky.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 8.726 8.726 8.726
diff --git a/iCub/conf_icub3/gazebo_icub_right_hand_thumb.ini b/iCub/conf_icub3/gazebo_icub_right_hand_thumb.ini
index 733f35a..e4885c0 100644
--- a/iCub/conf_icub3/gazebo_icub_right_hand_thumb.ini
+++ b/iCub/conf_icub3/gazebo_icub_right_hand_thumb.ini
@@ -46,6 +46,7 @@ stictionUp    0.0   0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 5.236
diff --git a/iCub/conf_icub3/gazebo_icub_right_leg.ini b/iCub/conf_icub3/gazebo_icub_right_leg.ini
index be92625..c91f2dd 100644
--- a/iCub/conf_icub3/gazebo_icub_right_leg.ini
+++ b/iCub/conf_icub3/gazebo_icub_right_leg.ini
@@ -48,6 +48,7 @@ stictionUp    0.0    0.0    0.0    0.0    0.0    0.0
 stictionDwn   0.0    0.0    0.0    0.0    0.0    0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726 8.726 8.726 8.726
diff --git a/iCub/conf_icub3/gazebo_icub_torso.ini b/iCub/conf_icub3/gazebo_icub_torso.ini
index cab5cf3..2b7ef44 100644
--- a/iCub/conf_icub3/gazebo_icub_torso.ini
+++ b/iCub/conf_icub3/gazebo_icub_torso.ini
@@ -48,6 +48,7 @@ stictionUp    0.0   0.0   0.0
 stictionDwn   0.0   0.0   0.0
 
 [VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
 controlUnits  metric_units
 controlLaw    joint_pid_gazebo_v1
 kp            8.726 8.726 8.726
diff --git a/iCub/robots/iCubDarmstadt01/model.urdf b/iCub/robots/iCubDarmstadt01/model.urdf
index 1acd7d5..676aece 100644
--- a/iCub/robots/iCubDarmstadt01/model.urdf
+++ b/iCub/robots/iCubDarmstadt01/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -525,7 +525,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.000327222" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.000248285" iyz="-4.29975e-06" izz="0.000150937"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2658,13 +2658,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2706,13 +2706,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2754,13 +2754,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2778,13 +2778,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2814,13 +2814,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2838,13 +2838,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2862,13 +2862,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -3004,24 +3004,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3063,13 +3063,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3087,178 +3087,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3276,25 +3276,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3368,45 +3368,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubErzelli02/model.urdf b/iCub/robots/iCubErzelli02/model.urdf
index bcb3414..de1a7ce 100644
--- a/iCub/robots/iCubErzelli02/model.urdf
+++ b/iCub/robots/iCubErzelli02/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -525,7 +525,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.000327222" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.000248285" iyz="-4.29975e-06" izz="0.000150937"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2658,13 +2658,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2706,13 +2706,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2754,13 +2754,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2778,13 +2778,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2814,13 +2814,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2838,13 +2838,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2862,13 +2862,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -3004,24 +3004,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3063,13 +3063,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3087,178 +3087,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3276,25 +3276,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3368,45 +3368,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGazeboV2_5/model.urdf b/iCub/robots/iCubGazeboV2_5/model.urdf
index 332206e..a2fd141 100644
--- a/iCub/robots/iCubGazeboV2_5/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.01" iyz="1.19785e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="0.01" iyz="3.92498e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="0.01" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="0.01" iyz="-1.59685e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.01" iyz="1.20112e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="0.01" iyz="3.9319e-06" izz="0.01"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="0.01" ixy="3.08362e-08" ixz="9.42153e-07" iyy="0.01" iyz="-1.58924e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="0.01" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="0.01" iyz="3.32338e-08" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.01" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.01" iyz="-4.29975e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2658,13 +2658,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2706,13 +2706,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2754,13 +2754,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2778,13 +2778,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2814,13 +2814,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2838,13 +2838,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2862,13 +2862,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -3004,24 +3004,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3063,13 +3063,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3087,178 +3087,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3276,25 +3276,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3368,45 +3368,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGazeboV2_5_plus/model.urdf b/iCub/robots/iCubGazeboV2_5_plus/model.urdf
index e666461..9788760 100644
--- a/iCub/robots/iCubGazeboV2_5_plus/model.urdf
+++ b/iCub/robots/iCubGazeboV2_5_plus/model.urdf
@@ -247,12 +247,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.01" iyz="1.19785e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -261,15 +261,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -298,7 +298,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -335,12 +335,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="0.01" iyz="3.92498e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -349,7 +349,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -365,7 +365,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -386,7 +386,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -395,35 +395,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -444,14 +444,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -479,7 +479,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -500,35 +500,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -542,7 +542,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -563,19 +563,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="0.01" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="0.01" iyz="-1.59685e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -584,14 +584,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -621,7 +621,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -667,12 +667,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.01" iyz="1.20112e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -681,15 +681,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -718,7 +718,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -755,7 +755,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="0.01" iyz="3.9319e-06" izz="0.01"/>
     </inertial>
@@ -785,7 +785,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -806,7 +806,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -815,35 +815,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -864,7 +864,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -920,35 +920,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -962,7 +962,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -983,19 +983,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="0.01" ixy="3.08362e-08" ixz="9.42153e-07" iyy="0.01" iyz="-1.58924e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1004,14 +1004,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1041,7 +1041,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1057,12 +1057,12 @@
   </joint>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1071,7 +1071,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1247,7 +1247,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1275,7 +1275,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1303,21 +1303,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1352,14 +1352,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1373,21 +1373,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1408,14 +1408,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1436,7 +1436,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1450,7 +1450,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1485,12 +1485,12 @@
   </joint>
   <link name="r_ankle_1">
     <inertial>
-      <origin xyz="0.015143405473968821 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015143405473968807 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6803"/>
       <inertia ixx="0.0004046207392315529" ixy="8.523488844593136e-06" ixz="-2.8913590319053794e-05" iyy="0.0004379396781535379" iyz="3.7330395179426065e-05" izz="0.00031892819418424145"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1499,7 +1499,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1515,12 +1515,12 @@
   </joint>
   <link name="r_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235187 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235048 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.01" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.01" iyz="2.0701e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1529,7 +1529,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1537,7 +1537,7 @@
   </link>
   <joint name="r_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.1979394973216095e-07"/>
+    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.197939497389039e-07"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1545,12 +1545,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <mass value="0.357881"/>
       <inertia ixx="0.00109611" ixy="-2.9193e-07" ixz="-3.70109e-05" iyy="0.00125706" iyz="5.10952e-06" izz="0.000215359"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1566,7 +1566,7 @@
     </collision>
   </link>
   <joint name="r_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot"/>
     <dynamics damping="0.1"/>
@@ -1580,12 +1580,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1594,7 +1594,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1610,12 +1610,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0015761999999999929 -1.940000000000275e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0015761999999999998 -1.939999999998887e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.421106"/>
       <inertia ixx="0.000494284" ixy="1.5483e-07" ixz="-1.86308e-07" iyy="0.000288736" iyz="1.52962e-05" izz="0.000319008"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1624,14 +1624,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1640,12 +1640,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012299999999999117 -0.0002823000000000131 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.0001229999999999981 -0.000282300000000027 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="0.01" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="0.01" iyz="3.32338e-08" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1654,26 +1654,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008574999999999972 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008575000000000041 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868000000000092 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868000000000023 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1682,7 +1682,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1698,7 +1698,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.051256699999999975 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.05125669999999998 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1712,12 +1712,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999700000000013 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997000000000061 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1726,7 +1726,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1742,12 +1742,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015152971856010347 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015152971856010333 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447618553062084" ixy="-8.97867775831874e-06" ixz="2.9134846047326194e-05" iyy="0.00043720150287133953" iyz="3.730206268073148e-05" izz="0.00031828795462876227"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1756,7 +1756,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1772,12 +1772,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235465 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235326 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.01" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.01" iyz="2.0701e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1786,7 +1786,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1794,7 +1794,7 @@
   </link>
   <joint name="l_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.1979394973216095e-07"/>
+    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.197939497389039e-07"/>
     <parent link="l_ankle_1"/>
     <child link="l_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1823,7 +1823,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2315,13 +2315,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2363,13 +2363,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2411,13 +2411,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2435,13 +2435,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2471,13 +2471,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2495,13 +2495,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2519,13 +2519,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2661,24 +2661,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2696,13 +2696,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2720,238 +2720,238 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b12" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577099999999994 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.0125771 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577099999999994 -0.040407200000000004 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.0125771 -0.040407200000000004 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012575099999999985 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575099999999992 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999985 0.040407 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999992 0.040407 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013080999999999995 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000002 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013080999999999995 -0.040407200000000004 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000002 -0.040407200000000004 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01307899999999998 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013078999999999986 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.01307899999999998 0.040407 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999986 0.040407 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001630000000005 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001630000000005 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352109 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352108999999999 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352109 0.007765700000000028 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352108999999999 0.007765700000000028 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053785 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053785 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 -0.014429399999999995 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429399999999995 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412948999999999 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.034129489999999985 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412948999999999 -0.041499899999999985 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.034129489999999985 -0.041499899999999985 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999998 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043784999999999984 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999998 0.01781110000000001 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043784999999999984 0.01781110000000001 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.043785 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378499999999999 -0.014191799999999997 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785 -0.014191799999999997 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313347 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133469999999995 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313347 0.017184100000000008 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133469999999995 0.017184100000000008 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.054451799999999995 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
+      <pose>0.05445179999999999 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b9" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.054451799999999995 -0.013799100000000002 -0.0043179999999999885"/>
+    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.05445179999999999 -0.013799100000000002 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.02591510000000001 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.02591510000000001 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999994 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999994 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGazeboV2_6/model.urdf b/iCub/robots/iCubGazeboV2_6/model.urdf
index cdfb5bb..4024cab 100644
--- a/iCub/robots/iCubGazeboV2_6/model.urdf
+++ b/iCub/robots/iCubGazeboV2_6/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.01" iyz="1.19785e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="0.01" iyz="3.92498e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="0.01" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="0.01" iyz="-1.59685e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.01" iyz="1.20112e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="0.01" iyz="3.9319e-06" izz="0.01"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="0.01" ixy="3.08362e-08" ixz="9.42153e-07" iyy="0.01" iyz="-1.58924e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="0.01" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="0.01" iyz="3.32338e-08" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.01" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.01" iyz="-4.29975e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2658,13 +2658,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2706,13 +2706,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2754,13 +2754,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2778,13 +2778,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2814,13 +2814,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2838,13 +2838,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2862,13 +2862,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -3004,24 +3004,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3063,13 +3063,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3087,178 +3087,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3276,25 +3276,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3368,45 +3368,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGazeboV2_7/model.urdf b/iCub/robots/iCubGazeboV2_7/model.urdf
index 5d055d0..3dba975 100644
--- a/iCub/robots/iCubGazeboV2_7/model.urdf
+++ b/iCub/robots/iCubGazeboV2_7/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.01" iyz="1.19785e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="0.01" iyz="3.92498e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="0.01" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="0.01" iyz="-1.59685e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.01" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.01" iyz="1.20112e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="0.01" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="0.01" iyz="3.9319e-06" izz="0.01"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="0.01" ixy="3.08362e-08" ixz="9.42153e-07" iyy="0.01" iyz="-1.58924e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="0.01" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="0.01" iyz="3.32338e-08" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.01" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.01" iyz="-4.29975e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2674,13 +2674,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2722,13 +2722,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2770,13 +2770,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2794,13 +2794,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2830,13 +2830,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2854,13 +2854,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2878,13 +2878,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -3020,24 +3020,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3079,13 +3079,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3103,178 +3103,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3292,25 +3292,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3384,45 +3384,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGazeboV3/model.urdf b/iCub/robots/iCubGazeboV3/model.urdf
index ca61cef..b6c9123 100644
--- a/iCub/robots/iCubGazeboV3/model.urdf
+++ b/iCub/robots/iCubGazeboV3/model.urdf
@@ -816,7 +816,7 @@
   </link>
   <joint name="r_shoulder_pitch" type="revolute">
     <origin xyz="-0.020835100000000002 -0.0397423 0.1052029" rpy="-0.25326164431680925 2.3014774394592079e-07 0.26179970938509745"/>
-    <axis xyz="1.499044390054707e-07 0.9999999783534812 -1.2401537036188515e-07"/>
+    <axis xyz="1.499044390098182e-07 0.9999999783534812 -1.2401537036323317e-07"/>
     <parent link="chest"/>
     <child link="r_shoulder_1"/>
     <limit effort="50000" lower="-1.53588974175501" upper="0.22689280275926285" velocity="120.0"/>
@@ -824,12 +824,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="0.0007118441514370015 -0.0002899713150672778 -0.027865575116629175" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.000711844151437005 -0.00028997131506725005 -0.027865575116629175" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.304658373"/>
       <inertia ixx="0.01" ixy="3.2537838826012804e-05" ixz="2.26161974125395e-05" iyy="0.01" iyz="-5.163216327773138e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.023893261066846312 0.14153171676199225 -0.0072523502076727205" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.023893261066846316 0.14153171676199228 -0.00725235020767272" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -838,15 +838,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.023893261066846312 0.14153171676199225 -0.0072523502076727205" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.023893261066846316 0.14153171676199228 -0.00725235020767272" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069357 -0.053747792579130614 -6.132332688812785e-07"/>
-    <axis xyz="-0.9999998262417886 4.2105014356162707e-08 2.4630481189336084e-08"/>
+    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069352 -0.05374779257913059 -6.132332688804098e-07"/>
+    <axis xyz="-0.9999998262417886 4.210501434612913e-08 2.4630481182686846e-08"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.8448866807507573" velocity="120.0"/>
@@ -854,12 +854,12 @@
   </joint>
   <link name="r_shoulder_3">
     <inertial>
-      <origin xyz="-8.928496484206933e-05 -3.376971940446949e-05 -0.00759450025288725" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-8.928496484206933e-05 -3.376971940446949e-05 -0.007594500252887257" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.24467077"/>
       <inertia ixx="0.01" ixy="5.557727008657165e-07" ixz="-2.7194783592437544e-06" iyy="0.01" iyz="-3.2422643569677497e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743207452236198 0.1415314977120317 0.04704763137021827" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743207452236198 0.1415314977120317 0.047047631370218265" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -868,26 +868,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743207452236198 0.1415314977120317 0.04704763137021827" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743207452236198 0.1415314977120317 0.047047631370218265" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_arm_ft_sensor" type="fixed">
-    <origin xyz="-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099" rpy="0 0 0"/>
+    <origin xyz="-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099" rpy="0 0 0"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_upper_arm">
     <inertial>
-      <origin xyz="-0.000798982178358984 -0.002084481892803941 -0.05063856093223067" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-0.0007989821783589805 -0.002084481892803941 -0.05063856093223067" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="1.960447492"/>
       <inertia ixx="0.004119602086814579" ixy="1.158223128604559e-05" ixz="0.00020349650199902935" iyy="0.003957940948860767" iyz="0.00020664971228357863" izz="0.0020250627669371347"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743125543818577 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03074312554381858 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -896,15 +896,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743125543818577 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03074312554381858 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="0 4.2183273077367467e-07 -0.022499941373153573" rpy="0 0 0"/>
-    <axis xyz="-3.787621790513063e-08 2.805197706068352e-08 1.0000002566182309"/>
+    <origin xyz="0 4.2183273077367467e-07 -0.02249994137315358" rpy="0 0 0"/>
+    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
     <parent link="r_shoulder_3"/>
     <child link="r_upper_arm"/>
     <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
@@ -912,12 +912,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.010252765750166596 -0.0018331179988757829 -0.015053741795128806" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <origin xyz="-0.010252765750166596 -0.0018331179988758106 -0.015053741795128806" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <mass value="0.217609341"/>
       <inertia ixx="0.01" ixy="2.353974571197465e-08" ixz="1.9513680659329553e-05" iyy="0.01" iyz="-6.334018264294134e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.020743191776066316 0.13914061722125232 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <origin xyz="0.020743191776066316 0.1391406172212523 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -926,15 +926,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.020743191776066316 0.13914061722125232 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <origin xyz="0.020743191776066316 0.1391406172212523 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_elbow" type="revolute">
-    <origin xyz="0.009999931922603816 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
-    <axis xyz="2.4949582809984105e-07 -1.0000002733514362 2.7942487004772865e-08"/>
+    <origin xyz="0.00999993192260382 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
+    <axis xyz="2.4949582807394525e-07 -1.0000002733514362 2.7942487010105877e-08"/>
     <parent link="r_upper_arm"/>
     <child link="r_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -963,8 +963,8 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.014999869468812953 -0.001999649905318951 -0.05530006367251966" rpy="0 0 0"/>
-    <axis xyz="-3.787621790513063e-08 2.805197706068352e-08 1.0000002566182309"/>
+    <origin xyz="-0.014999869468812953 -0.0019996499053189787 -0.05530006367251966" rpy="0 0 0"/>
+    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -972,12 +972,12 @@
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="-0.014389827079018234 0.0005092753390522065 -9.787315764231419e-05" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-0.014389827079018234 0.0005092753390521787 -9.78731576423697e-05" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.032535096"/>
       <inertia ixx="0.01" ixy="8.699001601283703e-07" ixz="1.3955745499815409e-08" iyy="0.01" iyz="-1.0544717252156218e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.01944310973607847 0.14403852854340193 0.3560480536175482" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.01944310973607847 0.1440385285434019 0.35604805361754815" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -986,7 +986,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.01944310973607847 0.14403852854340193 0.3560480536175482" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.01944310973607847 0.1440385285434019 0.35604805361754815" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -994,7 +994,7 @@
   </link>
   <joint name="r_wrist_pitch" type="revolute">
     <origin xyz="0.016299953353948633 -0.002007442890811839 -0.10750007971210637" rpy="0 0 0"/>
-    <axis xyz="0.9999998262417886 -4.2105014356162707e-08 -2.4630481189336084e-08"/>
+    <axis xyz="0.9999998262417886 -4.210501434612913e-08 -2.4630481182686846e-08"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -1002,12 +1002,12 @@
   </joint>
   <link name="r_hand">
     <inertial>
-      <origin xyz="0.004828314385249662 0.00615667237451184 -0.0671378028889843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.004828314385249669 0.00615667237451184 -0.0671378028889843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.2499009"/>
       <inertia ixx="0.01" ixy="4.527703539619043e-05" ixz="1.0511331468536763e-05" iyy="0.01" iyz="1.0826179610148775e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.03574323261035122 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03574323261035123 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1016,27 +1016,27 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03574323261035122 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03574323261035123 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="continuous">
-    <origin xyz="-0.016300122874272734 0.0069993900510918194 0" rpy="0 0 0"/>
-    <axis xyz="-2.4949582809984105e-07 1.0000002733514362 -2.7942487004772865e-08"/>
+    <origin xyz="-0.016300122874272734 0.006999390051091792 0" rpy="0 0 0"/>
+    <axis xyz="-2.4949582807394525e-07 1.0000002733514362 -2.7942487010105877e-08"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_shoulder_1">
     <inertial>
-      <origin xyz="0.0002918276617865645 0.05723090680782866 0.0005026028232598713" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <origin xyz="0.0002918276617865645 0.05723090680782866 0.0005026028232598678" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
       <mass value="1.376687449"/>
       <inertia ixx="0.01" ixy="0.0005787105562276275" ixz="0.0005349240605581856" iyy="0.01" iyz="-0.00015089444754685564" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677896" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677893" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1045,7 +1045,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677896" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677893" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1053,7 +1053,7 @@
   </link>
   <joint name="l_shoulder_pitch" type="revolute">
     <origin xyz="-0.020835100000000002 0.0397423 0.1052029" rpy="0.25326162380993705 -1.1977334179319483e-07 -0.26179930184968747"/>
-    <axis xyz="3.323079797673039e-07 0.9999999783534342 1.4452224303407313e-07"/>
+    <axis xyz="3.323079797554136e-07 0.9999999783534342 1.4452224303420037e-07"/>
     <parent link="chest"/>
     <child link="l_shoulder_1"/>
     <limit effort="50000" lower="-1.53588974175501" upper="0.22689280275926285" velocity="120.0"/>
@@ -1061,12 +1061,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.0032933620254868984 0.0002563804707528261 -0.027865685757097337" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.003293362025486902 0.0002563804707528261 -0.027865685757097337" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.304658373"/>
       <inertia ixx="0.01" ixy="-3.313235231845923e-05" ixz="-2.3110931755362807e-05" iyy="0.01" iyz="-5.228966531392095e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.0263930978691934 -0.14153112610649202 -0.007252397644601654" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.02639309786919337 -0.14153112610649204 -0.00725239764460165" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1075,15 +1075,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.0263930978691934 -0.14153112610649202 -0.007252397644601654" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.02639309786919337 -0.14153112610649204 -0.00725239764460165" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="0.004992849331690818 0.111499838456099 0.0002686502277366859" rpy="-0.17453296140985 -0.05374759931933852 0"/>
-    <axis xyz="0.9999998262417684 2.0348252649342313e-07 -4.0230228454118944e-08"/>
+    <origin xyz="0.004992849331690818 0.111499838456099 0.00026865022773668243" rpy="-0.17453296140985003 -0.05374759931933849 0"/>
+    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.8448866807507573" velocity="120.0"/>
@@ -1091,12 +1091,12 @@
   </joint>
   <link name="l_shoulder_3">
     <inertial>
-      <origin xyz="-4.41626402200232e-06 9.587749727052453e-05 -0.007326213473011087" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-4.416264022005789e-06 9.587749727052453e-05 -0.007326213473011087" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.242239185"/>
       <inertia ixx="0.01" ixy="-3.5592877751030733e-07" ixz="2.772905125990325e-06" iyy="0.01" iyz="-3.332197996904868e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743231026171724 -0.14153148936381538 0.04704764107946005" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.03074323102617169 -0.14153148936381538 0.04704764107946006" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1105,26 +1105,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743231026171724 -0.14153148936381538 0.04704764107946005" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.03074323102617169 -0.14153148936381538 0.04704764107946006" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_arm_ft_sensor" type="fixed">
-    <origin xyz="-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172" rpy="0 0 0"/>
+    <origin xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="0 0 0"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_arm">
     <inertial>
-      <origin xyz="-0.0007989833046956885 0.002084478118404877 -0.050638561069828086" rpy="-0.062128608300921935 0.07142689081245782 0.27099931149673573"/>
+      <origin xyz="-0.0007989833046956885 0.0020844781184049047 -0.050638561069828086" rpy="-0.06212860830092193 0.0714268908124578 0.27099931149673545"/>
       <mass value="2.07285"/>
       <inertia ixx="0.00418487" ixy="1.22463e-05" ixz="0.000218498" iyy="0.0043558" iyz="0.000215164" izz="0.00214117"/>
     </inertial>
     <visual>
-      <origin xyz="0.03074314946867922 -0.1415310659113312 0.06954758245258467" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.030743149468679187 -0.1415310659113312 0.06954758245258467" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1133,15 +1133,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03074314946867922 -0.1415310659113312 0.06954758245258467" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.030743149468679187 -0.1415310659113312 0.06954758245258467" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0 -4.2345248449371375e-07 -0.02249994137312443" rpy="0 0 0"/>
-    <axis xyz="2.227647404040578e-08 -4.393786245948661e-08 -1.0000002566182309"/>
+    <origin xyz="0 -4.2345248449371375e-07 -0.022499941373124423" rpy="0 0 0"/>
+    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
     <parent link="l_shoulder_3"/>
     <child link="l_upper_arm"/>
     <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
@@ -1149,12 +1149,12 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="-0.010241946078109292 0.0018224069895867168 -0.01508962025855548" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-0.010241946078109288 0.0018224069895867168 -0.015089620258555453" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.217768231"/>
       <inertia ixx="0.01" ixy="7.104417335699923e-07" ixz="-9.337153146297134e-06" iyy="0.01" iyz="-9.773438661251487e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.0207432192337597 -0.14003141848790235 0.19324791999016847" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.020743219233759668 -0.14003141848790235 0.1932479199901685" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1163,7 +1163,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.0207432192337597 -0.14003141848790235 0.19324791999016847" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.020743219233759668 -0.14003141848790235 0.1932479199901685" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1171,7 +1171,7 @@
   </link>
   <joint name="l_elbow" type="revolute">
     <origin xyz="0.009999930234919404 -0.001499647423428896 -0.12370033753758375" rpy="0 0 0"/>
-    <axis xyz="-8.811824154381087e-08 -1.0000002733514628 4.4047361189014644e-08"/>
+    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
     <parent link="l_upper_arm"/>
     <child link="l_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -1179,12 +1179,12 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="-0.006269874102298061 -4.68551245806359e-05 -0.030648347755607153" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-0.0062698741022980675 -4.68551245806359e-05 -0.030648347755607208" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.858696525"/>
       <inertia ixx="0.001246738111071889" ixy="-6.996239702553336e-07" ixz="-2.5468935337559064e-05" iyy="0.0012148276986463716" iyz="0.00010266470596331766" izz="0.0006118920888855694"/>
     </inertial>
     <visual>
-      <origin xyz="0.03574308988793752 -0.14203106199153712 0.24854798357264754" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035743089887937486 -0.14203106199153712 0.24854798357264754" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1193,15 +1193,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03574308988793752 -0.14203106199153712 0.24854798357264754" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035743089887937486 -0.14203106199153712 0.24854798357264754" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="-0.014999870654177704 0.0019996435036348514 -0.055300063582479825" rpy="0 0 0"/>
-    <axis xyz="2.227647404040578e-08 -4.393786245948661e-08 -1.0000002566182309"/>
+    <origin xyz="-0.0149998706541777 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
+    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -1209,12 +1209,12 @@
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.014390150453737368 -0.0005083960996520587 -9.893035257668714e-05" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-0.014390150453737368 -0.0005083960996520864 -9.893035257668714e-05" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.032535096"/>
       <inertia ixx="0.01" ixy="-8.699001601283703e-07" ixz="-1.395584007720818e-08" iyy="0.01" iyz="-1.0544717252156218e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.019143073953651363 -0.14403795720960977 0.3560476247151885" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.019143073953651328 -0.1440379572096098 0.3560476247151885" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1223,15 +1223,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.019143073953651363 -0.14403795720960977 0.3560476247151885" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.019143073953651328 -0.1440379572096098 0.3560476247151885" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.016600015934286182 0.002006895218072541 -0.10749964114254112" rpy="0 0 0"/>
-    <axis xyz="-0.9999998262417684 -2.0348252649342313e-07 4.0230228454118944e-08"/>
+    <origin xyz="0.016600015934286182 0.0020068952180725685 -0.10749964114254112" rpy="0 0 0"/>
+    <axis xyz="-0.9999998262417685 -2.0348252625938444e-07 4.023022846062156e-08"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -1239,12 +1239,12 @@
   </joint>
   <link name="l_hand">
     <inertial>
-      <origin xyz="0.004786523181334722 -0.0061337378723908265 -0.0672770617484692" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.004786523181334726 -0.0061337378723908265 -0.0672770617484692" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.249250207"/>
       <inertia ixx="0.01" ixy="-4.512230668414326e-05" ixz="-9.754806880214843e-06" iyy="0.01" iyz="1.1339356520717838e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.03544319569838081 -0.13703856452803848 0.35604770325391955" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035443195698380774 -0.13703856452803848 0.35604770325391955" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1253,15 +1253,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03544319569838081 -0.13703856452803848 0.35604770325391955" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035443195698380774 -0.13703856452803848 0.35604770325391955" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.016300121744729387 -0.0069993926815711816 0" rpy="0 0 0"/>
-    <axis xyz="8.811824154381087e-08 1.0000002733514628 -4.4047361189014644e-08"/>
+    <origin xyz="-0.016300121744729387 -0.006999392681571209 0" rpy="0 0 0"/>
+    <axis xyz="8.811824174789715e-08 1.0000002733514628 -4.404736120498342e-08"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
     <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
@@ -1524,7 +1524,7 @@
         <frame>sensor</frame>
         <measure_direction>child_to_parent</measure_direction>
       </force_torque>
-      <pose>-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172 3.141592646862139 -3.104683320142575e-08 -1.5707962571980942</pose>
+      <pose>-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172 3.141592646862139 -3.10468332187885e-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>
@@ -1536,7 +1536,7 @@
       <frame>sensor</frame>
       <measure_direction>child_to_parent</measure_direction>
     </force_torque>
-    <origin rpy="3.141592646862139 -3.104683320142575e-08 -1.5707962571980942" xyz="-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172"/>
+    <origin rpy="3.141592646862139 -3.10468332187885e-08 -1.5707962571980942" xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172"/>
   </sensor>
   <gazebo reference="r_arm_ft_sensor">
     <sensor name="r_arm_ft_sensor" type="force_torque">
@@ -1546,7 +1546,7 @@
         <frame>sensor</frame>
         <measure_direction>child_to_parent</measure_direction>
       </force_torque>
-      <pose>-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099 3.141592644717707 4.094299285979593e-08 1.5707964185756365</pose>
+      <pose>-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099 3.141592644717707 4.094299285979593e-08 1.5707964185756365</pose>
       <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_arm_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1558,7 +1558,7 @@
       <frame>sensor</frame>
       <measure_direction>child_to_parent</measure_direction>
     </force_torque>
-    <origin rpy="3.141592644717707 4.094299285979593e-08 1.5707964185756365" xyz="-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099"/>
+    <origin rpy="3.141592644717707 4.094299285979593e-08 1.5707964185756365" xyz="-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099"/>
   </sensor>
   <gazebo reference="l_foot_front_ft_sensor">
     <sensor name="l_foot_front_ft_sensor" type="force_torque">
diff --git a/iCub/robots/iCubGenova01/model.urdf b/iCub/robots/iCubGenova01/model.urdf
index 1acd7d5..676aece 100644
--- a/iCub/robots/iCubGenova01/model.urdf
+++ b/iCub/robots/iCubGenova01/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -525,7 +525,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.000327222" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.000248285" iyz="-4.29975e-06" izz="0.000150937"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2658,13 +2658,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2706,13 +2706,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2754,13 +2754,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2778,13 +2778,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2814,13 +2814,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2838,13 +2838,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2862,13 +2862,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -3004,24 +3004,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3063,13 +3063,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3087,178 +3087,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3276,25 +3276,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3368,45 +3368,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGenova02/model.urdf b/iCub/robots/iCubGenova02/model.urdf
index 612b79d..188d3d3 100644
--- a/iCub/robots/iCubGenova02/model.urdf
+++ b/iCub/robots/iCubGenova02/model.urdf
@@ -247,12 +247,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -261,15 +261,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -298,7 +298,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -335,12 +335,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -349,7 +349,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -365,7 +365,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -386,7 +386,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -395,35 +395,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -444,14 +444,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -479,7 +479,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -500,35 +500,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -542,7 +542,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -563,19 +563,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -584,14 +584,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -621,7 +621,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -667,12 +667,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -681,15 +681,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -718,7 +718,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -755,7 +755,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -785,7 +785,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -806,7 +806,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -815,35 +815,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -864,7 +864,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -920,35 +920,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -962,7 +962,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -983,19 +983,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1004,14 +1004,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1041,7 +1041,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1057,12 +1057,12 @@
   </joint>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1071,7 +1071,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1247,7 +1247,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1275,7 +1275,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1303,21 +1303,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1352,14 +1352,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1373,21 +1373,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1408,14 +1408,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1436,7 +1436,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1450,7 +1450,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1485,12 +1485,12 @@
   </joint>
   <link name="r_ankle_1">
     <inertial>
-      <origin xyz="0.015143405473968821 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015143405473968807 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6803"/>
       <inertia ixx="0.0004046207392315529" ixy="8.523488844593136e-06" ixz="-2.8913590319053794e-05" iyy="0.0004379396781535379" iyz="3.7330395179426065e-05" izz="0.00031892819418424145"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1499,7 +1499,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1515,12 +1515,12 @@
   </joint>
   <link name="r_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235187 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235048 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.000327646" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.000244632" iyz="2.0701e-05" izz="0.000155777"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1529,7 +1529,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1537,7 +1537,7 @@
   </link>
   <joint name="r_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.1979394973216095e-07"/>
+    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.197939497389039e-07"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1545,12 +1545,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <mass value="0.357881"/>
       <inertia ixx="0.00109611" ixy="-2.9193e-07" ixz="-3.70109e-05" iyy="0.00125706" iyz="5.10952e-06" izz="0.000215359"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1559,14 +1559,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot"/>
     <dynamics damping="0.1"/>
@@ -1580,12 +1580,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1594,7 +1594,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1610,12 +1610,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0015761999999999929 -1.940000000000275e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0015761999999999998 -1.939999999998887e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.421106"/>
       <inertia ixx="0.000494284" ixy="1.5483e-07" ixz="-1.86308e-07" iyy="0.000288736" iyz="1.52962e-05" izz="0.000319008"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1624,14 +1624,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1640,12 +1640,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012299999999999117 -0.0002823000000000131 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.0001229999999999981 -0.000282300000000027 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1654,26 +1654,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008574999999999972 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008575000000000041 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868000000000092 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868000000000023 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1682,7 +1682,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1698,7 +1698,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.051256699999999975 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.05125669999999998 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1712,12 +1712,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999700000000013 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997000000000061 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1726,7 +1726,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1742,12 +1742,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015152971856010347 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015152971856010333 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447618553062084" ixy="-8.97867775831874e-06" ixz="2.9134846047326194e-05" iyy="0.00043720150287133953" iyz="3.730206268073148e-05" izz="0.00031828795462876227"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1756,7 +1756,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1772,12 +1772,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235465 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235326 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.000327646" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.000244632" iyz="2.0701e-05" izz="0.000155777"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1786,7 +1786,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1794,7 +1794,7 @@
   </link>
   <joint name="l_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.1979394973216095e-07"/>
+    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.197939497389039e-07"/>
     <parent link="l_ankle_1"/>
     <child link="l_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1823,7 +1823,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2315,13 +2315,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2363,13 +2363,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2411,13 +2411,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2435,13 +2435,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2471,13 +2471,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2495,13 +2495,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2519,13 +2519,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2661,24 +2661,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2696,13 +2696,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2720,238 +2720,238 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b12" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577099999999994 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.0125771 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577099999999994 -0.040407200000000004 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.0125771 -0.040407200000000004 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012575099999999985 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575099999999992 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999985 0.040407 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999992 0.040407 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013080999999999995 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000002 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013080999999999995 -0.040407200000000004 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000002 -0.040407200000000004 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01307899999999998 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013078999999999986 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.01307899999999998 0.040407 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999986 0.040407 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001630000000005 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001630000000005 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352109 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352108999999999 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352109 0.007765700000000028 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352108999999999 0.007765700000000028 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053785 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053785 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 -0.014429399999999995 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429399999999995 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412948999999999 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.034129489999999985 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412948999999999 -0.041499899999999985 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.034129489999999985 -0.041499899999999985 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999998 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043784999999999984 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999998 0.01781110000000001 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043784999999999984 0.01781110000000001 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.043785 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378499999999999 -0.014191799999999997 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785 -0.014191799999999997 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313347 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133469999999995 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313347 0.017184100000000008 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133469999999995 0.017184100000000008 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.054451799999999995 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
+      <pose>0.05445179999999999 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b9" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.054451799999999995 -0.013799100000000002 -0.0043179999999999885"/>
+    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.05445179999999999 -0.013799100000000002 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.02591510000000001 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.02591510000000001 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999994 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999994 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGenova04/model.urdf b/iCub/robots/iCubGenova04/model.urdf
index bcb3414..de1a7ce 100644
--- a/iCub/robots/iCubGenova04/model.urdf
+++ b/iCub/robots/iCubGenova04/model.urdf
@@ -23,12 +23,12 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -37,7 +37,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -199,7 +199,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.05831029999999999 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -213,7 +213,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208553999999999 0.03044339999999999 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -241,7 +241,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999998 -0.031175900000000006 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -269,21 +269,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.03575341 0.041062799999999997 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.0234419 0.047990799999999986 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.024270400000000004 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -318,14 +318,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.04774867999999998 -0.031231900000000007 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322409999999998 -0.015601800000000027 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -339,21 +339,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880468999999998 -0.03126290000000001 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999998 -0.015912900000000008 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.05701059999999998 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -374,14 +374,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641649999999984 0.017638799999999975 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113139 0.03243849999999996 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -402,7 +402,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998339999999984 0.001266399999999973 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -416,7 +416,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127099999999976 -0.03074890000000001 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -511,12 +511,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.03360009999999998 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.033600099999999994 0.005371899999999999 -0.001146999999999898" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.382866"/>
       <inertia ixx="0.00120946" ixy="3.74585e-07" ixz="-5.78759e-05" iyy="0.00141439" iyz="-7.80325e-06" izz="0.000273599"/>
     </inertial>
     <visual>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -525,7 +525,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.047209015 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04720901500000001 -0.07009999199999985 -0.6474380002860779" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -539,14 +539,14 @@
   </joint>
   <link name="r_sole"/>
   <joint name="r_sole_fixed_joint" type="fixed">
-    <origin xyz="0.003499999999999996 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
+    <origin xyz="0.003500000000000003 0 0.0040000000000000036" rpy="-3.141592653589793 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_0"/>
   <joint name="r_sole_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.06047669999999999 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.060476699999999994 -0.0254106 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_0"/>
     <dynamics damping="0.1"/>
@@ -560,14 +560,14 @@
   </joint>
   <link name="r_sole_skin_2"/>
   <joint name="r_sole_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.09320869999999998 -0.032368 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.09320869999999999 -0.032368 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_3"/>
   <joint name="r_sole_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.11158309999999996 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.11158309999999998 -0.0263978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_3"/>
     <dynamics damping="0.1"/>
@@ -609,112 +609,112 @@
   </joint>
   <link name="r_sole_skin_12"/>
   <joint name="r_sole_skin_12_fixed_joint" type="fixed">
-    <origin xyz="0.08286799999999997 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.08286799999999998 -0.0005426000000000042 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_12"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_13"/>
   <joint name="r_sole_skin_13_fixed_joint" type="fixed">
-    <origin xyz="0.10124239999999997 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.10124239999999998 0.005427600000000005 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_13"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_14"/>
   <joint name="r_sole_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05013603999999998 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.050136039999999986 0.006414799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_14"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_15"/>
   <joint name="r_sole_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.046119189999999984 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.04611918999999999 -0.012483000000000008 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_17"/>
   <joint name="r_sole_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.029685499999999997 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.02968549999999999 0.03325689999999999 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_18"/>
   <joint name="r_sole_skin_18_fixed_joint" type="fixed">
-    <origin xyz="-0.04805990000000001 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.0480599 0.027286999999999978 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_18"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_19"/>
   <joint name="r_sole_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.05207670000000001 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.052076700000000004 0.008389200000000013 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_20"/>
   <joint name="r_sole_skin_20_fixed_joint" type="fixed">
-    <origin xyz="-0.037719200000000015 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.03771920000000001 -0.004538399999999984 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_21"/>
   <joint name="r_sole_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.03577849999999998 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.03577849999999999 0.01934240000000001 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_21"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_22"/>
   <joint name="r_sole_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.02774479999999999 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.027744799999999997 -0.018453200000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_22"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_24"/>
   <joint name="r_sole_skin_24_fixed_joint" type="fixed">
-    <origin xyz="-0.004987200000000004 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.004987199999999997 -0.011495800000000007 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_24"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_25"/>
   <joint name="r_sole_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.013387199999999995 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.013387200000000002 -0.005525599999999992 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_25"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_26"/>
   <joint name="r_sole_skin_26_fixed_joint" type="fixed">
-    <origin xyz="0.017404099999999985 0.0133722 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.017404099999999992 0.0133722 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_26"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_27"/>
   <joint name="r_sole_skin_27_fixed_joint" type="fixed">
-    <origin xyz="0.0030464999999999798 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="0.0030464999999999867 0.026299799999999998 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_27"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_28"/>
   <joint name="r_sole_skin_28_fixed_joint" type="fixed">
-    <origin xyz="-0.015327899999999998 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.015327899999999992 0.020329600000000003 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_28"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_sole_skin_29"/>
   <joint name="r_sole_skin_29_fixed_joint" type="fixed">
-    <origin xyz="-0.019344800000000002 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
+    <origin xyz="-0.019344799999999995 0.001431799999999997 0.007500000000000062" rpy="0 0 0"/>
     <parent link="r_foot"/>
     <child link="r_sole_skin_29"/>
     <dynamics damping="0.1"/>
@@ -938,12 +938,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -952,15 +952,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -989,7 +989,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -1026,12 +1026,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1040,7 +1040,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1056,7 +1056,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -1077,7 +1077,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -1086,35 +1086,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1135,14 +1135,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -1170,7 +1170,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -1191,35 +1191,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1254,19 +1254,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1275,14 +1275,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -1312,7 +1312,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -1358,12 +1358,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1372,15 +1372,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -1409,7 +1409,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -1446,7 +1446,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -1476,7 +1476,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -1497,7 +1497,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -1506,35 +1506,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1555,7 +1555,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -1576,7 +1576,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -1611,35 +1611,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -1653,7 +1653,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -1674,19 +1674,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1695,14 +1695,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1732,7 +1732,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1748,12 +1748,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1762,7 +1762,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1778,12 +1778,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0011062999999999906 1.7299999999997873e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0011062999999999976 1.730000000001175e-05 -0.04457399999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.415987"/>
       <inertia ixx="0.000475741" ixy="2.89355e-07" ixz="-6.72876e-08" iyy="0.000279828" iyz="6.7417e-06" izz="0.000307828"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1792,14 +1792,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1808,12 +1808,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012289999999999524 -0.00028229999999998534 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.00012290000000000217 -0.0002822999999999992 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1822,26 +1822,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 0.07010002460000005 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 0.07010002460000003 -0.22621299999998065" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008301999999999893 7.519999999998361e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008301999999999962 7.519999999999749e-05 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868999999999982 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868999999999913 0.00010179999999998524 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1850,7 +1850,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002459999994 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1866,7 +1866,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.05125669999999999 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.051256699999999995 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1880,12 +1880,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999800000000002 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997999999999951 0.0034968999999999695 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1894,7 +1894,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999991 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1910,12 +1910,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015153099999999996 0.000902 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.015153099999999989 0.0009020000000000139 4.199999999998649e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447429734460005" ixy="-1.1322603878053663e-06" ixz="3.046651057758992e-05" iyy="0.0004478873540695352" iyz="2.5726583414130826e-06" izz="0.0003076030453774591"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1924,7 +1924,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370906139999998 -0.07010002479999987 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370906139999997 -0.07010002479999985 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1940,12 +1940,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011447000000000054 3.5999999999786203e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0011446999999999985 3.599999999992498e-06 -0.03852" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.265122"/>
       <inertia ixx="0.000327222" ixy="-4.74606e-08" ixz="4.94571e-08" iyy="0.000248285" iyz="-4.29975e-06" izz="0.000150937"/>
     </inertial>
     <visual>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1954,7 +1954,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043681699618582974 -0.07017520019076887 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04368169961858297 -0.07017520019076885 0.5871379999999998" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1991,7 +1991,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.003499999999999996 0 -0.0605" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.003500000000000003 0 -0.0605" rpy="3.141592653589793 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2658,13 +2658,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2706,13 +2706,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2754,13 +2754,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2778,13 +2778,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2814,13 +2814,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2838,13 +2838,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2862,13 +2862,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -3004,24 +3004,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351374599999999 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351374599999999 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450330799999999 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450330799999999 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -3063,13 +3063,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.0164801 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.0164801 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
@@ -3087,178 +3087,178 @@
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07847939999999998 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
+      <pose>0.07847939999999999 -0.011812200000000009 -0.001214999999999966 2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999998 -0.011812200000000009 -0.001214999999999966"/>
+    <origin rpy="2.220446049250314e-16 2.220446049250313e-16 -1.5707963267948966" xyz="0.07847939999999999 -0.011812200000000009 -0.001214999999999966"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577000000000005 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.012577000000000012 -0.04040720000000002 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000005 -0.04040720000000002 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577000000000012 -0.04040720000000002 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012574999999999996 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575000000000003 0.04040679999999998 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012574999999999996 0.04040679999999998 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575000000000003 0.04040679999999998 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01308100000000001 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000016 -0.04040720000000002 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.01308100000000001 -0.04040720000000002 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000016 -0.04040720000000002 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013078999999999993 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013079 0.04040679999999998 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999993 0.04040679999999998 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013079 0.04040679999999998 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04200169 0.03156479999999999 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.042001689999999994 0.03156479999999999 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.04200169 0.03156479999999999 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.042001689999999994 0.03156479999999999 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352114999999999 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352114999999998 0.007765700000000014 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999999 0.007765700000000014 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352114999999998 0.007765700000000014 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053784999999999986 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999998 -0.014429400000000002 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429400000000002 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999998 -0.014429400000000002 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412954999999999 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.03412954999999998 -0.0414999 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999999 -0.0414999 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412954999999998 -0.0414999 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043785 0.017810999999999994 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999999 0.017810999999999994 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043785 0.017810999999999994 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.043785000000000004 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.04378500000000001 -0.014191800000000011 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785000000000004 -0.014191800000000011 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378500000000001 -0.014191800000000011 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.043513791999999996 0.011314499999999963 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351379199999999 0.011314499999999963 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.043513791999999996 0.011314499999999963 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351379199999999 0.011314499999999963 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503353999999995 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450335399999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503353999999995 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450335399999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313352999999999 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133529999999984 0.017184099999999994 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313352999999999 0.017184099999999994 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133529999999984 0.017184099999999994 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
@@ -3276,25 +3276,25 @@
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.025915200000000003 0.04274479999999997 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915199999999996 0.04274479999999997 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.025915200000000003 0.04274479999999997 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915199999999996 0.04274479999999997 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999984 -0.041718700000000025 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999977 -0.041718700000000025 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999984 -0.041718700000000025 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999977 -0.041718700000000025 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
@@ -3368,45 +3368,45 @@
     <sensor name="l_upper_leg_ft_acc_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_acc_3b12" type="accelerometer">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_hip_2">
     <sensor name="l_upper_leg_ft_gyro_3b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.006670000000000002 -0.008524800000000013 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.006669999999999995 -0.008524799999999999 -0.059281999999999946 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ft_gyro_3b12" type="gyroscope">
     <parent link="l_hip_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006670000000000002 -0.008524800000000013 -0.059281999999999946"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.006669999999999995 -0.008524799999999999 -0.059281999999999946"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_acc_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_acc_3b13" type="accelerometer">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo reference="l_ankle_2">
     <sensor name="l_foot_ft_gyro_3b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.004000199999999995 -0.008600000000000017 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
+      <pose>0.0040001999999999885 -0.008600000000000003 -0.045282000000000044 -2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966</pose>
     </sensor>
   </gazebo>
   <sensor name="l_foot_ft_gyro_3b13" type="gyroscope">
     <parent link="l_ankle_2"/>
-    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.004000199999999995 -0.008600000000000017 -0.045282000000000044"/>
+    <origin rpy="-2.220446049250314e-16 2.220446049250313e-16 1.5707963267948966" xyz="0.0040001999999999885 -0.008600000000000003 -0.045282000000000044"/>
   </sensor>
   <gazebo>
     <pose>0.0 0.0 0.63 0.0 0.0 3.14</pose>
diff --git a/iCub/robots/iCubGenova04_plus/model.urdf b/iCub/robots/iCubGenova04_plus/model.urdf
index 612b79d..188d3d3 100644
--- a/iCub/robots/iCubGenova04_plus/model.urdf
+++ b/iCub/robots/iCubGenova04_plus/model.urdf
@@ -247,12 +247,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="-0.021414170943809915 0.00027172527255134304 -0.019440234453461297" rpy="0 0 0"/>
+      <origin xyz="-0.02141417094380993 0.0002717252725513569 -0.019440234453461283" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
     </inertial>
     <visual>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -261,15 +261,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.10658780856753214 2.3191471541783582e-07 -0.04073921211753291" rpy="0 0 0"/>
+      <origin xyz="0.10658780856753214 2.3191471536232466e-07 -0.04073921211753291" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
-    <axis xyz="-0.258819 -2.7755575615628914e-17 -0.965926"/>
+    <origin xyz="-0.007580465732273667 0 0.014210246363868029" rpy="-0.2705976115829964 -0.252680154648383 -1.5015568003498747"/>
+    <axis xyz="-0.258819 9.361836320172563e-18 -0.965926"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -298,7 +298,7 @@
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="-0.025346071323150046 2.3275042730641626e-07 -0.0053876480645656255" rpy="-0.2526895461750575 0.008518618321396994 0.06703988914196896"/>
+    <origin xyz="-0.02534607132315006 2.3275042732029405e-07 -0.005387648064565612" rpy="-0.25268954617505757 0.008518618321396991 0.06703988914196844"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
@@ -335,12 +335,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.006739129328428656 0.01880190000000001 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="-0.006739129328428656 0.018801899999999996 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13344e-05" ixy="7.36974e-06" ixz="-1.04536e-05" iyy="5.86947e-05" iyz="3.92498e-06" izz="9.68353e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -349,7 +349,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.25939046331664584 0.02249989999999996 -0.08503258869619254" rpy="0 0 0"/>
+      <origin xyz="0.25939046331664584 0.022499899999999948 -0.08503258869619254" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_elbow_prosup_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -365,7 +365,7 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865203" rpy="0 0 0"/>
+      <origin xyz="-0.04043911338861204 -0.00032799999999999496 0.005327334483865201" rpy="0 0 0"/>
       <mass value="0.6347"/>
       <inertia ixx="0.000370044" ixy="-1.65419e-06" ixz="0.00010538" iyy="0.000672279" iyz="1.1456e-06" izz="0.000553637"/>
     </inertial>
@@ -386,7 +386,7 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.0384624299026574 0.02249990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="-0.0384624299026574 0.022499900000000017 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 -2.220446049250313e-16 -0.2588189794466167"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
@@ -395,35 +395,35 @@
   </joint>
   <link name="r_forearm_skin_0"/>
   <joint name="r_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014216" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
+    <origin xyz="-0.04544932866182965 0.030076999999999993 -0.006286560379014218" rpy="1.6918152229167889 -0.27102674434071783 -3.0688043822713253"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_1"/>
   <joint name="r_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820506 -0.2744425337639908 -3.0994697798425186"/>
+    <origin xyz="-0.03337289358257035 0.02882600000000002 -0.019845686771095515" rpy="1.8669402327820501 -0.2744425337639908 -3.0994697798425186"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_2"/>
   <joint name="r_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.01601173383706034" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
+    <origin xyz="-0.014698892985226357 0.031278999999999973 -0.016011733837060345" rpy="1.7284728701922574 -0.26401521911581066 -3.127332606870116"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_3"/>
   <joint name="r_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506463" rpy="-1.694872808587699 -0.26975772710448953 3.0795974357388127"/>
+    <origin xyz="-0.04556729362200617 -0.030891000000000002 -0.006330009293506465" rpy="-1.6948728085876994 -0.2697577271044894 3.0795974357388127"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_4"/>
   <joint name="r_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659852" rpy="-1.807349601935264 -0.03931317963272516 2.9799764363205914"/>
+    <origin xyz="-0.06353263329961753 -0.02811630000000001 -0.008997842580659854" rpy="-1.8073496019352646 -0.039313179632725175 2.979976436320592"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -444,14 +444,14 @@
   </joint>
   <link name="r_forearm_skin_7"/>
   <joint name="r_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570656 -0.26399290072226816 3.127212542664466"/>
+    <origin xyz="-0.014722426007426966 -0.031942200000000004 -0.015932648270323382" rpy="-1.725646408570657 -0.26399290072226783 3.127212542664466"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_7"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_8"/>
   <joint name="r_forearm_skin_8_fixed_joint" type="fixed">
-    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.27060509051232057 3.139805572846308"/>
+    <origin xyz="-0.0228528628554271 -0.009786200000000009 -0.041885146374950645" rpy="-2.9480197913728667 -0.2706050905123207 3.139805572846308"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_8"/>
     <dynamics damping="0.1"/>
@@ -479,7 +479,7 @@
   </joint>
   <link name="r_forearm_skin_12"/>
   <joint name="r_forearm_skin_12_fixed_joint" type="fixed">
-    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697442 -3.120265803141759"/>
+    <origin xyz="-0.03759946338683661 0.01821600000000001 -0.034019117996090566" rpy="2.5830350596406384 -0.2948305020697443 -3.120265803141759"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_12"/>
     <dynamics damping="0.1"/>
@@ -500,35 +500,35 @@
   </joint>
   <link name="r_forearm_skin_15"/>
   <joint name="r_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
+    <origin xyz="-0.06337864196616988 0.027145000000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.289783281915754 -3.027658914283702"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_16"/>
   <joint name="r_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="-0.03792482186997503 0.030487000000000014 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 -3.0657274234313623"/>
+    <origin xyz="-0.03792482186997503 0.030487000000000014 0.020636950360473926" rpy="1.4128000702560035 -0.25082462138972816 -3.0657274234313614"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_17"/>
   <joint name="r_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="-0.05431705488213744 0.027725000000000027 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 -3.031955428370696"/>
+    <origin xyz="-0.05431705488213744 0.027725000000000027 0.030329124017862287" rpy="1.272237162052993 -0.2305930470302688 -3.031955428370696"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_19"/>
   <joint name="r_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263516 3.0771521788870553"/>
+    <origin xyz="-0.038126610713769264 -0.03129409999999999 0.02069060539304483" rpy="-1.4156088870322079 -0.25256929022635194 3.077152178887055"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_forearm_skin_22"/>
   <joint name="r_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850168" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
+    <origin xyz="-0.05441739504212798 -0.028722300000000006 0.030603751660850165" rpy="-1.26582427279506 -0.23432815137878316 3.0478831797095465"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -542,7 +542,7 @@
   </joint>
   <link name="r_forearm_skin_25"/>
   <joint name="r_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146758 -0.13698027219744066 3.05963182792421"/>
+    <origin xyz="-0.053599921629492835 -0.018223800000000012 0.04546981250452139" rpy="-0.5642955277146757 -0.1369802721974407 3.05963182792421"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -563,19 +563,19 @@
   </joint>
   <link name="r_forearm_dh_frame"/>
   <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="-0.26179931982715643 0 -1.5707963267948968"/>
+    <origin xyz="-0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="-0.2617993198271565 0 -1.570796326794897"/>
     <parent link="r_forearm"/>
     <child link="r_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="0.0009861019555474082 -0.0011759999999999965 0.004101327367239241" rpy="0 0 0"/>
+      <origin xyz="0.0009861019555474082 -0.0011759999999999826 0.004101327367239241" rpy="0 0 0"/>
       <mass value="0.0362615"/>
       <inertia ixx="5.57722e-06" ixy="-3.10462e-08" ixz="-9.44554e-07" iyy="5.29845e-06" iyz="-1.59685e-07" izz="1.87724e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -584,14 +584,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.40039217624844703 -7.000000000090267e-06 -0.1047483045162671" rpy="0 0 0"/>
+      <origin xyz="0.40039217624844703 -7.0000000000763896e-06 -0.1047483045162671" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_pitch" type="revolute">
-    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.025197702993554973" rpy="0 0 0"/>
+    <origin xyz="-0.1035052088730253 7.000000000020878e-06 0.02519770299355497" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 0.9659258438815124"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
@@ -621,7 +621,7 @@
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
-    <origin xyz="0.0006341064996442103 -0.019499900000000014 0.002366518317509704" rpy="0 0 0"/>
+    <origin xyz="0.0006341064996442103 -0.0194999 0.002366518317509704" rpy="0 0 0"/>
     <axis xyz="2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
@@ -667,12 +667,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.021474595127247476 0.00026895789968721384 -0.01941073615345372" rpy="0 0 0"/>
+      <origin xyz="0.02147459512724749 0.0002689578996872277 -0.019410736153453706" rpy="0 0 0"/>
       <mass value="0.238162"/>
       <inertia ixx="0.000159621" ixy="1.19701e-07" ixz="-1.29687e-05" iyy="0.000222574" iyz="1.20112e-06" izz="0.000120056"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -681,15 +681,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10658669158059955 1.108009343442351e-07 -0.04074338077098992" rpy="0 0 0"/>
+      <origin xyz="-0.10658669158059955 1.1080093428872395e-07 -0.04074338077098992" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shoulder_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
-    <axis xyz="-0.258819 2.7755575615628914e-17 0.965926"/>
+    <origin xyz="-0.013671000000000016 0 0.0085204" rpy="0.2705973179159597 0.2526801775423606 -1.5015568855866182"/>
+    <axis xyz="-0.258819 -9.361836320172563e-18 0.965926"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
@@ -718,7 +718,7 @@
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0.025314346831209797 1.1163643820477631e-07 -0.005400616562648813" rpy="-0.2526895461750575 -0.008518618321396994 -0.06703988914196896"/>
+    <origin xyz="0.025314346831209825 1.116364382186541e-07 -0.0054006165626487995" rpy="-0.25268954617505757 -0.008518618321396991 -0.06703988914196844"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
@@ -755,7 +755,7 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536202" rpy="0 0 0"/>
+      <origin xyz="0.006739129328428652 0.030301900000000007 -0.004657692361536201" rpy="0 0 0"/>
       <mass value="0.267903"/>
       <inertia ixx="8.13352e-05" ixy="-7.37159e-06" ixz="1.04505e-05" iyy="5.86823e-05" iyz="3.9319e-06" izz="9.68469e-05"/>
     </inertial>
@@ -785,7 +785,7 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791274" rpy="0 0 0"/>
+      <origin xyz="0.040451182882303864 -0.000686000000000006 0.005323632143791272" rpy="0 0 0"/>
       <mass value="0.636185"/>
       <inertia ixx="0.000371447" ixy="4.34562e-06" ixz="-0.000105871" iyy="0.000674229" iyz="4.05222e-06" izz="0.000557115"/>
     </inertial>
@@ -806,7 +806,7 @@
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="0.03846242990265739 0.03399990000000003 -0.005223168194033808" rpy="0 0 0"/>
+    <origin xyz="0.03846242990265739 0.03399990000000003 -0.0052231681940338075" rpy="0 0 0"/>
     <axis xyz="0.9659258438815126 2.220446049250313e-16 0.2588189794466167"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
@@ -815,35 +815,35 @@
   </joint>
   <link name="l_forearm_skin_0"/>
   <joint name="l_forearm_skin_0_fixed_joint" type="fixed">
-    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014216" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
+    <origin xyz="0.045449328661829665 -0.032316899999999996 -0.006286560379014218" rpy="1.6918152229167884 -0.2710267443407179 0.07278827131846853"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_0"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_1"/>
   <joint name="l_forearm_skin_1_fixed_joint" type="fixed">
-    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399064 0.04212287374727489"/>
+    <origin xyz="0.03337289358257036 -0.031066200000000016 -0.019845686771095515" rpy="1.8669402327820506 -0.27444253376399047 0.04212287374727479"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_1"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_2"/>
   <joint name="l_forearm_skin_2_fixed_joint" type="fixed">
-    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.01601173383706034" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
+    <origin xyz="0.01469889298522637 -0.033518400000000004 -0.016011733837060345" rpy="1.7284728701922578 -0.2640152191158104 0.014260046719677696"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_2"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_3"/>
   <joint name="l_forearm_skin_3_fixed_joint" type="fixed">
-    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506463" rpy="-1.6948728085876998 -0.2697577271044896 -0.061995217850980305"/>
+    <origin xyz="0.04556729362200616 0.02865100000000001 -0.006330009293506465" rpy="-1.6948728085876994 -0.26975772710448953 -0.061995217850980264"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_4"/>
   <joint name="l_forearm_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659852" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
+    <origin xyz="0.0635326332996175 0.02587700000000001 -0.008997842580659854" rpy="-1.8174787010829498 -0.28740688488516286 -0.09983161528264395"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_4"/>
     <dynamics damping="0.1"/>
@@ -864,7 +864,7 @@
   </joint>
   <link name="l_forearm_skin_7"/>
   <joint name="l_forearm_skin_7_fixed_joint" type="fixed">
-    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222682 -0.014380110925327112"/>
+    <origin xyz="0.014722426007426952 0.02970199999999998 -0.015932648270323382" rpy="-1.7256464085706567 -0.2639929007222678 -0.014380110925326664"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_7"/>
     <dynamics damping="0.1"/>
@@ -885,7 +885,7 @@
   </joint>
   <link name="l_forearm_skin_10"/>
   <joint name="l_forearm_skin_10_fixed_joint" type="fixed">
-    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.9412331340552202 -0.3283044719221903 -0.01396627819357275"/>
+    <origin xyz="0.05422852108875307 0.007546000000000011 -0.03214690828659369" rpy="-2.94123313405522 -0.3283044719221903 -0.013966278193572868"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_10"/>
     <dynamics damping="0.1"/>
@@ -920,35 +920,35 @@
   </joint>
   <link name="l_forearm_skin_15"/>
   <joint name="l_forearm_skin_15_fixed_joint" type="fixed">
-    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439964" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
+    <origin xyz="0.06337864196616988 -0.029384300000000002 -0.008794054555439966" rpy="1.8049276974140638 -0.28978328191575403 0.11393373930609194"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_15"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_16"/>
   <joint name="l_forearm_skin_16_fixed_joint" type="fixed">
-    <origin xyz="0.03792482186997504 -0.0327268 0.02063695036047393" rpy="1.4128000702560044 -0.25082462138972744 0.0758652301584316"/>
+    <origin xyz="0.03792482186997504 -0.0327268 0.020636950360473926" rpy="1.4128000702560033 -0.2508246213897282 0.07586523015843241"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_16"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_17"/>
   <joint name="l_forearm_skin_17_fixed_joint" type="fixed">
-    <origin xyz="0.05431705488213746 -0.029964599999999994 0.03032912401786229" rpy="1.2722371620529929 -0.23059304703026898 0.10963722521909765"/>
+    <origin xyz="0.05431705488213746 -0.029964599999999994 0.030329124017862287" rpy="1.272237162052993 -0.23059304703026878 0.10963722521909744"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_17"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_19"/>
   <joint name="l_forearm_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.03812661071376925 0.029053999999999996 0.020690605393044834" rpy="-1.415608887032208 -0.2525692902263517 -0.06444047470273734"/>
+    <origin xyz="0.03812661071376925 0.029053999999999996 0.02069060539304483" rpy="-1.4156088870322079 -0.252569290226352 -0.06444047470273781"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_forearm_skin_22"/>
   <joint name="l_forearm_skin_22_fixed_joint" type="fixed">
-    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850168" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
+    <origin xyz="0.05441739504212797 0.026483000000000007 0.030603751660850165" rpy="-1.2658242727950597 -0.23432815137878313 -0.09370947388024636"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_22"/>
     <dynamics damping="0.1"/>
@@ -962,7 +962,7 @@
   </joint>
   <link name="l_forearm_skin_25"/>
   <joint name="l_forearm_skin_25_fixed_joint" type="fixed">
-    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146758 -0.1369802721974407 -0.08196082566558262"/>
+    <origin xyz="0.05359992162949282 0.015984000000000026 0.04546981250452139" rpy="-0.5642955277146757 -0.13698027219744066 -0.08196082566558259"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_skin_25"/>
     <dynamics damping="0.1"/>
@@ -983,19 +983,19 @@
   </joint>
   <link name="l_forearm_dh_frame"/>
   <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064677" rpy="2.879793333762637 0 1.5707963267948968"/>
+    <origin xyz="0.1028711023733811 7.000000000020878e-06 0.027564221311064674" rpy="2.879793333762637 0 1.5707963267948968"/>
     <parent link="l_forearm"/>
     <child link="l_forearm_dh_frame"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.000981313804427647 -0.0011779999999999985 0.0040834577391274315" rpy="0 0 0"/>
+      <origin xyz="-0.000981313804427647 -0.0011779999999999846 0.004083457739127433" rpy="0 0 0"/>
       <mass value="0.0362152"/>
       <inertia ixx="5.56774e-06" ixy="3.08362e-08" ixz="9.42153e-07" iyy="5.28838e-06" iyz="-1.58924e-07" izz="1.87579e-06"/>
     </inertial>
     <visual>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1004,14 +1004,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.4003145305546131 -7.000000000090267e-06 -0.10503808226943155" rpy="0 0 0"/>
+      <origin xyz="-0.4003145305546131 -7.0000000000763896e-06 -0.10503808226943155" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_wrist_pitch_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.02548748074671943" rpy="0 0 0"/>
+    <origin xyz="0.10342756317919133 7.000000000020878e-06 0.025487480746719426" rpy="0 0 0"/>
     <axis xyz="0.2588189794466167 0.0 -0.9659258438815124"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
@@ -1041,7 +1041,7 @@
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.0006341064996442103 -0.019499900000000014 0.0023665183175097006" rpy="0 0 0"/>
+    <origin xyz="-0.0006341064996442103 -0.0194999 0.0023665183175097023" rpy="0 0 0"/>
     <axis xyz="-2.220446049250313e-16 1.0000000000000002 0.0"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
@@ -1057,12 +1057,12 @@
   </joint>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.000514500000000008 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005145000000000149 -0.038075299999999986 -7.099999999998774e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919978"/>
       <inertia ixx="0.000404268" ixy="-1.26852e-06" ixz="1.11055e-06" iyy="0.000574741" iyz="-7.69474e-07" izz="0.000580025"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1071,7 +1071,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000011214 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000011213 0.025999999999999232 0.1519129999999679" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1233,7 +1233,7 @@
   </joint>
   <link name="r_lower_leg_skin_4"/>
   <joint name="r_lower_leg_skin_4_fixed_joint" type="fixed">
-    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.9863636151870843e-07 -1.256367488755018 -2.758923800870981"/>
+    <origin xyz="0.058310299999999995 0.016783599999999996 0.007272000000000001" rpy="-2.986363594545861e-07 -1.2563674887550182 -2.7589238008709835"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_4"/>
     <dynamics damping="0.1"/>
@@ -1247,7 +1247,7 @@
   </joint>
   <link name="r_lower_leg_skin_6"/>
   <joint name="r_lower_leg_skin_6_fixed_joint" type="fixed">
-    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257605 -1.4678512341064953 0.8374928890768593"/>
+    <origin xyz="0.05208552 0.03044339999999998 -0.022148999999999974" rpy="-3.1415892795257623 -1.4678512341064949 0.8374928890768587"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_6"/>
     <dynamics damping="0.1"/>
@@ -1275,7 +1275,7 @@
   </joint>
   <link name="r_lower_leg_skin_14"/>
   <joint name="r_lower_leg_skin_14_fixed_joint" type="fixed">
-    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.141591818848249 -1.4650273563521197 -0.6540334071673023"/>
+    <origin xyz="0.05504769999999999 -0.03117600000000001 -0.02190000000000003" rpy="-3.1415918188482506 -1.46502735635212 -0.6540334071673009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_14"/>
     <dynamics damping="0.1"/>
@@ -1303,21 +1303,21 @@
   </joint>
   <link name="r_lower_leg_skin_19"/>
   <joint name="r_lower_leg_skin_19_fixed_joint" type="fixed">
-    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777174257102e-07 -1.4349869697782824 -2.1586361801672447"/>
+    <origin xyz="0.035753400000000005 0.04106279999999999 0.006064999999999987" rpy="-8.908777145556939e-07 -1.434986969778283 -2.158636180167247"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_19"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_20"/>
   <joint name="r_lower_leg_skin_20_fixed_joint" type="fixed">
-    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055654966350685e-06 -1.5001551303072314 -1.8675509241250097"/>
+    <origin xyz="0.023441900000000012 0.04799079999999998 -0.004952000000000012" rpy="-3.2055655013539e-06 -1.5001551303072311 -1.8675509241250052"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_20"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_21"/>
   <joint name="r_lower_leg_skin_21_fixed_joint" type="fixed">
-    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114955750172e-05 -1.5554551388057172 -1.8741197231626954"/>
+    <origin xyz="0.02427040000000001 0.04853779999999998 -0.024276999999999993" rpy="2.2967114952132173e-05 -1.555455138805717 -1.8741197231627162"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_21"/>
     <dynamics damping="0.1"/>
@@ -1352,14 +1352,14 @@
   </joint>
   <link name="r_lower_leg_skin_35"/>
   <joint name="r_lower_leg_skin_35_fixed_joint" type="fixed">
-    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530436 -1.517584673322529 -0.541532486468427"/>
+    <origin xyz="0.047748659999999984 -0.031231999999999996 -0.08011299999999999" rpy="-3.1415866827530343 -1.5175846733225289 -0.5415324864684377"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_35"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_36"/>
   <joint name="r_lower_leg_skin_36_fixed_joint" type="fixed">
-    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.5158726528065043 -0.20740733566009956"/>
+    <origin xyz="0.05322408999999999 -0.01560180000000004 -0.088362" rpy="-3.141590341443774 -1.515872652806505 -0.2074073356601009"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_36"/>
     <dynamics damping="0.1"/>
@@ -1373,21 +1373,21 @@
   </joint>
   <link name="r_lower_leg_skin_38"/>
   <joint name="r_lower_leg_skin_38_fixed_joint" type="fixed">
-    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890936 -1.515729955403707 -0.5421535995897704"/>
+    <origin xyz="0.04880466999999999 -0.031263 -0.060757000000000005" rpy="3.1415866519890927 -1.515729955403707 -0.5421535995897713"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_38"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_41"/>
   <joint name="r_lower_leg_skin_41_fixed_joint" type="fixed">
-    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.4068134359704096 -0.24823308941290081"/>
+    <origin xyz="0.05660089999999999 -0.01591290000000002 -0.049853999999999954" rpy="-3.1415919882511 -1.40681343597041 -0.24823308941290148"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_41"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_42"/>
   <joint name="r_lower_leg_skin_42_fixed_joint" type="fixed">
-    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.14158970239263 -1.4352475041611767 0.006879436515041273"/>
+    <origin xyz="0.057010599999999995 0.000796199999999983 -0.059079999999999966" rpy="3.141589702392633 -1.4352475041611767 0.006879436515037955"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_42"/>
     <dynamics damping="0.1"/>
@@ -1408,14 +1408,14 @@
   </joint>
   <link name="r_lower_leg_skin_47"/>
   <joint name="r_lower_leg_skin_47_fixed_joint" type="fixed">
-    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185022 -1.51690559405793 0.26264042505775587"/>
+    <origin xyz="0.052641629999999995 0.01763889999999997 -0.08886" rpy="3.141589635185034 -1.5169055940579301 0.26264042505774415"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_47"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_lower_leg_skin_49"/>
   <joint name="r_lower_leg_skin_49_fixed_joint" type="fixed">
-    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.141589347506919 -1.489961356950246 0.729763592113092"/>
+    <origin xyz="0.04113137 0.032438499999999953 -0.138859" rpy="-3.1415893475069225 -1.4899613569502461 0.7297635921130964"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_49"/>
     <dynamics damping="0.1"/>
@@ -1436,7 +1436,7 @@
   </joint>
   <link name="r_lower_leg_skin_52"/>
   <joint name="r_lower_leg_skin_52_fixed_joint" type="fixed">
-    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.1415916404202857 -1.5031867442105085 0.010871726099418466"/>
+    <origin xyz="0.052998319999999995 0.0012663999999999592 -0.11689500000000003" rpy="-3.141591640420276 -1.5031867442105085 0.010871726099408735"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_52"/>
     <dynamics damping="0.1"/>
@@ -1450,7 +1450,7 @@
   </joint>
   <link name="r_lower_leg_skin_54"/>
   <joint name="r_lower_leg_skin_54_fixed_joint" type="fixed">
-    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928216 -1.4974514557145624 -0.5751153744726872"/>
+    <origin xyz="0.045127079999999986 -0.030749000000000026 -0.11878700000000003" rpy="3.141588251928225 -1.4974514557145626 -0.5751153744726966"/>
     <parent link="r_lower_leg"/>
     <child link="r_lower_leg_skin_54"/>
     <dynamics damping="0.1"/>
@@ -1485,12 +1485,12 @@
   </joint>
   <link name="r_ankle_1">
     <inertial>
-      <origin xyz="0.015143405473968821 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015143405473968807 -0.0008685000000000082 4.2859082720503316e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6803"/>
       <inertia ixx="0.0004046207392315529" ixy="8.523488844593136e-06" ixz="-2.8913590319053794e-05" iyy="0.0004379396781535379" iyz="3.7330395179426065e-05" izz="0.00031892819418424145"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1499,7 +1499,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424511754 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424511755 0.07010000019073763 0.5784445022502984" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1515,12 +1515,12 @@
   </joint>
   <link name="r_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235187 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235048 -8.299999999988872e-06 -0.038511854107377785" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.000327646" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.000244632" iyz="2.0701e-05" izz="0.000155777"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1529,7 +1529,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442308744 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442308745 0.07010000019073363 0.5784445022026116" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1537,7 +1537,7 @@
   </link>
   <joint name="r_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.1979394973216095e-07"/>
+    <axis xyz="-1.0000001561184808 -6.013654019199066e-16 1.197939497389039e-07"/>
     <parent link="r_ankle_1"/>
     <child link="r_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1545,12 +1545,12 @@
   </joint>
   <link name="r_foot">
     <inertial>
-      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="0.039779742643594365 0.004553699994905444 -0.006311816589999797" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <mass value="0.357881"/>
       <inertia ixx="0.00109611" ixy="-2.9193e-07" ixz="-3.70109e-05" iyy="0.00125706" iyz="5.10952e-06" izz="0.000215359"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1559,14 +1559,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.8325955078225733 0 -1.5707963267948988"/>
+      <origin xyz="-0.10624279747351179 -0.07010000018839523 -0.638744845567865" rpy="-1.832595507822573 0 -1.5707963267948983"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_r_sole_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="3.141592652417863 0 0"/>
     <parent link="r_ankle_2"/>
     <child link="r_foot"/>
     <dynamics damping="0.1"/>
@@ -1580,12 +1580,12 @@
   </joint>
   <link name="l_hip_1">
     <inertial>
-      <origin xyz="-0.0005107999999999918 0.03809229999999999 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0005107999999999918 0.03809229999999998 -8.299999999999974e-05" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.919725"/>
       <inertia ixx="0.000404539" ixy="1.18502e-06" ixz="-9.64514e-07" iyy="0.000573315" iyz="-1.06028e-06" izz="0.000578982"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1594,7 +1594,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285150000005946 -0.02599999999999967 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285150000005946 -0.025999999999999673 0.15191299999998345" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_1_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1610,12 +1610,12 @@
   </joint>
   <link name="l_hip_2">
     <inertial>
-      <origin xyz="-0.0015761999999999929 -1.940000000000275e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="-0.0015761999999999998 -1.939999999998887e-05 -0.04405299999999995" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="0.421106"/>
       <inertia ixx="0.000494284" ixy="1.5483e-07" ixz="-1.86308e-07" iyy="0.000288736" iyz="1.52962e-05" izz="0.000319008"/>
     </inertial>
     <visual>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1624,14 +1624,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04285149980932398 -0.0701000000000088 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04285149980932397 -0.07010000000000878 0.15191299999997981" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_hip_2_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_hip_roll" type="revolute">
-    <origin xyz="0 0.04409999999999999 0" rpy="0 0 0"/>
+    <origin xyz="0 0.044099999999999986 0" rpy="0 0 0"/>
     <axis xyz="0.9999999999999998 0.0 2.220446049250313e-16"/>
     <parent link="l_hip_1"/>
     <child link="l_hip_2"/>
@@ -1640,12 +1640,12 @@
   </joint>
   <link name="l_hip_3">
     <inertial>
-      <origin xyz="-0.00012299999999999117 -0.0002823000000000131 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="-0.0001229999999999981 -0.000282300000000027 0.008718000000000031" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <mass value="0.126212"/>
       <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1654,26 +1654,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 0.07010000000001014 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 0.07010000000001013 -0.22621299999998032" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_upper_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_leg_ft_sensor" type="fixed">
-    <origin xyz="-0.0008574999999999972 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
+    <origin xyz="-0.0008575000000000041 0 -0.07429999999999995" rpy="3.141592653589793 0 0"/>
     <parent link="l_hip_2"/>
     <child link="l_hip_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_leg">
     <inertial>
-      <origin xyz="0.0033868000000000092 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0033868000000000023 0.00010179999999999911 -0.07891600000000001" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.8084"/>
       <inertia ixx="0.0064851267308372795" ixy="-0.00012421121423996344" ixz="-3.973994214790893e-06" iyy="0.0014625052983306384" iyz="-0.00013681806887964204" izz="0.006476192420462333"/>
     </inertial>
     <visual>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1682,7 +1682,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.043708999809320984 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980932098 -0.07010000000000993 0.24081299999999994" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_thigh_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1698,7 +1698,7 @@
   </joint>
   <link name="l_upper_leg_back_contact"/>
   <joint name="l_upper_leg_back_contact_fixed_joint" type="fixed">
-    <origin xyz="-0.051256699999999975 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
+    <origin xyz="-0.05125669999999998 0 0.014599999999999946" rpy="0 1.5707963267948963 0"/>
     <parent link="l_upper_leg"/>
     <child link="l_upper_leg_back_contact"/>
     <dynamics damping="0.1"/>
@@ -1712,12 +1712,12 @@
   </joint>
   <link name="l_lower_leg">
     <inertial>
-      <origin xyz="0.000999700000000013 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.0009997000000000061 0.0034968999999999834 -0.088567" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <mass value="1.4724"/>
       <inertia ixx="0.0042369860649861665" ixy="-0.00023961518142281364" ixz="6.76166218238396e-05" iyy="0.0015222131363604985" iyz="-0.00023314283575035712" izz="0.004570071832070392"/>
     </inertial>
     <visual>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1726,7 +1726,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.04370899980931699 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
+      <origin xyz="0.04370899980931698 -0.0701000001907579 0.38663799999999987" rpy="1.5707963267948961 0 1.5707963267948961"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_shank_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1742,12 +1742,12 @@
   </joint>
   <link name="l_ankle_1">
     <inertial>
-      <origin xyz="0.015152971856010347 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.015152971856010333 0.0009021000000000029 4.2316556559285345e-05" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.6798"/>
       <inertia ixx="0.00040447618553062084" ixy="-8.97867775831874e-06" ixz="2.9134846047326194e-05" iyy="0.00043720150287133953" iyz="3.730206268073148e-05" izz="0.00031828795462876227"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1756,7 +1756,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272424516862 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272424516864 -0.07010000019077839 0.5784445022502847" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_ankle_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1772,12 +1772,12 @@
   </joint>
   <link name="l_ankle_2">
     <inertial>
-      <origin xyz="0.0011452157753235465 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="0.0011452157753235326 -8.299999999974994e-06 -0.038511854107377674" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <mass value="0.265337"/>
       <inertia ixx="0.000327646" ixy="-7.94989e-08" ixz="8.24551e-09" iyy="0.000244632" iyz="2.0701e-05" izz="0.000155777"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1786,7 +1786,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.10974272442313177 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
+      <origin xyz="-0.10974272442313178 -0.07010000019077639 0.5784445022025997" rpy="1.3089971473150515 0 1.5707963267948972"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/2-5_plus/sim_sea_2-5_l_foot_prt-binary.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1794,7 +1794,7 @@
   </link>
   <joint name="l_ankle_roll" type="revolute">
     <origin xyz="0 0 0" rpy="0 0 0"/>
-    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.1979394973216095e-07"/>
+    <axis xyz="1.0000001561184808 6.013654019199066e-16 -1.197939497389039e-07"/>
     <parent link="l_ankle_1"/>
     <child link="l_ankle_2"/>
     <limit effort="37.0" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="7.64"/>
@@ -1823,7 +1823,7 @@
     </collision>
   </link>
   <joint name="l_foot_ft_sensor" type="fixed">
-    <origin xyz="-0.0034999280659716026 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
+    <origin xyz="-0.0034999280659716164 0 -0.060500343280208146" rpy="-3.141592643239301 0 0"/>
     <parent link="l_ankle_2"/>
     <child link="l_foot"/>
     <dynamics damping="0.1"/>
@@ -2315,13 +2315,13 @@
     <sensor name="chest_mtb_acc_0b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915177 0.11636373439232388 -1.4859735024675946</pose>
+      <pose>-0.060753999809265005 -0.0017836000000000032 0.0442341 0.8751677714915175 0.11636373439232388 -1.4859735024675942</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="chest_mtb_acc_0b7" type="accelerometer">
     <parent link="chest"/>
-    <origin rpy="0.8751677714915177 0.11636373439232388 -1.4859735024675946" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
+    <origin rpy="0.8751677714915175 0.11636373439232388 -1.4859735024675942" xyz="-0.060753999809265005 -0.0017836000000000032 0.0442341"/>
   </sensor>
   <gazebo reference="chest">
     <sensor name="chest_mtb_acc_0b8" type="imu">
@@ -2363,13 +2363,13 @@
     <sensor name="r_upper_arm_mtb_acc_2b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460495 1.5707838122500957 -2.682571611648293</pose>
+      <pose>-0.041219399999999996 0.029588000000000003 0.029527899999999982 1.5707954636460493 1.5707838122500961 -2.682571611620687</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
     <parent link="r_upper_arm"/>
-    <origin rpy="1.5707954636460495 1.5707838122500957 -2.682571611648293" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
+    <origin rpy="1.5707954636460493 1.5707838122500961 -2.682571611620687" xyz="-0.041219399999999996 0.029588000000000003 0.029527899999999982"/>
   </sensor>
   <gazebo reference="r_upper_arm">
     <sensor name="r_upper_arm_mtb_acc_2b11" type="imu">
@@ -2411,13 +2411,13 @@
     <sensor name="r_forearm_mtb_acc_2b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.07687278473420911 0.005497000000000016 -0.01104791260682077 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
+      <pose>-0.07687278473420911 0.005497000000000016 -0.011047912606820772 0.261799499979578 0.0003704130423281237 1.571258168627349</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.01104791260682077"/>
+    <origin rpy="0.261799499979578 0.0003704130423281237 1.571258168627349" xyz="-0.07687278473420911 0.005497000000000016 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="r_forearm">
     <sensor name="r_forearm_mtb_acc_2b8" type="imu">
@@ -2435,13 +2435,13 @@
     <sensor name="r_forearm_mtb_acc_2b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848174 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
+      <pose>-0.02163799567592114 0.027924000000000004 -0.013249025401848176 -1.7668262673154445 0.2611912256456084 0.054048344372003315</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
     <parent link="r_forearm"/>
-    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848174"/>
+    <origin rpy="-1.7668262673154445 0.2611912256456084 0.054048344372003315" xyz="-0.02163799567592114 0.027924000000000004 -0.013249025401848176"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b10" type="imu">
@@ -2471,13 +2471,13 @@
     <sensor name="l_upper_arm_mtb_acc_1b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.570797189926001 -1.5707838122500954 2.6825716116085734</pose>
+      <pose>-0.04121939999999998 -0.02958800000000003 0.0049920999999999854 -1.5707971899437438 -1.5707838122500957 2.682571611632433</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
     <parent link="l_upper_arm"/>
-    <origin rpy="-1.570797189926001 -1.5707838122500954 2.6825716116085734" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
+    <origin rpy="-1.5707971899437438 -1.5707838122500957 2.682571611632433" xyz="-0.04121939999999998 -0.02958800000000003 0.0049920999999999854"/>
   </sensor>
   <gazebo reference="l_upper_arm">
     <sensor name="l_upper_arm_mtb_acc_1b13" type="imu">
@@ -2495,13 +2495,13 @@
     <sensor name="l_forearm_mtb_acc_1b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.07687278473420911 -0.005482999999999988 -0.01104791260682077 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
+      <pose>0.07687278473420911 -0.005482999999999988 -0.011047912606820772 0.261799499979528 0.0003704130423280127 -1.570334484962444</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.01104791260682077"/>
+    <origin rpy="0.261799499979528 0.0003704130423280127 -1.570334484962444" xyz="0.07687278473420911 -0.005482999999999988 -0.011047912606820772"/>
   </sensor>
   <gazebo reference="l_forearm">
     <sensor name="l_forearm_mtb_acc_1b8" type="imu">
@@ -2519,13 +2519,13 @@
     <sensor name="l_forearm_mtb_acc_1b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177506 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
+      <pose>0.022099187499622495 0.025657000000000013 -0.015117424321177507 1.766821342184715 0.261099829847155 3.0875254820176163</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
     <parent link="l_forearm"/>
-    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177506"/>
+    <origin rpy="1.766821342184715 0.261099829847155 3.0875254820176163" xyz="0.022099187499622495 0.025657000000000013 -0.015117424321177507"/>
   </sensor>
   <gazebo reference="r_upper_leg">
     <sensor name="r_upper_leg_ems_acc_eb8" type="imu">
@@ -2661,24 +2661,24 @@
     <sensor name="r_lower_leg_ems_acc_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04351373 0.01383959999999998 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04351373 0.01383959999999998 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_ems_gyro_eb9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.044503292 0.014343499999999974 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
     <parent link="r_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.044503292 0.014343499999999974 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b8" type="imu">
@@ -2696,13 +2696,13 @@
     <sensor name="r_lower_leg_mtb_acc_11b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918765586 -1.5706802104434696 -1.2217307215014315</pose>
+      <pose>0.0537517 0.01572289999999999 -0.0043179999999999885 -1.5707962918786202 -1.5706802104434698 -1.221730721500941</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b9" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707962918765586 -1.5706802104434696 -1.2217307215014315" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
+    <origin rpy="-1.5707962918786202 -1.5706802104434698 -1.221730721500941" xyz="0.0537517 0.01572289999999999 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="r_lower_leg">
     <sensor name="r_lower_leg_mtb_acc_11b10" type="imu">
@@ -2720,238 +2720,238 @@
     <sensor name="r_lower_leg_mtb_acc_11b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.01648010000000001 0.04224899999999997 -0.09295800000000004 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
     <parent link="r_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.01648010000000001 0.04224899999999997 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b12" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.2617991810276566 7.344347213722464e-08 -1.5707960526999072</pose>
+      <pose>0.11154352975660221 -0.00081220000825033 -0.022848096181402044 -0.26179918102765654 7.344347216498023e-08 -1.570796052699907</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b12" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.2617991810276566 7.344347213722464e-08 -1.5707960526999072" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
+    <origin rpy="-0.26179918102765654 7.344347216498023e-08 -1.570796052699907" xyz="0.11154352975660221 -0.00081220000825033 -0.022848096181402044"/>
   </sensor>
   <gazebo reference="r_foot">
     <sensor name="r_foot_mtb_acc_11b13" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986</pose>
+      <pose>0.08739538274799673 -0.011812200008250326 -0.016377625097255266 -0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
     <parent link="r_foot"/>
-    <origin rpy="-0.26179918102766675 1.5543122344752188e-15 -1.5707963267948986" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
+    <origin rpy="-0.26179918102766664 1.6098233857064772e-15 -1.5707963267948981" xyz="0.08739538274799673 -0.011812200008250326 -0.016377625097255266"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012577099999999994 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.0125771 -0.040407200000000004 -0.08340699999999998 1.5707963267948972 1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.012577099999999994 -0.040407200000000004 -0.08340699999999998"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.0125771 -0.040407200000000004 -0.08340699999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_acc_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.012575099999999985 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.012575099999999992 0.040407 -0.040226999999999985 -1.5707963267948968 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999985 0.040407 -0.040226999999999985"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.012575099999999992 0.040407 -0.040226999999999985"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.013080999999999995 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
+      <pose>-0.013081000000000002 -0.040407200000000004 -0.087979 1.5707963267948972 1.5707963267948961 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013080999999999995 -0.040407200000000004 -0.087979"/>
+    <origin rpy="1.5707963267948972 1.5707963267948961 0.0" xyz="-0.013081000000000002 -0.040407200000000004 -0.087979"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_ems_gyro_eb10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.01307899999999998 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
+      <pose>-0.013078999999999986 0.040407 -0.035654999999999964 -1.5707963267948968 -1.5707963267948963 0.0</pose>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.01307899999999998 0.040407 -0.035654999999999964"/>
+    <origin rpy="-1.5707963267948968 -1.5707963267948963 0.0" xyz="-0.013078999999999986 0.040407 -0.035654999999999964"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b1" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.042001630000000005 0.03156500000000001 0.04913199999999998 -1.5707955185655114 -1.5706802103355728 -0.7330390947129777</pose>
+      <pose>0.04200163 0.03156500000000001 0.04913199999999998 -1.570795518564555 -1.5706802103355733 -0.733039094711841</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707955185655114 -1.5706802103355728 -0.7330390947129777" xyz="0.042001630000000005 0.03156500000000001 0.04913199999999998"/>
+    <origin rpy="-1.570795518564555 -1.5706802103355733 -0.733039094711841" xyz="0.04200163 0.03156500000000001 0.04913199999999998"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b2" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05352109 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
+      <pose>0.05352108999999999 0.007765700000000028 0.03925200000000001 -1.5707963863768455 -1.5706802103927162 -1.3089969473954748</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352109 0.007765700000000028 0.03925200000000001"/>
+    <origin rpy="-1.5707963863768455 -1.5706802103927162 -1.3089969473954748" xyz="0.05352108999999999 0.007765700000000028 0.03925200000000001"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b3" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.053785 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.05378499999999999 0.015070600000000003 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053785 0.015070600000000003 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 0.015070600000000003 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b4" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05378499999999999 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
+      <pose>0.053784999999999986 -0.014429399999999995 0.0041819999999999635 3.141592653589793 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.05378499999999999 -0.014429399999999995 0.0041819999999999635"/>
+    <origin rpy="3.141592653589793 -1.5707963267948961 0.0" xyz="0.053784999999999986 -0.014429399999999995 0.0041819999999999635"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b5" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.03412948999999999 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
+      <pose>0.034129489999999985 -0.041499899999999985 -0.004858000000000029 1.9547688910448342 -1.5707963267948961 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.03412948999999999 -0.041499899999999985 -0.004858000000000029"/>
+    <origin rpy="1.9547688910448342 -1.5707963267948961 0.0" xyz="0.034129489999999985 -0.041499899999999985 -0.004858000000000029"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b6" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999998 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
+      <pose>-0.043784999999999984 0.01781110000000001 -0.02641099999999999 -1.5707963267919405 1.5707743139371833 1.570796326789853</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.04378499999999998 0.01781110000000001 -0.02641099999999999"/>
+    <origin rpy="-1.5707963267919405 1.5707743139371833 1.570796326789853" xyz="-0.043784999999999984 0.01781110000000001 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_upper_leg">
     <sensor name="l_upper_leg_mtb_acc_10b7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>-0.04378499999999999 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
+      <pose>-0.043785 -0.014191799999999997 -0.02641099999999999 1.5707963267982468 1.5706473388946958 -1.5707963267919158</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
     <parent link="l_upper_leg"/>
-    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.04378499999999999 -0.014191799999999997 -0.02641099999999999"/>
+    <origin rpy="1.5707963267982468 1.5706473388946958 -1.5707963267919158" xyz="-0.043785 -0.014191799999999997 -0.02641099999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_acc_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.04351373 0.011314599999999966 -0.09276499999999999 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.043513729999999994 0.011314599999999966 -0.09276499999999999 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.04351373 0.011314599999999966 -0.09276499999999999"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.043513729999999994 0.011314599999999966 -0.09276499999999999"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_ems_gyro_eb7" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.044503292 0.011818499999999968 -0.08830099999999996 -3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16</pose>
+      <pose>0.04450329199999999 0.011818499999999968 -0.08830099999999996 3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15</pose>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
     <parent link="l_lower_leg"/>
-    <origin rpy="-3.1415926535897922 -1.3526297834539558 -8.476849710455595e-16" xyz="0.044503292 0.011818499999999968 -0.08830099999999996"/>
+    <origin rpy="3.1415926535897922 -1.3526297834539556 1.0015766540504487e-15" xyz="0.04450329199999999 0.011818499999999968 -0.08830099999999996"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b8" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.05313347 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
+      <pose>0.053133469999999995 0.017184100000000008 -0.0043179999999999885 -2.8972464875193467 -1.5707963267948963 0.0</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.05313347 0.017184100000000008 -0.0043179999999999885"/>
+    <origin rpy="-2.8972464875193467 -1.5707963267948963 0.0" xyz="0.053133469999999995 0.017184100000000008 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b9" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.054451799999999995 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
+      <pose>0.05445179999999999 -0.013799100000000002 -0.0043179999999999885 -1.5707963664504991 -1.5706802104435666 -1.919861927351539</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b9" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.054451799999999995 -0.013799100000000002 -0.0043179999999999885"/>
+    <origin rpy="-1.5707963664504991 -1.5706802104435666 -1.919861927351539" xyz="0.05445179999999999 -0.013799100000000002 -0.0043179999999999885"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b10" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.02591510000000001 0.042743999999999976 -0.04346800000000001 -1.5707936954714847 -1.5706802103471817 -0.26180181049562956</pose>
+      <pose>0.025915100000000003 0.042743999999999976 -0.04346800000000001 -1.5707936954748312 -1.5706802103471822 -0.26180181049683376</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="-1.5707936954714847 -1.5706802103471817 -0.26180181049562956" xyz="0.02591510000000001 0.042743999999999976 -0.04346800000000001"/>
+    <origin rpy="-1.5707936954748312 -1.5706802103471822 -0.26180181049683376" xyz="0.025915100000000003 0.042743999999999976 -0.04346800000000001"/>
   </sensor>
   <gazebo reference="l_lower_leg">
     <sensor name="l_lower_leg_mtb_acc_10b11" type="imu">
       <always_on>1</always_on>
       <update_rate>100</update_rate>
-      <pose>0.018459299999999994 -0.04171870000000001 -0.09295800000000004 1.5707953192782331 -1.5707699978643246 0.26180018702487956</pose>
+      <pose>0.018459299999999988 -0.04171870000000001 -0.09295800000000004 1.570795319271908 -1.5707699978643241 0.2618001870286602</pose>
       <plugin name="iCub_yarp_gazebo_plugin_ACC" filename="libgazebo_yarp_inertial.so"/>
     </sensor>
   </gazebo>
   <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
     <parent link="l_lower_leg"/>
-    <origin rpy="1.5707953192782331 -1.5707699978643246 0.26180018702487956" xyz="0.018459299999999994 -0.04171870000000001 -0.09295800000000004"/>
+    <origin rpy="1.570795319271908 -1.5707699978643241 0.2618001870286602" xyz="0.018459299999999988 -0.04171870000000001 -0.09295800000000004"/>
   </sensor>
   <gazebo reference="l_foot">
     <sensor name="l_foot_mtb_acc_10b12" type="imu">
diff --git a/iCub/robots/iCubGenova09/model.urdf b/iCub/robots/iCubGenova09/model.urdf
index 6eedab8..9498b76 100644
--- a/iCub/robots/iCubGenova09/model.urdf
+++ b/iCub/robots/iCubGenova09/model.urdf
@@ -816,7 +816,7 @@
   </link>
   <joint name="r_shoulder_pitch" type="revolute">
     <origin xyz="-0.020835100000000002 -0.0397423 0.1052029" rpy="-0.25326164431680925 2.3014774394592079e-07 0.26179970938509745"/>
-    <axis xyz="1.499044390054707e-07 0.9999999783534812 -1.2401537036188515e-07"/>
+    <axis xyz="1.499044390098182e-07 0.9999999783534812 -1.2401537036323317e-07"/>
     <parent link="chest"/>
     <child link="r_shoulder_1"/>
     <limit effort="50000" lower="-1.53588974175501" upper="0.22689280275926285" velocity="120.0"/>
@@ -824,12 +824,12 @@
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <origin xyz="0.0007118441514370015 -0.0002899713150672778 -0.027865575116629175" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.000711844151437005 -0.00028997131506725005 -0.027865575116629175" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.304658373"/>
       <inertia ixx="0.00029297617341568824" ixy="3.2537838826012804e-05" ixz="2.26161974125395e-05" iyy="0.00020398452123901825" iyz="-5.163216327773138e-05" izz="0.00021005828141797307"/>
     </inertial>
     <visual>
-      <origin xyz="0.023893261066846312 0.14153171676199225 -0.0072523502076727205" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.023893261066846316 0.14153171676199228 -0.00725235020767272" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -838,15 +838,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.023893261066846312 0.14153171676199225 -0.0072523502076727205" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.023893261066846316 0.14153171676199228 -0.00725235020767272" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_roll" type="revolute">
-    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069357 -0.053747792579130614 -6.132332688812785e-07"/>
-    <axis xyz="-0.9999998262417886 4.2105014356162707e-08 2.4630481189336084e-08"/>
+    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069352 -0.05374779257913059 -6.132332688804098e-07"/>
+    <axis xyz="-0.9999998262417886 4.210501434612913e-08 2.4630481182686846e-08"/>
     <parent link="r_shoulder_1"/>
     <child link="r_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.8448866807507573" velocity="120.0"/>
@@ -854,12 +854,12 @@
   </joint>
   <link name="r_shoulder_3">
     <inertial>
-      <origin xyz="-8.928496484206933e-05 -3.376971940446949e-05 -0.00759450025288725" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-8.928496484206933e-05 -3.376971940446949e-05 -0.007594500252887257" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.24467077"/>
       <inertia ixx="6.812995999935833e-05" ixy="5.557727008657165e-07" ixz="-2.7194783592437544e-06" iyy="6.835543250434674e-05" iyz="-3.2422643569677497e-06" izz="0.00011328101827807606"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743207452236198 0.1415314977120317 0.04704763137021827" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743207452236198 0.1415314977120317 0.047047631370218265" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -868,26 +868,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743207452236198 0.1415314977120317 0.04704763137021827" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743207452236198 0.1415314977120317 0.047047631370218265" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_arm_ft_sensor" type="fixed">
-    <origin xyz="-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099" rpy="0 0 0"/>
+    <origin xyz="-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099" rpy="0 0 0"/>
     <parent link="r_shoulder_2"/>
     <child link="r_shoulder_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_upper_arm">
     <inertial>
-      <origin xyz="-0.000798982178358984 -0.002084481892803941 -0.05063856093223067" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-0.0007989821783589805 -0.002084481892803941 -0.05063856093223067" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="1.960447492"/>
       <inertia ixx="0.004119602086814579" ixy="1.158223128604559e-05" ixz="0.00020349650199902935" iyy="0.003957940948860767" iyz="0.00020664971228357863" izz="0.0020250627669371347"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743125543818577 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03074312554381858 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -896,15 +896,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743125543818577 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03074312554381858 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_shoulder_yaw" type="revolute">
-    <origin xyz="0 4.2183273077367467e-07 -0.022499941373153573" rpy="0 0 0"/>
-    <axis xyz="-3.787621790513063e-08 2.805197706068352e-08 1.0000002566182309"/>
+    <origin xyz="0 4.2183273077367467e-07 -0.02249994137315358" rpy="0 0 0"/>
+    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
     <parent link="r_shoulder_3"/>
     <child link="r_upper_arm"/>
     <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
@@ -912,12 +912,12 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <origin xyz="-0.010252765750166596 -0.0018331179988757829 -0.015053741795128806" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <origin xyz="-0.010252765750166596 -0.0018331179988758106 -0.015053741795128806" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <mass value="0.217609341"/>
       <inertia ixx="0.0001036369617336083" ixy="2.353974571197465e-08" ixz="1.9513680659329553e-05" iyy="0.00011767792153336144" iyz="-6.334018264294134e-06" izz="4.556653686435985e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.020743191776066316 0.13914061722125232 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <origin xyz="0.020743191776066316 0.1391406172212523 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -926,15 +926,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.020743191776066316 0.13914061722125232 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <origin xyz="0.020743191776066316 0.1391406172212523 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_elbow" type="revolute">
-    <origin xyz="0.009999931922603816 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
-    <axis xyz="2.4949582809984105e-07 -1.0000002733514362 2.7942487004772865e-08"/>
+    <origin xyz="0.00999993192260382 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
+    <axis xyz="2.4949582807394525e-07 -1.0000002733514362 2.7942487010105877e-08"/>
     <parent link="r_upper_arm"/>
     <child link="r_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -963,8 +963,8 @@
     </collision>
   </link>
   <joint name="r_wrist_prosup" type="revolute">
-    <origin xyz="-0.014999869468812953 -0.001999649905318951 -0.05530006367251966" rpy="0 0 0"/>
-    <axis xyz="-3.787621790513063e-08 2.805197706068352e-08 1.0000002566182309"/>
+    <origin xyz="-0.014999869468812953 -0.0019996499053189787 -0.05530006367251966" rpy="0 0 0"/>
+    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
     <parent link="r_elbow_1"/>
     <child link="r_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -972,12 +972,12 @@
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <origin xyz="-0.014389827079018234 0.0005092753390522065 -9.787315764231419e-05" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="-0.014389827079018234 0.0005092753390521787 -9.78731576423697e-05" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.032535096"/>
       <inertia ixx="4.8400453637243645e-06" ixy="8.699001601283703e-07" ixz="1.3955745499815409e-08" iyy="1.5212679049734742e-06" iyz="-1.0544717252156218e-07" izz="5.2825824422365954e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.01944310973607847 0.14403852854340193 0.3560480536175482" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.01944310973607847 0.1440385285434019 0.35604805361754815" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -986,7 +986,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.01944310973607847 0.14403852854340193 0.3560480536175482" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.01944310973607847 0.1440385285434019 0.35604805361754815" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -994,7 +994,7 @@
   </link>
   <joint name="r_wrist_pitch" type="revolute">
     <origin xyz="0.016299953353948633 -0.002007442890811839 -0.10750007971210637" rpy="0 0 0"/>
-    <axis xyz="0.9999998262417886 -4.2105014356162707e-08 -2.4630481189336084e-08"/>
+    <axis xyz="0.9999998262417886 -4.210501434612913e-08 -2.4630481182686846e-08"/>
     <parent link="r_forearm"/>
     <child link="r_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -1002,12 +1002,12 @@
   </joint>
   <link name="r_hand">
     <inertial>
-      <origin xyz="0.004828314385249662 0.00615667237451184 -0.0671378028889843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.004828314385249669 0.00615667237451184 -0.0671378028889843" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <mass value="0.2499009"/>
       <inertia ixx="0.00040551381372824335" ixy="4.527703539619043e-05" ixz="1.0511331468536763e-05" iyy="0.0003291983694662585" iyz="1.0826179610148775e-05" izz="0.0001528748981981539"/>
     </inertial>
     <visual>
-      <origin xyz="0.03574323261035122 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03574323261035123 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1016,27 +1016,27 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03574323261035122 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03574323261035123 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="continuous">
-    <origin xyz="-0.016300122874272734 0.0069993900510918194 0" rpy="0 0 0"/>
-    <axis xyz="-2.4949582809984105e-07 1.0000002733514362 -2.7942487004772865e-08"/>
+    <origin xyz="-0.016300122874272734 0.006999390051091792 0" rpy="0 0 0"/>
+    <axis xyz="-2.4949582807394525e-07 1.0000002733514362 -2.7942487010105877e-08"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_shoulder_1">
     <inertial>
-      <origin xyz="0.0002918276617865645 0.05723090680782866 0.0005026028232598713" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <origin xyz="0.0002918276617865645 0.05723090680782866 0.0005026028232598678" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
       <mass value="1.376687449"/>
       <inertia ixx="0.0010357832356805553" ixy="0.0005787105562276275" ixz="0.0005349240605581856" iyy="0.0029031098267078225" iyz="-0.00015089444754685564" izz="0.00291252027731825"/>
     </inertial>
     <visual>
-      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677896" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677893" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1045,7 +1045,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677896" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677893" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1053,7 +1053,7 @@
   </link>
   <joint name="l_shoulder_pitch" type="revolute">
     <origin xyz="-0.020835100000000002 0.0397423 0.1052029" rpy="0.25326162380993705 -1.1977334179319483e-07 -0.26179930184968747"/>
-    <axis xyz="3.323079797673039e-07 0.9999999783534342 1.4452224303407313e-07"/>
+    <axis xyz="3.323079797554136e-07 0.9999999783534342 1.4452224303420037e-07"/>
     <parent link="chest"/>
     <child link="l_shoulder_1"/>
     <limit effort="50000" lower="-1.53588974175501" upper="0.22689280275926285" velocity="120.0"/>
@@ -1061,12 +1061,12 @@
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <origin xyz="0.0032933620254868984 0.0002563804707528261 -0.027865685757097337" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.003293362025486902 0.0002563804707528261 -0.027865685757097337" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.304658373"/>
       <inertia ixx="0.0002941725774372357" ixy="-3.313235231845923e-05" ixz="-2.3110931755362807e-05" iyy="0.00020431270479275498" iyz="-5.228966531392095e-05" izz="0.00021152233946173234"/>
     </inertial>
     <visual>
-      <origin xyz="0.0263930978691934 -0.14153112610649202 -0.007252397644601654" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.02639309786919337 -0.14153112610649204 -0.00725239764460165" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1075,15 +1075,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.0263930978691934 -0.14153112610649202 -0.007252397644601654" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.02639309786919337 -0.14153112610649204 -0.00725239764460165" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_roll" type="revolute">
-    <origin xyz="0.004992849331690818 0.111499838456099 0.0002686502277366859" rpy="-0.17453296140985 -0.05374759931933852 0"/>
-    <axis xyz="0.9999998262417684 2.0348252649342313e-07 -4.0230228454118944e-08"/>
+    <origin xyz="0.004992849331690818 0.111499838456099 0.00026865022773668243" rpy="-0.17453296140985003 -0.05374759931933849 0"/>
+    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
     <parent link="l_shoulder_1"/>
     <child link="l_shoulder_2"/>
     <limit effort="50000" lower="0.20943951023931956" upper="2.8448866807507573" velocity="120.0"/>
@@ -1091,12 +1091,12 @@
   </joint>
   <link name="l_shoulder_3">
     <inertial>
-      <origin xyz="-4.41626402200232e-06 9.587749727052453e-05 -0.007326213473011087" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-4.416264022005789e-06 9.587749727052453e-05 -0.007326213473011087" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.242239185"/>
       <inertia ixx="6.718030833348364e-05" ixy="-3.5592877751030733e-07" ixz="2.772905125990325e-06" iyy="6.678157004483109e-05" iyz="-3.332197996904868e-06" izz="0.00011344369128865253"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743231026171724 -0.14153148936381538 0.04704764107946005" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.03074323102617169 -0.14153148936381538 0.04704764107946006" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1105,26 +1105,26 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743231026171724 -0.14153148936381538 0.04704764107946005" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.03074323102617169 -0.14153148936381538 0.04704764107946006" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_arm_ft_sensor" type="fixed">
-    <origin xyz="-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172" rpy="0 0 0"/>
+    <origin xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="0 0 0"/>
     <parent link="l_shoulder_2"/>
     <child link="l_shoulder_3"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_arm">
     <inertial>
-      <origin xyz="0.007262813975630954 -0.026319886762204744 -0.04875860499850361" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.0072628139756309475 -0.02631988676220473 -0.04875860499850361" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="1.968325789"/>
       <inertia ixx="0.004152600139586394" ixy="0.0002114343826153783" ixz="-0.00027715810460443645" iyy="0.02016881822599814" iyz="0.00020013900456156893" izz="0.018214281820907084"/>
     </inertial>
     <visual>
-      <origin xyz="0.03074314946867922 -0.1415310659113312 0.06954758245258467" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.030743149468679187 -0.1415310659113312 0.06954758245258467" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1133,15 +1133,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03074314946867922 -0.1415310659113312 0.06954758245258467" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.030743149468679187 -0.1415310659113312 0.06954758245258467" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_shoulder_yaw" type="revolute">
-    <origin xyz="0 -4.2345248449371375e-07 -0.02249994137312443" rpy="0 0 0"/>
-    <axis xyz="2.227647404040578e-08 -4.393786245948661e-08 -1.0000002566182309"/>
+    <origin xyz="0 -4.2345248449371375e-07 -0.022499941373124423" rpy="0 0 0"/>
+    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
     <parent link="l_shoulder_3"/>
     <child link="l_upper_arm"/>
     <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
@@ -1149,12 +1149,12 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <origin xyz="-0.010241946078109292 0.0018224069895867168 -0.01508962025855548" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-0.010241946078109288 0.0018224069895867168 -0.015089620258555453" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.217768231"/>
       <inertia ixx="0.00011018266310900137" ixy="7.104417335699923e-07" ixz="-9.337153146297134e-06" iyy="0.00011868517076007366" iyz="-9.773438661251487e-06" izz="4.151767865601075e-05"/>
     </inertial>
     <visual>
-      <origin xyz="0.0207432192337597 -0.14003141848790235 0.19324791999016847" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.020743219233759668 -0.14003141848790235 0.1932479199901685" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1163,7 +1163,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.0207432192337597 -0.14003141848790235 0.19324791999016847" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.020743219233759668 -0.14003141848790235 0.1932479199901685" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1171,7 +1171,7 @@
   </link>
   <joint name="l_elbow" type="revolute">
     <origin xyz="0.009999930234919404 -0.001499647423428896 -0.12370033753758375" rpy="0 0 0"/>
-    <axis xyz="-8.811824154381087e-08 -1.0000002733514628 4.4047361189014644e-08"/>
+    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
     <parent link="l_upper_arm"/>
     <child link="l_elbow_1"/>
     <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
@@ -1179,12 +1179,12 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <origin xyz="-0.006269874102298061 -4.68551245806359e-05 -0.030648347755607153" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-0.0062698741022980675 -4.68551245806359e-05 -0.030648347755607208" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.858696525"/>
       <inertia ixx="0.001246738111071889" ixy="-6.996239702553336e-07" ixz="-2.5468935337559064e-05" iyy="0.0012148276986463716" iyz="0.00010266470596331766" izz="0.0006118920888855694"/>
     </inertial>
     <visual>
-      <origin xyz="0.03574308988793752 -0.14203106199153712 0.24854798357264754" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035743089887937486 -0.14203106199153712 0.24854798357264754" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1193,15 +1193,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03574308988793752 -0.14203106199153712 0.24854798357264754" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035743089887937486 -0.14203106199153712 0.24854798357264754" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_prosup" type="revolute">
-    <origin xyz="-0.014999870654177704 0.0019996435036348514 -0.055300063582479825" rpy="0 0 0"/>
-    <axis xyz="2.227647404040578e-08 -4.393786245948661e-08 -1.0000002566182309"/>
+    <origin xyz="-0.0149998706541777 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
+    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
     <parent link="l_elbow_1"/>
     <child link="l_forearm"/>
     <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
@@ -1209,12 +1209,12 @@
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <origin xyz="-0.014390150453737368 -0.0005083960996520587 -9.893035257668714e-05" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="-0.014390150453737368 -0.0005083960996520864 -9.893035257668714e-05" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.032535096"/>
       <inertia ixx="4.8400453637243645e-06" ixy="-8.699001601283703e-07" ixz="-1.395584007720818e-08" iyy="1.5212679049734742e-06" iyz="-1.0544717252156218e-07" izz="5.2825824422365954e-06"/>
     </inertial>
     <visual>
-      <origin xyz="0.019143073953651363 -0.14403795720960977 0.3560476247151885" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.019143073953651328 -0.1440379572096098 0.3560476247151885" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1223,15 +1223,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.019143073953651363 -0.14403795720960977 0.3560476247151885" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.019143073953651328 -0.1440379572096098 0.3560476247151885" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_pitch" type="revolute">
-    <origin xyz="0.016600015934286182 0.002006895218072541 -0.10749964114254112" rpy="0 0 0"/>
-    <axis xyz="-0.9999998262417684 -2.0348252649342313e-07 4.0230228454118944e-08"/>
+    <origin xyz="0.016600015934286182 0.0020068952180725685 -0.10749964114254112" rpy="0 0 0"/>
+    <axis xyz="-0.9999998262417685 -2.0348252625938444e-07 4.023022846062156e-08"/>
     <parent link="l_forearm"/>
     <child link="l_wrist_1"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
@@ -1239,12 +1239,12 @@
   </joint>
   <link name="l_hand">
     <inertial>
-      <origin xyz="0.004786523181334722 -0.0061337378723908265 -0.0672770617484692" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.004786523181334726 -0.0061337378723908265 -0.0672770617484692" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <mass value="0.249250207"/>
       <inertia ixx="0.00040424931311404676" ixy="-4.512230668414326e-05" ixz="-9.754806880214843e-06" iyy="0.0003277645754827484" iyz="1.1339356520717838e-05" izz="0.00015247010662812995"/>
     </inertial>
     <visual>
-      <origin xyz="0.03544319569838081 -0.13703856452803848 0.35604770325391955" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035443195698380774 -0.13703856452803848 0.35604770325391955" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1253,15 +1253,15 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03544319569838081 -0.13703856452803848 0.35604770325391955" rpy="-0.07156449498805939 -0.06196998841864677 -1.2953574498236577"/>
+      <origin xyz="0.035443195698380774 -0.13703856452803848 0.35604770325391955" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
     </collision>
   </link>
   <joint name="l_wrist_yaw" type="revolute">
-    <origin xyz="-0.016300121744729387 -0.0069993926815711816 0" rpy="0 0 0"/>
-    <axis xyz="8.811824154381087e-08 1.0000002733514628 -4.4047361189014644e-08"/>
+    <origin xyz="-0.016300121744729387 -0.006999392681571209 0" rpy="0 0 0"/>
+    <axis xyz="8.811824174789715e-08 1.0000002733514628 -4.404736120498342e-08"/>
     <parent link="l_wrist_1"/>
     <child link="l_hand"/>
     <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
@@ -1524,7 +1524,7 @@
         <frame>sensor</frame>
         <measure_direction>child_to_parent</measure_direction>
       </force_torque>
-      <pose>-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172 3.141592646862139 -3.104683320142575e-08 -1.5707962571980942</pose>
+      <pose>-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172 3.141592646862139 -3.10468332187885e-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>
@@ -1536,7 +1536,7 @@
       <frame>sensor</frame>
       <measure_direction>child_to_parent</measure_direction>
     </force_torque>
-    <origin rpy="3.141592646862139 -3.104683320142575e-08 -1.5707962571980942" xyz="-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172"/>
+    <origin rpy="3.141592646862139 -3.10468332187885e-08 -1.5707962571980942" xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172"/>
   </sensor>
   <gazebo reference="r_arm_ft_sensor">
     <sensor name="r_arm_ft_sensor" type="force_torque">
@@ -1546,7 +1546,7 @@
         <frame>sensor</frame>
         <measure_direction>child_to_parent</measure_direction>
       </force_torque>
-      <pose>-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099 3.141592644717707 4.094299285979593e-08 1.5707964185756365</pose>
+      <pose>-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099 3.141592644717707 4.094299285979593e-08 1.5707964185756365</pose>
       <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_arm_ft.ini</yarpConfigurationFile>
 </plugin>
@@ -1558,7 +1558,7 @@
       <frame>sensor</frame>
       <measure_direction>child_to_parent</measure_direction>
     </force_torque>
-    <origin rpy="3.141592644717707 4.094299285979593e-08 1.5707964185756365" xyz="-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099"/>
+    <origin rpy="3.141592644717707 4.094299285979593e-08 1.5707964185756365" xyz="-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099"/>
   </sensor>
   <gazebo reference="l_foot_front_ft_sensor">
     <sensor name="l_foot_front_ft_sensor" type="force_torque">
diff --git a/iCub/robots/left_hand_mk3/model.urdf b/iCub/robots/left_hand_mk3/model.urdf
index 7b48eae..c60aa07 100644
--- a/iCub/robots/left_hand_mk3/model.urdf
+++ b/iCub/robots/left_hand_mk3/model.urdf
@@ -75,7 +75,7 @@
   </link>
   <joint name="l_hand_index_proximal_joint" type="revolute">
     <origin xyz="0.030713454915364542 0.009192528035413853 0.002499999999999999" rpy="0 0 0"/>
-    <axis xyz="-1.2968763118959004e-08 -1.0000002970337007 0.0"/>
+    <axis xyz="-1.2968763121522752e-08 -1.0000002970337007 0.0"/>
     <parent link="l_hand_index_proximal_link"/>
     <child link="l_hand_index_medial_link"/>
     <limit effort="50000" lower="0.0" upper="1.6580627893946132" velocity="50000"/>
@@ -105,7 +105,7 @@
   </link>
   <joint name="l_hand_index_distal_joint" type="revolute">
     <origin xyz="0.0299999616287908 0 0.0014999999999999996" rpy="0 0 0"/>
-    <axis xyz="-1.2968763118959004e-08 -1.0000002970337007 0.0"/>
+    <axis xyz="-1.2968763121522752e-08 -1.0000002970337007 0.0"/>
     <parent link="l_hand_index_medial_link"/>
     <child link="l_hand_index_fingertip_link"/>
     <limit effort="50000" lower="0.0" upper="1.837831702350029" velocity="50000"/>
@@ -135,7 +135,7 @@
   </link>
   <joint name="l_hand_pinky_proximal_joint" type="revolute">
     <origin xyz="0.00924221 0.0323841 0.00310435" rpy="3.071779488995382 0 0.0872664183029469"/>
-    <axis xyz="-1.0351589968090255e-08 -0.9999999809599038 3.313676352756367e-08"/>
+    <axis xyz="-1.0351589974869193e-08 -0.9999999809599038 3.3136763531130714e-08"/>
     <parent link="l_hand_palm_link"/>
     <child link="l_hand_pinky_medial_link"/>
     <limit effort="50000" lower="0.0" upper="1.6580627893946132" velocity="50000"/>
@@ -165,7 +165,7 @@
   </link>
   <joint name="l_hand_pinky_distal_joint" type="revolute">
     <origin xyz="0.029999983211558306 0.007999988365394325 0.0015000049599501373" rpy="0 0 0"/>
-    <axis xyz="-1.0351589968090255e-08 -0.9999999809599038 3.313676352756367e-08"/>
+    <axis xyz="-1.0351589974869193e-08 -0.9999999809599038 3.3136763531130714e-08"/>
     <parent link="l_hand_pinky_medial_link"/>
     <child link="l_hand_pinky_fingertip_link"/>
     <limit effort="50000" lower="0.0" upper="1.837831702350029" velocity="50000"/>
@@ -203,7 +203,7 @@
   </joint>
   <link name="l_hand_thumb_proximal_link">
     <inertial>
-      <origin xyz="0.007942237973736326 -5.9058138708734456e-05 -0.001045447354816556" rpy="0 0 0"/>
+      <origin xyz="0.007942237973736329 -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.9198619241239645 0.17453290528623008 -1.4311699319115785"/>
-    <axis xyz="1.0000003237804127 -2.554693537271113e-07 6.571824259088421e-08"/>
+    <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"/>
     <parent link="l_hand_metacarpus_link"/>
     <child link="l_hand_thumb_proximal_link"/>
     <limit effort="50000" lower="-1.2217304763960306" upper="0.0" velocity="50000"/>
@@ -233,12 +233,12 @@
   </joint>
   <link name="l_hand_thumb_medial_link">
     <inertial>
-      <origin xyz="0.009644287345556363 1.4222829970995943e-05 -0.0047316267826139" rpy="0 0 0"/>
+      <origin xyz="0.009644287345556363 1.4222829970995943e-05 -0.004731626782613893" rpy="0 0 0"/>
       <mass value="0.00963921"/>
       <inertia ixx="0.01" ixy="1.25397e-09" ixz="-1.8712e-08" iyy="0.01" iyz="2.42496e-10" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.0029999603842260775 0 0.004499998155619252" rpy="0 0 0"/>
+      <origin xyz="-0.0029999603842260775 0 0.004499998155619259" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_l_hand_proximal_thumb_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -247,7 +247,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.0029999603842260775 0 0.004499998155619252" rpy="0 0 0"/>
+      <origin xyz="-0.0029999603842260775 0 0.004499998155619259" rpy="0 0 0"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_l_hand_proximal_thumb_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -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.628557915773655e-08 -1.0000002854169352 -2.1835508223766453e-07"/>
+    <axis xyz="2.628557918787494e-08 -1.0000002854169352 -2.183550824779987e-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.010517869126825605 8.912381595133773e-06 -0.003989284736807201" rpy="0 0 0"/>
+      <origin xyz="0.010517869126825619 8.912381595140711e-06 -0.003989284736807208" 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.0029999461796957824 0 0.0030000193982093193" rpy="0 0 0"/>
+      <origin xyz="-0.0029999461796957755 0 0.0030000193982093123" 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.0029999461796957824 0 0.0030000193982093193" rpy="0 0 0"/>
+      <origin xyz="-0.0029999461796957755 0 0.0030000193982093123" 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.020000016253208942 0 0.001499998093791144" rpy="0 0 0"/>
-    <axis xyz="2.628557915773655e-08 -1.0000002854169352 -2.1835508223766453e-07"/>
+    <origin xyz="0.020000016253208935 0 0.0014999980937911578" rpy="0 0 0"/>
+    <axis xyz="2.628557918787494e-08 -1.0000002854169352 -2.183550824779987e-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"/>