From 97b56628a37387cfb19ad94abb8d06ec5c5f17e8 Mon Sep 17 00:00:00 2001
From: Mohamed Elobaid <36491318+mebbaid@users.noreply.github.com>
Date: Tue, 13 Dec 2022 15:25:49 +0100
Subject: [PATCH] Fix minor issues in `iCubGazeboV3_visuomanip` (#176)

---
 .../gazebo_icub_left_depth_camera.ini         |   2 +-
 .../gazebo_icub_left_rgb_camera.ini           |   7 +
 .../gazebo_icub_right_depth_camera.ini        |  11 +
 .../gazebo_icub_right_rgb_camera.ini          |   2 +-
 .../robots/iCubGazeboV3_visuomanip/model.urdf | 298 ++++++++----------
 5 files changed, 159 insertions(+), 161 deletions(-)
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_rgb_camera.ini
 create mode 100644 iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_depth_camera.ini

diff --git a/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_depth_camera.ini b/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_depth_camera.ini
index 9a157a7..2c51540 100644
--- a/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_depth_camera.ini
+++ b/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_depth_camera.ini
@@ -1,6 +1,6 @@
 [include "gazebo_icub_robotname.ini"]
 
-device RGBDSensorWrapper
+device RGBDSensor_nws_yarp
 period 30
 name /${gazeboYarpPluginsRobotName}/cam/left
 
diff --git a/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_rgb_camera.ini b/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_rgb_camera.ini
new file mode 100644
index 0000000..2d0f7f3
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_rgb_camera.ini
@@ -0,0 +1,7 @@
+[include "gazebo_icub_robotname.ini"]
+
+device frameGrabber_nws_yarp
+subdevice gazebo_camera
+name /${gazeboYarpPluginsRobotName}/cam/left/rgbImage:o
+freq 30
+stamp 1
diff --git a/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_depth_camera.ini b/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_depth_camera.ini
new file mode 100644
index 0000000..635197d
--- /dev/null
+++ b/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_depth_camera.ini
@@ -0,0 +1,11 @@
+[include "gazebo_icub_robotname.ini"]
+
+device RGBDSensor_nws_yarp
+period 30
+name /${gazeboYarpPluginsRobotName}/cam/right
+
+[CAMERA_PARAM]
+focalLengthX 343.12110728152936
+focalLengthY 343.12110728152936
+tangentialPointX 0.0
+tangentialPointY 0.0
diff --git a/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_rgb_camera.ini b/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_rgb_camera.ini
index c87250b..22284fc 100644
--- a/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_rgb_camera.ini
+++ b/iCub_manual/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_rgb_camera.ini
@@ -1,6 +1,6 @@
 [include "gazebo_icub_robotname.ini"]
 
-device grabberDual
+device frameGrabber_nws_yarp
 subdevice gazebo_camera
 name /${gazeboYarpPluginsRobotName}/cam/right/rgbImage:o
 freq 30
diff --git a/iCub_manual/robots/iCubGazeboV3_visuomanip/model.urdf b/iCub_manual/robots/iCubGazeboV3_visuomanip/model.urdf
index 9c50d8a..9e32baf 100644
--- a/iCub_manual/robots/iCubGazeboV3_visuomanip/model.urdf
+++ b/iCub_manual/robots/iCubGazeboV3_visuomanip/model.urdf
@@ -1,9 +1,9 @@
 <robot name="iCub">
   <link name="root_link">
     <inertial>
-      <origin xyz="0.00317113 -0.00014949699999999998 0.025427000000000033" rpy="0 0 -1.5707963267948966"/>
-      <mass value="2.52649319"/>
-      <inertia ixx="0.004052537258277314" ixy="-2.0916736253220285e-06" ixz="-1.1533334776339305e-05" iyy="0.004338170443113407" iyz="0.0001520237016342299" izz="0.0023554026859211635"/>
+      <origin xyz="0.0035506 -0.00014959200000000001 0.024519000000000013" rpy="0 0 -1.5707963267948966"/>
+      <mass value="2.5751"/>
+      <inertia ixx="0.004051489764137304" ixy="-2.0525354991945143e-06" ixz="-1.200186769383287e-05" iyy="0.004466379288694287" iyz="0.00011702292411912923" izz="0.002537512875376926"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0.313858" rpy="0 0 -1.5707963267948966"/>
@@ -23,9 +23,9 @@
   </link>
   <link name="r_hip_1">
     <inertial>
-      <origin xyz="-0.002814900000000002 -0.0288454 -0.0006279999999999619" rpy="0 0 -1.5707963267948966"/>
-      <mass value="2.134290199"/>
-      <inertia ixx="0.0026435421721158216" ixy="3.8119607441411114e-05" ixz="-3.6217277756713986e-06" iyy="0.0020429284372709845" iyz="1.1023562629489777e-05" izz="0.002171950916930118"/>
+      <origin xyz="-0.004106499999999999 -0.02810980000000001 -0.0008360000000000034" rpy="0 0 -1.5707963267948966"/>
+      <mass value="2.1623"/>
+      <inertia ixx="0.002675989970439473" ixy="6.426984653122192e-05" ixz="-6.447456052305588e-06" iyy="0.0021990073364667796" iyz="-1.5695577440979572e-05" izz="0.002366627191660443"/>
     </inertial>
     <visual>
       <origin xyz="-0.0216 0.04054999999999986 0.313858" rpy="0 0 -1.5707963267948966"/>
@@ -53,9 +53,9 @@
   </joint>
   <link name="r_hip_2">
     <inertial>
-      <origin xyz="-0.005953899999999998 -6.729999999999237e-05 -0.03664600000000001" rpy="0 0 -1.5707963267948966"/>
-      <mass value="1.779000764"/>
-      <inertia ixx="0.005017321245620606" ixy="4.276118215983818e-06" ixz="-3.0414292646883787e-06" iyy="0.002992476006025625" iyz="0.0010524477725526635" izz="0.003048721181695524"/>
+      <origin xyz="-0.003963499999999998 -7.010000000000349e-05 -0.03753499999999993" rpy="0 0 -1.5707963267948966"/>
+      <mass value="1.7731"/>
+      <inertia ixx="0.005229204654206729" ixy="4.628243459077994e-06" ixz="-3.2636330686460046e-06" iyy="0.0029659325947175245" iyz="0.0009467254134739666" izz="0.00327641680237346"/>
     </inertial>
     <visual>
       <origin xyz="0.025699999999999973 0.07364999999999984 0.3228580000000001" rpy="0 0 -1.5707963267948966"/>
@@ -78,17 +78,17 @@
     <axis xyz="-1.0 0.0 0.0"/>
     <parent link="r_hip_1"/>
     <child link="r_hip_2"/>
-    <limit effort="50000" lower="-0.17453292519943295" upper="2.007128639793479" velocity="120.0"/>
+    <limit effort="50000" lower="-0.17453292519943295" upper="1.9547687622336491" velocity="120.0"/>
     <dynamics damping="2.0" friction="0.0"/>
   </joint>
   <link name="r_hip_3">
     <inertial>
-      <origin xyz="0.002330500000000001 0.012397400000000003 -0.038593000000000044" rpy="0 0 -1.5707963267948966"/>
-      <mass value="2.1678368"/>
-      <inertia ixx="0.003915863467597387" ixy="-2.601086379959252e-06" ixz="0.00012293074975119214" iyy="0.0036779729507837693" iyz="3.1782922099252663e-06" izz="0.0036465259675847357"/>
+      <origin xyz="0.0024329000000000017 0.01233370000000001 -0.038242000000000054" rpy="0 0 -1.5707963267948966"/>
+      <mass value="2.1635"/>
+      <inertia ixx="0.003977864038008573" ixy="-2.0706800781267456e-06" ixz="0.00012771514554905494" iyy="0.0037304828460840533" iyz="1.0430951778634928e-05" izz="0.003698882105935934"/>
     </inertial>
     <visual>
-      <origin xyz="-0.011000000000000025 0.07364999999999984 0.41255800000000004" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.011000000000000029 0.07364999999999984 0.41255800000000004" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hip_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -97,7 +97,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.011000000000000025 0.07364999999999984 0.41255800000000004" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.011000000000000029 0.07364999999999984 0.41255800000000004" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hip_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -111,9 +111,9 @@
   </joint>
   <link name="r_upper_leg">
     <inertial>
-      <origin xyz="0.006267499999999999 -0.004376999999999999 -0.05732899999999996" rpy="0 0 -1.5707963267948966"/>
-      <mass value="0.980691904"/>
-      <inertia ixx="0.00218281785834479" ixy="-4.919689000647147e-05" ixz="0.00024386118755841528" iyy="0.0026930250628850057" iyz="0.00027207835277447823" izz="0.0011180088209673262"/>
+      <origin xyz="0.005240300000000003 -0.0014998000000000025 -0.05185799999999996" rpy="0 0 -1.5707963267948966"/>
+      <mass value="0.90926"/>
+      <inertia ixx="0.0018609457769770859" ixy="-2.472258346821965e-05" ixz="9.915722347596187e-05" iyy="0.0023282830098543227" iyz="0.00022002726995059012" izz="0.001011608294817175"/>
     </inertial>
     <visual>
       <origin xyz="-0.011000000000000012 0.05864999999999984 0.48345800000000005" rpy="0 0 -1.5707963267948966"/>
@@ -141,9 +141,9 @@
   </joint>
   <link name="r_lower_leg">
     <inertial>
-      <origin xyz="-0.012638400000000003 0.022404399999999998 -0.12942700000000007" rpy="0 0 -1.5707963267948966"/>
-      <mass value="5.074105512"/>
-      <inertia ixx="0.05708057777820247" ixy="-7.655169857870125e-05" ixz="9.837467541293897e-05" iyy="0.05456396792571687" iyz="1.958555944612649e-05" izz="0.005675806886779712"/>
+      <origin xyz="-0.013102400000000002 0.021986699999999998 -0.127262" rpy="0 0 -1.5707963267948966"/>
+      <mass value="5.0761"/>
+      <inertia ixx="0.05584150942576732" ixy="-0.00010551678502849829" ixz="-0.00025239804506766437" iyy="0.05341052337773173" iyz="-0.00012711716941235637" izz="0.005785572738044439"/>
     </inertial>
     <visual>
       <origin xyz="-0.026000000000000037 0.0835499999999998 0.589658" rpy="0 0 -1.5707963267948966"/>
@@ -171,9 +171,9 @@
   </joint>
   <link name="r_ankle_1">
     <inertial>
-      <origin xyz="-0.005962499999999999 0.0010981000000000046 -0.015748999999999902" rpy="0 0 -1.5707963267948966"/>
-      <mass value="0.346176903"/>
-      <inertia ixx="0.01" ixy="-3.4912488034212415e-05" ixz="-5.635271015717806e-05" iyy="0.01" iyz="-2.51373469956309e-05" izz="0.01"/>
+      <origin xyz="-0.005665800000000002 0.0004764000000000018 -0.015185999999999922" rpy="0 0 -1.5707963267948966"/>
+      <mass value="0.36569"/>
+      <inertia ixx="0.01" ixy="-3.63258049040399e-05" ixz="-6.07134864852001e-05" iyy="0.01" iyz="-2.6127082529919777e-05" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="-0.026000000000000245 0.08304999999999924 0.8456470000000003" rpy="0 0 -1.5707963267948966"/>
@@ -201,9 +201,9 @@
   </joint>
   <link name="r_ankle_2">
     <inertial>
-      <origin xyz="0.0429772 4.709999999999437e-05 -0.025934000000000013" rpy="0 0 -1.5707963267948966"/>
-      <mass value="1.00828013"/>
-      <inertia ixx="0.01" ixy="-4.017870761480629e-06" ixz="1.0016003725188637e-06" iyy="0.01" iyz="0.0007932901510502437" izz="0.01"/>
+      <origin xyz="0.0417371 5.619999999997849e-05 -0.025592999999999977" rpy="0 0 -1.5707963267948966"/>
+      <mass value="1.3771"/>
+      <inertia ixx="0.01" ixy="-5.974924439638223e-06" ixz="1.456936368743329e-06" iyy="0.01" iyz="0.0011141795748834335" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0.026749999999999875 0.0648499999999391 0.8806470000000108" rpy="0 0 -1.5707963267948966"/>
@@ -267,11 +267,11 @@
   <link name="r_foot_front">
     <inertial>
       <origin xyz="-0.0004180000000000017 0.0004849999999999993 0.005581000000000058" rpy="0 0 -1.5707963267948966"/>
-      <mass value="0.17569926"/>
-      <inertia ixx="0.01" ixy="-1.136706769061166e-05" ixz="2.9035165024379218e-05" iyy="0.01" iyz="6.901558256730527e-05" izz="0.01"/>
+      <mass value="0.17964"/>
+      <inertia ixx="0.01" ixy="-1.1622018441861841e-05" ixz="2.9686391650024495e-05" iyy="0.01" iyz="7.056362242091155e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="-0.10850000000000011 0.06484999999993908 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="-0.10850000000000012 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -625,12 +625,12 @@
   </joint>
   <link name="chest">
     <inertial>
-      <origin xyz="-0.00433942 -6.07081e-05 0.06080120000000001" rpy="0 0 -1.5707963267948966"/>
-      <mass value="5.771074695"/>
-      <inertia ixx="0.028334441193895898" ixy="5.239833478049791e-05" ixz="0.0001456075712956853" iyy="0.02463845124958448" iyz="-0.00252919810357917" izz="0.024688861000098004"/>
+      <origin xyz="-0.00597433 -7.26041e-05 0.06416630000000001" rpy="0 0 -1.5707963267948966"/>
+      <mass value="7.5231"/>
+      <inertia ixx="0.03385562771578811" ixy="5.2898427148176666e-05" ixz="0.00014073569655152914" iyy="0.03297344897602433" iyz="-0.0017469131032638945" izz="0.029559708315070623"/>
     </inertial>
     <visual>
-      <origin xyz="0 0 0.11638800000000013" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0 0 0.11638800000000014" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_chest_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -639,7 +639,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 0.11638800000000013" rpy="0 0 -1.5707963267948966"/>
+      <origin xyz="0 0 0.11638800000000014" rpy="0 0 -1.5707963267948966"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_chest_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -670,8 +670,8 @@
   <link name="neck_1">
     <inertial>
       <origin xyz="-0.0021259300000000003 0.00033753499999999997 0.031454700000000016" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
-      <mass value="0.359964395"/>
-      <inertia ixx="0.00017575884401652112" ixy="-8.640042073685441e-06" ixz="-1.4551961415933398e-05" iyy="0.00026164457458078367" iyz="5.4539188590076795e-06" izz="0.00027752506463589357"/>
+      <mass value="0.36804"/>
+      <inertia ixx="0.000179701897883096" ixy="-8.833876708276077e-06" ixz="-1.4878426738622656e-05" iyy="0.0002675144279997793" iyz="5.57627455590208e-06" izz="0.00028375118819347193"/>
     </inertial>
     <visual>
       <origin xyz="0.00865 0 0.049000000000000016" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
@@ -698,8 +698,8 @@
   <link name="neck_2">
     <inertial>
       <origin xyz="-0.0063756 3.63093e-05 0.006597499999999992" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
-      <mass value="0.118740971"/>
-      <inertia ixx="3.836881878953795e-05" ixy="5.576292542558762e-08" ixz="9.453729207583973e-08" iyy="3.6566268258439336e-05" iyz="-8.883645056458357e-06" izz="4.484708647035341e-05"/>
+      <mass value="0.1214"/>
+      <inertia ixx="3.922803192379071e-05" ixy="5.7011653935913464e-08" ixz="9.665431536690854e-08" iyy="3.7385115930831784e-05" iyz="-9.08258119140734e-06" izz="4.585137085918645e-05"/>
     </inertial>
     <visual>
       <origin xyz="0 0 0" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
@@ -727,9 +727,9 @@
   </joint>
   <link name="neck_3">
     <inertial>
-      <origin xyz="0.0012653999999999999 -8.218820291562122e-05 0.03610899999999334" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
-      <mass value="0.179822835"/>
-      <inertia ixx="0.00011705938532913803" ixy="-5.125016528788269e-07" ixz="-3.5211636372789045e-07" iyy="3.9042209951899456e-05" iyz="-9.756793036595435e-06" izz="0.0001047000115866525"/>
+      <origin xyz="0.0012653999999999999 -8.218820000000001e-05 0.036108999999999974" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
+      <mass value="0.18386"/>
+      <inertia ixx="0.00011968746119821388" ixy="-5.240077207007726e-07" ixz="-3.6002165484161086e-07" iyy="3.99187384725429e-05" iyz="-9.97584076409671e-06" izz="0.00010705060973108298"/>
     </inertial>
     <visual>
       <origin xyz="0 0 -0.009500499999999995" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
@@ -757,12 +757,12 @@
   </joint>
   <link name="head">
     <inertial>
-      <origin xyz="-0.02065171000002118 0.0005260309913323862 0.027897000000042443" rpy="-1.5707963267948966 0 -1.5707963267948966"/>
-      <mass value="1.831737118"/>
-      <inertia ixx="0.007475075585024412" ixy="4.7924162038447815e-05" ixz="4.818377698242177e-05" iyy="0.007242282789724778" iyz="-0.00018904885945743614" izz="0.005471491343818336"/>
+      <origin xyz="-0.02065171 0.000526031 0.027896999999999977" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
+      <mass value="1.8728"/>
+      <inertia ixx="0.007642647745719656" ixy="4.8998499721183834e-05" ixz="4.926393457113943e-05" iyy="0.007404636328713935" iyz="-0.00019328685350792046" izz="0.005594148247588756"/>
     </inertial>
     <visual>
-      <origin xyz="0 0 -0.07865050000000001" rpy="-1.5707963267948966 0 -1.5707963267948966"/>
+      <origin xyz="0 0 -0.07865050000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_head_head_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -771,7 +771,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 -0.07865050000000001" rpy="-1.5707963267948966 0 -1.5707963267948966"/>
+      <origin xyz="0 0 -0.07865050000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_head_head_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -779,7 +779,7 @@
   </link>
   <joint name="neck_yaw" type="revolute">
     <origin xyz="0 0 0.06915000000000002" rpy="0 0 0"/>
-    <axis xyz="-1.6255499221370677e-21 -1.0999983743979075e-15 1.0"/>
+    <axis xyz="0.0 0.0 1.0"/>
     <parent link="neck_3"/>
     <child link="head"/>
     <limit effort="50000" lower="-0.7853981633974483" upper="0.7853981633974483" velocity="120.0"/>
@@ -787,7 +787,7 @@
   </joint>
   <link name="imu_frame"/>
   <joint name="imu_frame_fixed_joint" type="fixed">
-    <origin xyz="0.0034999999997785457 0.0054999999863750895 0.1013340000004441" rpy="-3.141592653589793 0 -1.570796326794897"/>
+    <origin xyz="0.0035000000000000005 0.0055 0.10133400000000001" rpy="3.141592653589793 0 -1.5707963267948966"/>
     <parent link="head"/>
     <child link="imu_frame"/>
     <dynamics damping="0.1"/>
@@ -832,8 +832,8 @@
     </visual>
   </link>
   <joint name="l_eye_pan_joint" type="revolute">
-    <origin xyz="0.0 0.0 -0.034" rpy="0.0 -1.5707963267949 0.0"/>
-    <axis xyz="0.0 1.0 0.0"/>
+    <origin xyz="0.0 0.0 0.034" rpy="0.0 -1.57 0.0"/>
+    <axis xyz="0.0 -1.0 0.0"/>
     <parent link="eyes_tilt_frame"/>
     <child link="l_eye"/>
     <limit effort="50000" lower="-0.5235987755982988" upper="0.9599310885968813" velocity="50000"/>
@@ -864,20 +864,20 @@
     </visual>
   </link>
   <joint name="r_eye_pan_joint" type="revolute">
-    <origin xyz="0.0 0.0 0.034" rpy="0.0 -1.5707963267949 0.0"/>
-    <axis xyz="0.0 1.0 0.0"/>
+    <origin xyz="0.0 0.0 -0.034" rpy="0.0 -1.57 0.0"/>
+    <axis xyz="0.0 -1.0 0.0"/>
     <parent link="eyes_tilt_frame"/>
     <child link="r_eye"/>
     <limit effort="50000" lower="-0.9599310885968813" upper="0.5235987755982988" velocity="50000"/>
   </joint>
   <link name="r_shoulder_1">
     <inertial>
-      <origin xyz="0.00026441971857374055 -0.05747129038042943 0.0004210903629773867" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
-      <mass value="1.380716446"/>
-      <inertia ixx="0.01" ixy="-0.0005832833732058771" ixz="-0.0005435249287056798" iyy="0.01" iyz="-0.00015262994271578897" izz="0.01"/>
+      <origin xyz="0.00016744811157206815 -0.08721740246539644 0.0004507343002208668" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
+      <mass value="0.78697"/>
+      <inertia ixx="0.01" ixy="-0.0001736132173031554" ixz="-0.0001629290145974639" iyy="0.01" iyz="-4.081377037378354e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.03041123335618441 0.029140462447480264 0.01909577433393811" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
+      <origin xyz="0.030411233356184383 0.029140462447480246 0.01909577433393813" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -886,7 +886,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03041123335618441 0.029140462447480264 0.01909577433393811" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
+      <origin xyz="0.030411233356184383 0.029140462447480246 0.01909577433393813" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -897,17 +897,17 @@
     <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"/>
+    <limit effort="50000" lower="-3.0543261909900767" upper="0.3490658503988659" velocity="120.0"/>
     <dynamics damping="2.0" friction="0.0"/>
   </joint>
   <link name="r_shoulder_2">
     <inertial>
-      <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"/>
+      <origin xyz="0.005108924921141532 -0.00021262473188485287 -0.020403359362654718" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <mass value="0.42492"/>
+      <inertia ixx="0.01" ixy="4.8603409990214605e-05" ixz="6.290223922809094e-05" iyy="0.01" iyz="-7.540635367635347e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.023893261066846316 0.14153171676199228 -0.00725235020767272" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.023893259221697872 0.1406408982359711 0.017434528479984186" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -916,7 +916,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.023893261066846316 0.14153171676199228 -0.00725235020767272" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.023893259221697872 0.1406408982359711 0.017434528479984186" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -932,12 +932,12 @@
   </joint>
   <link name="r_shoulder_3">
     <inertial>
-      <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"/>
+      <origin xyz="-8.296442829116082e-05 -3.0309267538636275e-05 -0.007966520489017762" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <mass value="0.2625"/>
+      <inertia ixx="0.01" ixy="1.6702215931276847e-06" ixz="-1.0784251066431893e-05" iyy="0.01" iyz="-5.495630207834499e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743207452236198 0.1415314977120317 0.047047631370218265" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743205607087747 0.1406406791860105 0.07173451005787518" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -946,7 +946,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743207452236198 0.1415314977120317 0.047047631370218265" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743205607087747 0.1406406791860105 0.07173451005787518" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -960,12 +960,12 @@
   </joint>
   <link name="r_upper_arm">
     <inertial>
-      <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"/>
+      <origin xyz="0.001047638776241297 -0.002559626059706027 -0.05065314965013519" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <mass value="1.6219"/>
+      <inertia ixx="0.0028621730577300286" ixy="-6.813413059160908e-05" ixz="0.00046088710437670286" iyy="0.002840193682070483" iyz="0.00027786456283648023" izz="0.0014181870624638555"/>
     </inertial>
     <visual>
-      <origin xyz="0.03074312554381858 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743123698670136 0.14064025735327973 0.09423445143102875" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -974,7 +974,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03074312554381858 0.14153107587930094 0.06954757274337194" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.030743123698670136 0.14064025735327973 0.09423445143102875" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -990,9 +990,9 @@
   </joint>
   <link name="r_elbow_1">
     <inertial>
-      <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"/>
+      <origin xyz="-0.009961389267727768 -0.00048246745077001885 -0.014881572540082794" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <mass value="0.22157"/>
+      <inertia ixx="0.01" ixy="1.4282851377772346e-06" ixz="2.3557271962747337e-05" iyy="0.01" iyz="-5.224075189187301e-06" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0.020743191776066316 0.1391406172212523 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
@@ -1020,12 +1020,12 @@
   </joint>
   <link name="r_forearm">
     <inertial>
-      <origin xyz="-0.005853260593877814 -3.2973527508139977e-06 -0.03040488207175987" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
-      <mass value="0.854281652"/>
-      <inertia ixx="0.001230149235906481" ixy="1.1257168785020514e-06" ixz="1.906424884901568e-05" iyy="0.0011905307659583599" iyz="0.00011224066697453452" izz="0.0006081089929552"/>
+      <origin xyz="-0.005904319801167034 -3.4912323017555202e-06 -0.030210371584223267" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <mass value="0.8686"/>
+      <inertia ixx="0.0012354585621581545" ixy="-2.385589710648161e-05" ixz="0.00012368703711331712" iyy="0.0012067292165090081" iyz="0.00013988125965072048" izz="0.0006565375890204164"/>
     </inertial>
     <visual>
-      <origin xyz="0.035743063090027095 0.1420310856525903 0.2485479739054418" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03574306124487927 0.14114026712657127 0.27323485259309493" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1034,7 +1034,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.035743063090027095 0.1420310856525903 0.2485479739054418" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.03574306124487927 0.14114026712657127 0.27323485259309493" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1050,12 +1050,12 @@
   </joint>
   <link name="r_wrist_1">
     <inertial>
-      <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"/>
+      <origin xyz="-0.014340376761973205 0.000501617899800938 -0.00011512626658716218" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <mass value="0.033183"/>
+      <inertia ixx="0.01" ixy="8.77585378818565e-07" ixz="-1.8326418199726652e-08" iyy="0.01" iyz="-1.496169319469983e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.01944310973607847 0.1440385285434019 0.35604805361754815" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.019443107890930632 0.14314771001738308 0.3807349323052013" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1064,7 +1064,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.01944310973607847 0.1440385285434019 0.35604805361754815" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+      <origin xyz="0.019443107890930632 0.14314771001738308 0.3807349323052013" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1080,9 +1080,9 @@
   </joint>
   <link name="r_hand">
     <inertial>
-      <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"/>
+      <origin xyz="0.004771493181005178 0.00622015783889579 -0.06696908446509364" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
+      <mass value="0.25705"/>
+      <inertia ixx="0.01" ixy="4.072835073167062e-05" ixz="5.517886762216454e-05" iyy="0.01" iyz="2.68326820148215e-05" izz="0.01"/>
     </inertial>
     <visual>
       <origin xyz="0.00271606739979117 -0.00155679999998751 -0.00248235390712302" rpy="-1.57079632679979 0.261799319827 2.0696157436707e-13"/>
@@ -1102,16 +1102,16 @@
         <color rgba="0.1 0.1 0.1 1"/>
       </material>
     </visual>
-    <collision>
-      <origin xyz="0.03574323261035123 0.13703913849231014 0.3560481329144417" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
+   <collision>
+      <origin xyz="-0.00271606739980213 -0.0015568 -0.00248235390708518" rpy="-1.57079632679979 0.261799319827 0"/>
       <geometry>
-        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
+        <mesh filename="package://iCub/meshes/col_RightHandPalm.dae" scale="1.0 1.0 1.0"/>
       </geometry>
     </collision>
   </link>
   <joint name="r_wrist_yaw" type="revolute">
     <origin xyz="-0.016300122874272734 0.006999390051091792 0" rpy="0 -1.57079632679 3.14159265359 "/> 
-    <axis xyz="-2.4949582807394525e-07 1.0000002733514362 -2.7942487010105877e-08"/>
+    <axis xyz="-2.4949582807394525e-07 -1.0000002733514362 -2.7942487010105877e-08"/>
     <parent link="r_wrist_1"/>
     <child link="r_hand"/>
     <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
@@ -1774,12 +1774,12 @@
   </joint>
   <link name="l_shoulder_1">
     <inertial>
-      <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"/>
+      <origin xyz="0.000167535101724358 0.08722598325113093 0.00045078764626123313" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <mass value="0.78644"/>
+      <inertia ixx="0.01" ixy="0.00017359471090191915" ixz="0.00016289691907417792" iyy="0.01" iyz="-4.081880122662575e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677893" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <origin xyz="0.030411223823216114 -0.029140477503746667 0.019095766539677955" 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>
@@ -1788,7 +1788,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030411223823216205 -0.02914047750374663 0.019095766539677893" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
+      <origin xyz="0.030411223823216114 -0.029140477503746667 0.019095766539677955" 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>
@@ -1799,17 +1799,17 @@
     <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"/>
+    <limit effort="50000" lower="-3.0543261909900767" upper="0.3490658503988659" velocity="120.0"/>
     <dynamics damping="2.0" friction="0.0"/>
   </joint>
   <link name="l_shoulder_2">
     <inertial>
-      <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"/>
+      <origin xyz="0.007668668705576271 0.00018849980855933923 -0.020403454228093053" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <mass value="0.42492"/>
+      <inertia ixx="0.01" ixy="-4.916851307375464e-05" ixz="-6.324300296292925e-05" iyy="0.01" iyz="-7.587195452290467e-05" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.02639309786919337 -0.14153112610649204 -0.00725239764460165" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.026393096265395596 -0.14064030580326706 0.017434480978924874" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1818,7 +1818,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.02639309786919337 -0.14153112610649204 -0.00725239764460165" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.026393096265395596 -0.14064030580326706 0.017434480978924874" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1834,12 +1834,12 @@
   </joint>
   <link name="l_shoulder_3">
     <inertial>
-      <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"/>
+      <origin xyz="-9.561931744893337e-05 3.089115052354319e-05 -0.007966608685190321" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <mass value="0.2625"/>
+      <inertia ixx="0.01" ixy="-1.6986505676585652e-06" ixz="1.0777869051741286e-05" iyy="0.01" iyz="-5.511652932595611e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.03074323102617169 -0.14153148936381538 0.04704764107946006" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.030743229422373917 -0.14064066906059042 0.0717345197029866" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1848,7 +1848,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.03074323102617169 -0.14153148936381538 0.04704764107946006" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.030743229422373917 -0.14064066906059042 0.0717345197029866" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1862,12 +1862,12 @@
   </joint>
   <link name="l_upper_arm">
     <inertial>
-      <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"/>
+      <origin xyz="0.0010476375729993916 0.0025596225822598698 -0.05065314985074498" rpy="-0.06212860830092193 0.0714268908124578 0.27099931149673545"/>
+      <mass value="1.67729"/>
+      <inertia ixx="0.002960829234557446" ixy="-1.0441222117313817e-05" ixz="0.0002229885938828402" iyy="0.0030756376591531418" iyz="0.00020156448677983343" izz="0.001327263106289413"/>
     </inertial>
     <visual>
-      <origin xyz="0.030743149468679187 -0.1415310659113312 0.06954758245258467" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.030743147864881363 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1876,7 +1876,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.030743149468679187 -0.1415310659113312 0.06954758245258467" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.030743147864881363 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1892,12 +1892,12 @@
   </joint>
   <link name="l_elbow_1">
     <inertial>
-      <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"/>
+      <origin xyz="-0.009947433863160948 0.0004713243585720006 -0.014889454628597226" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <mass value="0.22161"/>
+      <inertia ixx="0.01" ixy="-1.4473906751261063e-06" ixz="-2.4098487110118167e-05" iyy="0.01" iyz="-5.285154621856073e-06" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.020743219233759668 -0.14003141848790235 0.1932479199901685" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.020743217629961962 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1906,7 +1906,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.020743219233759668 -0.14003141848790235 0.1932479199901685" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.020743217629961962 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1922,12 +1922,12 @@
   </joint>
   <link name="l_forearm">
     <inertial>
-      <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"/>
+      <origin xyz="-0.006323186767737077 -4.644477335560837e-05 -0.030456750179120307" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <mass value="0.87311"/>
+      <inertia ixx="0.0012499578358808174" ixy="2.299990880778632e-05" ixz="-0.00013204802784752087" iyy="0.0012325327102048142" iyz="0.00013098820888298704" izz="0.0006619865197576301"/>
     </inertial>
     <visual>
-      <origin xyz="0.035743089887937486 -0.14203106199153712 0.24854798357264754" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.03574308828413966 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1936,7 +1936,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.035743089887937486 -0.14203106199153712 0.24854798357264754" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.03574308828413966 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1952,12 +1952,12 @@
   </joint>
   <link name="l_wrist_1">
     <inertial>
-      <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"/>
+      <origin xyz="-0.014340376682819098 -0.0005016202223036004 -0.00011512600676955032" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
+      <mass value="0.033183"/>
+      <inertia ixx="0.01" ixy="-8.77585378818565e-07" ixz="1.8326321501111727e-08" iyy="0.01" iyz="-1.496169319469983e-07" izz="0.01"/>
     </inertial>
     <visual>
-      <origin xyz="0.019143073953651328 -0.1440379572096098 0.3560476247151885" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.01914307234985348 -0.14314713690638445 0.38073450333871567" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -1966,7 +1966,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0.019143073953651328 -0.1440379572096098 0.3560476247151885" rpy="-0.0715644949880594 -0.06196998841864678 -1.295357449823658"/>
+      <origin xyz="0.01914307234985348 -0.14314713690638445 0.38073450333871567" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
       <geometry>
         <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
       </geometry>
@@ -3339,10 +3339,9 @@
     <parent link="chest"/>
     <origin rpy="-1.5707963267948968 -0.0 -1.5707963267948968" xyz="0.0894115 0.0175 0.09929950000000001"/>
   </sensor>
-
   <gazebo reference="l_eye">
   <sensor name="left_camera_rgb" type="camera">
-    <pose>0.0 0.0 0.0 0.0 -1.57 1.57</pose>
+    <pose>0.0 0.0 0.0 0.0 -1.57 -1.57</pose>
     <camera>
       <!-- please use these intrinsic parameters -->
       <!-- fx = fy = 343.12110728152936, cx = 160.0, cy = 120.0 -->
@@ -3359,33 +3358,14 @@
     <always_on>1</always_on>
     <update_rate>30</update_rate>
     <visualize>false</visualize>
-  </sensor>
-  <sensor name="left_camera_depth" type="depth">
-    <pose>0.0 0.0 0.0 0.0 -1.57 1.57</pose>
-    <camera>
-      <!-- please use these intrinsic parameters -->
-      <!-- fx = fy = 343.12110728152936, cx = 160.0, cy = 120.0 -->
-      <horizontal_fov>0.8726646259971648</horizontal_fov>
-      <image>
-        <width>320</width>
-        <height>240</height>
-      </image>
-      <clip>
-        <near>0.1</near>
-        <far>100</far>
-      </clip>
-    </camera>
-    <always_on>1</always_on>
-    <update_rate>30</update_rate>
-    <visualize>false</visualize>
-    <plugin filename="libgazebo_yarp_depthCamera.so" name="left_camera_depth_plugin">
-      <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_depth_camera.ini</yarpConfigurationFile>
+    <plugin filename="libgazebo_yarp_camera.so" name="left_camera_plugin">
+      <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_left_rgb_camera.ini</yarpConfigurationFile>
     </plugin>
-</sensor>
-</gazebo>
-<gazebo reference="r_eye">
+  </sensor>
+ </gazebo>
+ <gazebo reference="r_eye">
   <sensor name="right_camera_rgb" type="camera">
-    <pose>0.0 0.0 0.0 0.0 -1.57 1.57</pose>
+    <pose>0.0 0.0 0.0 0.0 -1.57 -1.57</pose>
     <camera>
       <!-- please use these intrinsic parameters -->
       <!-- fx = fy = 343.12110728152936, cx = 160.0, cy = 120.0 -->
@@ -3403,10 +3383,10 @@
     <update_rate>30</update_rate>
     <visualize>false</visualize>
     <plugin filename="libgazebo_yarp_camera.so" name="right_camera_plugin">
-      <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_rgb_camera.ini</yarpConfigurationFile>
+      <yarpConfigurationFile>model://iCub/conf_manual/iCubGazeboV3_visuomanip/gazebo_icub_right_rgb_camera.ini</yarpConfigurationFile>
     </plugin>
   </sensor>
-</gazebo>
+ </gazebo>
   <gazebo>
     <pose>0.0 0.0 0.64 0.0 0.0 0.0</pose>
   </gazebo>