From c09724a96815a85d24e400325757ccfc8519569b Mon Sep 17 00:00:00 2001
From: Gabriele <gabriele.nava@iit.it>
Date: Wed, 21 Nov 2018 11:48:00 +0100
Subject: [PATCH 01/20] removed icub standup/seesaw and added standup/seesaw
 config directly in the world file

---
 models/icub_on_seesaw/icub.sdf               | 2184 -----------------
 models/icub_on_seesaw/model.config           |   16 -
 models/icub_standup/icub_standup.sdf         | 2236 ------------------
 models/icub_standup/model.config             |   16 -
 worlds/icub_seesaw_world/icub_seesaw_world   |   29 +-
 worlds/icub_standup_world/icub_standup_world |   29 +-
 6 files changed, 52 insertions(+), 4458 deletions(-)
 delete mode 100644 models/icub_on_seesaw/icub.sdf
 delete mode 100644 models/icub_on_seesaw/model.config
 delete mode 100644 models/icub_standup/icub_standup.sdf
 delete mode 100644 models/icub_standup/model.config

diff --git a/models/icub_on_seesaw/icub.sdf b/models/icub_on_seesaw/icub.sdf
deleted file mode 100644
index dd8cc67..0000000
--- a/models/icub_on_seesaw/icub.sdf
+++ /dev/null
@@ -1,2184 +0,0 @@
-<?xml version='1.0'?>
-<sdf version='1.5'>
-  <model name='iCub'>
-    <pose>-0.05 0 0.6926 0 0 3.14</pose>
-    <link name='root_link'>
-      <pose>0 0 0 0 -0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>4.72</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='root_link_collision'>
-        <pose>0 0 0 0 -0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_root_link.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='root_link_visual'>
-        <pose>0 0 0 0 -0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_root_link.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <link name='l_hip_1'>
-      <pose>0 -0.0681 -0.1199 2.65359e-06 1.57079 3.14159</pose>
-      <inertial>
-        <pose>0 -0.0782 0 0 -0 0</pose>
-        <mass>0.754</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>2.059e-06</ixy>
-          <ixz>1.451e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.545e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hip_1_collision'>
-        <pose>0 0 0 -0.785403 1.57079 -2.35619</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hip_1_visual'>
-        <pose>0 0 0 -0.785403 1.57079 -2.35619</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_pitch' type='revolute'>
-      <child>l_hip_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>2.65359e-06 1 -9.74718e-12</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>2.30383</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hip_2'>
-      <pose>0 -0.0681 -0.1199 3.14159 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0 0 0.03045 0 -0 0</pose>
-        <mass>0.526</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-7.4e-08</ixy>
-          <ixz>1.0835e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.2e-08</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hip_2_collision'>
-        <pose>0 0 0 0.785363 -1.57079 2.35623</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hip_2_visual'>
-        <pose>0 0 0 0.785363 -1.57079 2.35623</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_roll' type='revolute'>
-      <child>l_hip_2</child>
-      <parent>l_hip_1</parent>
-      <axis>
-        <xyz>-1 2.65359e-06 7.34641e-06</xyz>
-        <limit>
-          <lower>-0.296706</lower>
-          <upper>2.07694</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hip_3'>
-      <pose>-4.88536e-07 -0.0680998 -0.1864 3.14159 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_leg_ft_sensor' type='revolute'>
-      <child>l_hip_3</child>
-      <parent>l_hip_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_leg_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_leg_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='l_upper_leg'>
-      <pose>0 -0.0681 -0.1199 1.57079 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0.00144 -0.15943 -0.00039 0 -0 0</pose>
-        <mass>2.175</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-6.726e-05</ixy>
-          <ixz>2.267e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.63726e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_upper_leg_collision'>
-        <pose>0 0 0 -2.67797 1.57079 -1.10716</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_upper_leg_visual'>
-        <pose>0 0 0 -2.67797 1.57079 -1.10716</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_yaw' type='revolute'>
-      <child>l_upper_leg</child>
-      <parent>l_hip_3</parent>
-      <axis>
-        <xyz>7.34639e-06 -6.3268e-06 1</xyz>
-        <limit>
-          <lower>-1.37881</lower>
-          <upper>1.37881</upper>
-          <effort>40</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_lower_leg'>
-      <pose>-1.64265e-06 -0.0680986 -0.3435 2.09198 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.1071 0.00182 0.00211 0 -0 0</pose>
-        <mass>1.264</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-0.000185699</ixy>
-          <ixz>-6.3147e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>7.86e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_lower_leg_collision'>
-        <pose>5.55112e-17 2.28232e-12 2.2823e-12 -3.14159 -2.33487e-16 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_lower_leg_visual'>
-        <pose>5.55112e-17 2.28232e-12 2.2823e-12 -3.14159 -2.33487e-16 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_knee' type='revolute'>
-      <child>l_lower_leg</child>
-      <parent>l_upper_leg</parent>
-      <axis>
-        <xyz>5.30723e-06 1 6.32676e-06</xyz>
-        <limit>
-          <lower>-2.18166</lower>
-          <upper>0.401426</upper>
-          <effort>30</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_ankle_1'>
-      <pose>-3.98983e-06 -0.0680972 -0.5565 0.52118 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.0054 0.00163 0.0172 0 -0 0</pose>
-        <mass>0.746</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-7.081e-06</ixy>
-          <ixz>4.1421e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>2.0817e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_ankle_1_collision'>
-        <pose>1.11022e-16 -5.54343e-12 4.45647e-12 1.5708 1.53106e-11 1.53105e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_ankle_1_visual'>
-        <pose>1.11022e-16 -5.54343e-12 4.45647e-12 1.5708 1.53106e-11 1.53105e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_ankle_pitch' type='revolute'>
-      <child>l_ankle_1</child>
-      <parent>l_lower_leg</parent>
-      <axis>
-        <xyz>-8.98043e-06 -1 -6.32672e-06</xyz>
-        <limit>
-          <lower>-0.733038</lower>
-          <upper>0.366519</upper>
-          <effort>24</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_ankle_2'>
-      <pose>-3.98983e-06 -0.0680972 -0.5565 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0 0 0.037 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_ankle_roll' type='revolute'>
-      <child>l_ankle_2</child>
-      <parent>l_ankle_1</parent>
-      <axis>
-        <xyz>-1 8.98038e-06 7.34645e-06</xyz>
-        <limit>
-          <lower>-0.418879</lower>
-          <upper>0.418879</upper>
-          <effort>11</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_foot'>
-      <pose>-4.26164e-06 -0.068097 -0.5935 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0.024069 -0.000613931 -0.0284154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.51893e-06</ixy>
-          <ixz>0.000567785</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.62361e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_foot_collision'>
-        <pose>0.03 0 -0.01 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <!--<kp>100000</kp>-->
-              <kp>18000000</kp>
-              <!--<kd>1</kd>-->
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <!--<min_depth>0.001</min_depth>-->
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='l_foot_visual'>
-        <pose>4.45647e-12 -5.54343e-12 -0.037 -0.785403 1.57079 -0.785403</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_foot.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_foot_ft_sensor' type='revolute'>
-      <child>l_foot</child>
-      <parent>l_ankle_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_foot_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_foot_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_foot_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_hip_1'>
-      <pose>0 0.0681 -0.1199 3.14159 1.57079 -3.14159</pose>
-      <inertial>
-        <pose>0 -0.0782 0 0 -0 0</pose>
-        <mass>0.754</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hip_1_collision'>
-        <pose>0 0 0 1.76819 -1.57079 2.76109</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hip_1_visual'>
-        <pose>0 0 0 1.76819 -1.57079 2.76109</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_pitch' type='revolute'>
-      <child>r_hip_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>5.7211e-18 1 -1.22967e-22</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>2.30383</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hip_2'>
-      <pose>0 0.0681 -0.1199 -3.14159 -7.34641e-06 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0.03045 0 -0 0</pose>
-        <mass>0.526</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hip_2_collision'>
-        <pose>0 0 0 -2.35627 -1.57079 2.35627</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hip_2_visual'>
-        <pose>0 0 0 -2.35627 -1.57079 2.35627</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_roll' type='revolute'>
-      <child>r_hip_2</child>
-      <parent>r_hip_1</parent>
-      <axis>
-        <xyz>1 5.55111e-17 -7.34641e-06</xyz>
-        <limit>
-          <lower>-0.296706</lower>
-          <upper>2.07694</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hip_3'>
-      <pose>-4.88536e-07 0.0681 -0.1864 -3.14159 -7.34641e-06 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_leg_ft_sensor' type='revolute'>
-      <child>r_hip_3</child>
-      <parent>r_hip_2</parent>
-      <axis>
-        <xyz>0 0 -0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_leg_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_leg_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_upper_leg'>
-      <pose>0 0.0681 -0.1199 1.57079 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0.00144 -0.15943 0.00039 0 -0 0</pose>
-        <mass>2.175</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-2.71051e-20</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_upper_leg_collision'>
-        <pose>0 0 0 1.57087 -1.57079 -7.26536e-05</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_upper_leg_visual'>
-        <pose>0 0 0 1.57087 -1.57079 -7.26536e-05</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_yaw' type='revolute'>
-      <child>r_upper_leg</child>
-      <parent>r_hip_3</parent>
-      <axis>
-        <xyz>-7.34641e-06 3.67321e-06 -1</xyz>
-        <limit>
-          <lower>-1.37881</lower>
-          <upper>1.37881</upper>
-          <effort>40</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_lower_leg'>
-      <pose>-1.64266e-06 0.0681008 -0.3435 1.89254 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.1071 0.00182 -0.00211 0 -0 0</pose>
-        <mass>1.264</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>5.42101e-20</ixy>
-          <ixz>-5.42101e-20</ixz>
-          <iyy>0.01</iyy>
-          <iyz>8.47033e-22</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_lower_leg_collision'>
-        <pose>5.55112e-17 2.28232e-12 -8.32667e-17 3.14159 -1.14426e-17 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_lower_leg_visual'>
-        <pose>5.55112e-17 2.28232e-12 -8.32667e-17 3.14159 -1.14426e-17 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_knee' type='revolute'>
-      <child>r_lower_leg</child>
-      <parent>r_upper_leg</parent>
-      <axis>
-        <xyz>2.65362e-06 1 3.67318e-06</xyz>
-        <limit>
-          <lower>-2.18166</lower>
-          <upper>0.401426</upper>
-          <effort>30</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_ankle_1'>
-      <pose>-3.98984e-06 0.0681016 -0.5565 -2.81984 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.0054 0.00163 -0.0172 0 -0 0</pose>
-        <mass>0.746</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.69407e-21</ixy>
-          <ixz>1.35525e-20</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.38813e-21</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_ankle_1_collision'>
-        <pose>1.11022e-16 -1.08717e-12 -4.45647e-12 -1.5708 -1.53106e-11 5.10376e-12</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_ankle_1_visual'>
-        <pose>1.11022e-16 -1.08717e-12 -4.45647e-12 -1.5708 -1.53106e-11 5.10376e-12</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_ankle_pitch' type='revolute'>
-      <child>r_ankle_1</child>
-      <parent>r_lower_leg</parent>
-      <axis>
-        <xyz>1.01959e-06 -1 -3.67322e-06</xyz>
-        <limit>
-          <lower>-0.733038</lower>
-          <upper>0.366519</upper>
-          <effort>24</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_ankle_2'>
-      <pose>-3.98984e-06 0.0681016 -0.5565 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0.037 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_ankle_roll' type='revolute'>
-      <child>r_ankle_2</child>
-      <parent>r_ankle_1</parent>
-      <axis>
-        <xyz>1 2.79362e-06 -1.4236e-05</xyz>
-        <limit>
-          <lower>-0.418879</lower>
-          <upper>0.418879</upper>
-          <effort>11</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_foot'>
-      <pose>-4.51657e-06 0.0681019 -0.5935 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0.024069 0.000613931 -0.0284154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.51893e-06</ixy>
-          <ixz>0.000567784</ixz>
-          <iyy>0.01</iyy>
-          <iyz>6.62535e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_foot_collision'>
-        <pose>0.03 0 -0.01 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <!--<kp>100000</kp>-->
-              <kp>18000000</kp>
-              <!--<kd>1</kd>-->
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <!--<min_depth>0.001</min_depth>-->
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='r_foot_visual'>
-        <pose>4.45647e-12 1.08717e-12 -0.037 -2.81983 1.57079 0.321763</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_foot.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_foot_ft_sensor' type='revolute'>
-      <child>r_foot</child>
-      <parent>r_ankle_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_foot_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_foot_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_foot_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='torso_1'>
-      <pose>0 0 0 3.14159 -1.57079 -2.65359e-06</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.263e-05</ixy>
-          <ixz>-3.889e-08</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='torso_1_collision'>
-        <pose>0 0 0 -1.5708 5.10314e-12 1.11022e-16</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_lap_belt_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='torso_1_visual'>
-        <pose>0 0 0 -1.5708 5.10314e-12 1.11022e-16</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_lap_belt_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='torso_pitch' type='revolute'>
-      <child>torso_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>-2.65359e-06 -1 -1.05879e-22</xyz>
-        <limit>
-          <lower>-0.383972</lower>
-          <upper>1.46608</upper>
-          <effort>36</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='torso_2'>
-      <pose>1.17543e-07 -3.1191e-13 0.032 -3.14159 3.67321e-06 1.57079</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.923e-06</ixy>
-          <ixz>5.095e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.849e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='torso_roll' type='revolute'>
-      <child>torso_2</child>
-      <parent>torso_1</parent>
-      <axis>
-        <xyz>1 -2.6536e-06 -7.34641e-06</xyz>
-        <limit>
-          <lower>-0.680678</lower>
-          <upper>0.680678</upper>
-          <effort>80</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='chest'>
-      <pose>-0.00549988 1.45945e-08 0.032 1.5708 -7.34642e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00222898 0.075 -1.18362e-17 0 -0 0</pose>
-        <mass>4.81</mass>
-        <inertia>
-          <ixx>0.07472</ixx>
-          <ixy>-3.6e-06</ixy>
-          <ixz>-4.705e-05</ixz>
-          <iyy>0.08145</iyy>
-          <iyz>0.004567</iyz>
-          <izz>0.01306</izz>
-        </inertia>
-      </inertial>
-      <collision name='chest_collision'>
-        <pose>-0.0055 -6.93889e-18 -3.55271e-18 -1.10717 -1.57079 2.67798</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_chest.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='chest_visual'>
-        <pose>-0.0055 -6.93889e-18 -3.55271e-18 -1.10717 -1.57079 2.67798</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_chest.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='torso_yaw' type='revolute'>
-      <child>chest</child>
-      <parent>torso_2</parent>
-      <axis>
-        <xyz>-7.34642e-06 1.94942e-11 -1</xyz>
-        <limit>
-          <lower>-1.02974</lower>
-          <upper>1.02974</upper>
-          <effort>80</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_1'>
-      <pose>-0.0107487 -0.11026 0.1753 -0.261794 -1.57079 -1.42917e-06</pose>
-      <inertial>
-        <pose>-5e-06 0.0187 -0.00119 0 -0 0</pose>
-        <mass>0.189</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>9e-09</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.7e-08</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_1_collision'>
-        <pose>3.08642e-14 0.109285 -0.00521101 -0.301063 1.57079 -1.87186</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_shoulder_1_visual'>
-        <pose>3.08642e-14 0.109285 -0.00521101 -0.301063 1.57079 -1.87186</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_shoulder_pitch' type='revolute'>
-      <child>l_shoulder_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>-0.258815 -0.965927 2.85203e-06</xyz>
-        <limit>
-          <lower>-1.65806</lower>
-          <upper>0.0872665</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_2'>
-      <pose>-0.0107487 -0.11026 0.1753 3.14159 8.21174e-07 -1.83259</pose>
-      <inertial>
-        <pose>9.4e-05 -0.00627 0.0166 0 -0 0</pose>
-        <mass>0.179</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.66e-07</ixy>
-          <ixz>3.65e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-2.0524e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_2_collision'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -2.14033 1.57079 -0.569533</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_shoulder_2_visual'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -2.14033 1.57079 -0.569533</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_shoulder_roll' type='revolute'>
-      <child>l_shoulder_2</child>
-      <parent>l_shoulder_1</parent>
-      <axis>
-        <xyz>-0.965927 0.258815 6.97095e-06</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>2.80649</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_3'>
-      <pose>-0.0107487 -0.11026 0.1753 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00559398 9.77357e-05 0.0501346 0 -0 0</pose>
-        <mass>0.156157</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.662e-08</ixy>
-          <ixz>-4.3618e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-7.42844e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_shoulder_yaw' type='revolute'>
-      <child>l_shoulder_3</child>
-      <parent>l_shoulder_2</parent>
-      <axis>
-        <xyz>-6.5209e-06 2.59738e-06 -1</xyz>
-        <limit>
-          <lower>-0.645772</lower>
-          <upper>1.74533</upper>
-          <effort>34</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_upper_arm'>
-      <pose>-0.0257492 -0.11026 0.1073 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.000156 -9.87e-05 0.0298 0 -0 0</pose>
-        <mass>0.727843</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.08e-06</ixy>
-          <ixz>-2.29e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.57e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_upper_arm_collision'>
-        <pose>-0.0204334 0.000143922 -0.068 1.15274e-11 4.92958e-12 -1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_upper_arm_visual'>
-        <pose>-0.0204334 0.000143922 -0.068 1.15274e-11 4.92958e-12 -1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_arm_ft_sensor' type='revolute'>
-      <child>l_upper_arm</child>
-      <parent>l_shoulder_3</parent>
-      <axis>
-        <xyz>-0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_arm_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_arm_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='l_elbow_1'>
-      <pose>-0.0257497 -0.11026 0.0230202 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.0013 0.00371 -0.00105 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.15e-07</ixy>
-          <ixz>-4.08e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.71e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_elbow' type='revolute'>
-      <child>l_elbow_1</child>
-      <parent>l_upper_arm</parent>
-      <axis>
-        <xyz>-1.01957e-06 1 2.59739e-06</xyz>
-        <limit>
-          <lower>0.0959931</lower>
-          <upper>1.85005</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_forearm'>
-      <pose>-0.0107497 -0.11026 0.0230201 -1.5708 2.59741e-06 -1.57079</pose>
-      <inertial>
-        <pose>0.000347 0.066 0.00476 0 -0 0</pose>
-        <mass>0.525</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.337e-06</ixy>
-          <ixz>2.39e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.9381e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_forearm_collision'>
-        <pose>-0.000143922 -1.25974e-12 0.0204334 -2.66395 1.57079 -1.09315</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_forearm_visual'>
-        <pose>-0.000143922 -1.25974e-12 0.0204334 -2.66395 1.57079 -1.09315</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_wrist_prosup' type='revolute'>
-      <child>l_forearm</child>
-      <parent>l_elbow_1</parent>
-      <axis>
-        <xyz>-2.8477e-06 2.5974e-06 -1</xyz>
-        <limit>
-          <lower>-0.872665</lower>
-          <upper>0.872665</upper>
-          <effort>0.45</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_wrist_1'>
-      <pose>-0.0107501 -0.110259 -0.11428 -2.7153 1.57079 1.99709</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_wrist_pitch' type='revolute'>
-      <child>l_wrist_1</child>
-      <parent>l_forearm</parent>
-      <axis>
-        <xyz>1 1.01958e-06 -2.84771e-06</xyz>
-        <limit>
-          <lower>-1.13446</lower>
-          <upper>0.174533</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hand'>
-      <pose>-0.0107501 -0.110259 -0.11428 -2.7153 1.57079 1.99709</pose>
-      <inertial>
-        <pose>0.07023 -0.00805 -0.007 0 -0 0</pose>
-        <mass>0.213</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.278e-05</ixy>
-          <ixz>4.823e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.8188e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hand_collision'>
-        <pose>-1.25973e-12 0.0204334 -0.000143922 5.10271e-12 -2.64186e-12 1.49622e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hand_visual'>
-        <pose>-1.25973e-12 0.0204334 -0.000143922 5.10271e-12 -2.64186e-12 1.49622e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_wrist_yaw' type='revolute'>
-      <child>l_hand</child>
-      <parent>l_wrist_1</parent>
-      <axis>
-        <xyz>1.01956e-06 -1 -6.27061e-06</xyz>
-        <limit>
-          <lower>-0.436332</lower>
-          <upper>0.436332</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='neck_1'>
-      <pose>-0.00780846 1.22355e-08 0.2253 6.12199e-06 -1.57079 -1.42918e-06</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_1_collision'>
-        <pose>-5.55112e-17 9.62889e-13 -0.0055 3.14159 -1.57079 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_neck_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='neck_1_visual'>
-        <pose>-5.55112e-17 9.62889e-13 -0.0055 3.14159 -1.57079 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_neck_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='neck_pitch' type='revolute'>
-      <child>neck_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>-4.69281e-06 1 6.74621e-11</xyz>
-        <limit>
-          <lower>-0.523599</lower>
-          <upper>0.383972</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-    <link name='neck_2'>
-      <pose>-0.00780836 1.22353e-08 0.2348 -1.46928e-05 3.67327e-06 -1.57079</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_2_collision'>
-        <pose>-9.62885e-13 0.0055 0.0235 -3.14157 -1.57079 -1.57082</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_neck_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='neck_2_visual'>
-        <pose>-9.62885e-13 0.0055 0.0235 -3.14157 -1.57079 -1.57082</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_neck_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='neck_roll' type='revolute'>
-      <child>neck_2</child>
-      <parent>neck_1</parent>
-      <axis>
-        <xyz>-1 -4.69282e-06 1.46928e-05</xyz>
-        <limit>
-          <lower>-0.349066</lower>
-          <upper>0.349066</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-    <link name='head'>
-      <pose>-0.00780836 1.22353e-08 0.2348 -1.5708 1.46928e-05 8.36603e-06</pose>
-      <inertial>
-        <pose>0.0185 -0.1108 6.78454e-18 0 -0 0</pose>
-        <mass>3</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='head_collision'>
-        <pose>0.0045 -0.0235 9.42472e-13 -1.32582 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_head.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='head_visual'>
-     <!--   <pose>0.0045 -0.0235 9.42472e-13 -1.32582 1.57079 -2.89662</pose>-->
-
-       <pose>0.0045 -0.0235 9.42472e-13 -2.89662 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head_without_eyes.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <sensor name='imu_sensor' type='imu'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <visualize>1</visualize>
-        <imu/>
-        <plugin name='iCub_yarp_gazebo_plugin_IMU' filename='libgazebo_yarp_imu.so'>
-          <yarpConfigurationFile>model://icub/conf/gazebo_icub_inertial.ini</yarpConfigurationFile>
-        </plugin>
-        <pose>0.0185 -0.1108 0.0066 1.5708 -0 0</pose>
-      </sensor>
-    </link>
-    <joint name='neck_yaw' type='revolute'>
-      <child>head</child>
-      <parent>neck_2</parent>
-      <axis>
-        <xyz>1.46928e-05 1.48834e-12 1</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>0.767945</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <link name='eyes_tilt_link'>
-      <pose>-62.81e-03 0e-03 340.8e-03 1.57079 0 0</pose>
-      <inertial>
-        <pose>0.0 0.0 0.0 0 0 0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-  <!--     <visual name='eyes_tilt_visual'>
-        <pose>0.0 0.0 0.0 0 0 0</pose>
-        <geometry>
-          <cylinder><radius>0.01</radius><length>0.05</length></cylinder> 
-        </geometry>
-      </visual> -->
-    </link>
-
-    <link name='left_eye_link'>
-      <pose>-62.81e-03 -34e-03 340.8e-03 -3.14159 0 0</pose>
-      <inertial>
-        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-       <visual name='left_eye_visual'>
-         <pose>0.055 0.033 -0.14 0 0 0</pose> 
-        <geometry>
-             <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_eye.dae</uri>
-          </mesh> 
-   <!-- <cylinder><radius>0.01</radius><length>0.05</length></cylinder> -->
-        </geometry>
-      </visual>
-    </link>
-
-    <link name='right_eye_link'>
-      <pose> -62.81e-03 34e-03 340.8e-03 -3.14159 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-      <visual name='right_eye_visual'>
-        <pose>0.055 0.033 -0.14 0 0 0</pose> 
-        <geometry>
- <!-- <cylinder><radius>0.01</radius><length>0.05</length></cylinder>-->
-            <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_eye.dae</uri>
-          </mesh> 
-        </geometry>
-      </visual>
-    </link>
-
-    <joint name='eyes_tilt' type='revolute'>
-      <child>eyes_tilt_link</child>
-      <parent>head</parent>
-      <axis>
-        <xyz>0.0 1.0 0.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <joint name='left_eye' type='revolute'>
-      <child>left_eye_link</child>
-      <parent>eyes_tilt_link</parent>
-      <axis>
-        <xyz>0.0 0.0 1.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <joint name='right_eye' type='revolute'>
-      <child>right_eye_link</child>
-      <parent>eyes_tilt_link</parent>
-      <axis>
-        <xyz>0.0 0.0 1.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <link name='r_shoulder_1'>
-      <pose>-0.0107489 0.11026 0.1753 -2.87979 -1.57079 -1.42916e-06</pose>
-      <inertial>
-        <pose>5e-06 0.0187 0.00119 0 -0 0</pose>
-        <mass>0.189</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>2.1e-08</ixy>
-          <ixz>-1e-09</ixz>
-          <iyy>0.01</iyy>
-          <iyz>4.22e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_1_collision'>
-        <pose>-3.08642e-14 0.109285 0.00521101 3.06003 1.57079 -1.65235</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_shoulder_1_visual'>
-        <pose>-3.08642e-14 0.109285 0.00521101 3.06003 1.57079 -1.65235</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_shoulder_pitch' type='revolute'>
-      <child>r_shoulder_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>0.258817 -0.965926 -2.85208e-06</xyz>
-        <limit>
-          <lower>-1.65806</lower>
-          <upper>0.0872665</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_shoulder_2'>
-      <pose>-0.0107489 0.11026 0.1753 6.97094e-06 8.21122e-07 1.83259</pose>
-      <inertial>
-        <pose>-9.4e-05 -0.00627 -0.0166 0 -0 0</pose>
-        <mass>0.179</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.53e-07</ixy>
-          <ixz>2.03e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>2.07e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_2_collision'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -1.00125 -1.57079 2.57205</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_shoulder_2_visual'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -1.00125 -1.57079 2.57205</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_shoulder_roll' type='revolute'>
-      <child>r_shoulder_2</child>
-      <parent>r_shoulder_1</parent>
-      <axis>
-        <xyz>0.965926 0.258817 -6.97094e-06</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>2.80649</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_shoulder_3'>
-      <pose>-0.0107489 0.11026 0.1753 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00553932 -2.47652e-05 0.0493895 0 -0 0</pose>
-        <mass>0.155</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>3.56977e-07</ixy>
-          <ixz>-3.95372e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.5128e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_shoulder_yaw' type='revolute'>
-      <child>r_shoulder_3</child>
-      <parent>r_shoulder_2</parent>
-      <axis>
-        <xyz>6.5209e-06 -5.62481e-08 1</xyz>
-        <limit>
-          <lower>-0.645772</lower>
-          <upper>1.74533</upper>
-          <effort>34</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_upper_arm'>
-      <pose>-0.0257494 0.11026 0.1073 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.00015906 8.28733e-05 0.0298828 0 -0 0</pose>
-        <mass>0.729</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.08e-06</ixy>
-          <ixz>-2.29e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.57e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_upper_arm_collision'>
-        <pose>-0.0204334 -0.000143922 -0.068 -3.14159 -1.51363e-11 1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_upper_arm_visual'>
-        <pose>-0.0204334 -0.000143922 -0.068 -3.14159 -1.51363e-11 1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_arm_ft_sensor' type='revolute'>
-      <child>r_upper_arm</child>
-      <parent>r_shoulder_3</parent>
-      <axis>
-        <xyz>-0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_arm_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_arm_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_arm_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_elbow_1'>
-      <pose>-0.0257499 0.11026 0.0230202 -2.59732e-06 6.5209e-06 3.6732e-06</pose>
-      <inertial>
-        <pose>0.0013 -0.00371 0.00105 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.02e-07</ixy>
-          <ixz>-3.99e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.71e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_elbow' type='revolute'>
-      <child>r_elbow_1</child>
-      <parent>r_upper_arm</parent>
-      <axis>
-        <xyz>-3.67322e-06 1 -2.59732e-06</xyz>
-        <limit>
-          <lower>0.0959931</lower>
-          <upper>1.85005</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_forearm'>
-      <pose>-0.0107499 0.11026 0.0230201 1.57079 -2.59734e-06 -1.5708</pose>
-      <inertial>
-        <pose>-0.000347 -0.066 -0.00476 0 -0 0</pose>
-        <mass>0.525</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>5.66e-06</ixy>
-          <ixz>1.4e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.82e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_forearm_collision'>
-        <pose>0.000143922 -1.07214e-12 -0.0204334 -1.74165 1.57079 -0.170863</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_forearm_visual'>
-        <pose>0.000143922 -1.07214e-12 -0.0204334 -1.74165 1.57079 -0.170863</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_wrist_prosup' type='revolute'>
-      <child>r_forearm</child>
-      <parent>r_elbow_1</parent>
-      <axis>
-        <xyz>2.84768e-06 2.59734e-06 1</xyz>
-        <limit>
-          <lower>-0.872665</lower>
-          <upper>0.872665</upper>
-          <effort>0.45</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_wrist_1'>
-      <pose>-0.0107503 0.11026 -0.11428 0.426292 -1.57079 1.14451</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_wrist_pitch' type='revolute'>
-      <child>r_wrist_1</child>
-      <parent>r_forearm</parent>
-      <axis>
-        <xyz>-1 -3.67321e-06 2.84771e-06</xyz>
-        <limit>
-          <lower>-1.13446</lower>
-          <upper>0.174533</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hand'>
-      <pose>-0.0107503 0.11026 -0.11428 -0.720145 1.57079 -2.29094</pose>
-      <inertial>
-        <pose>0.07023 -0.00805 0.007 0 -0 0</pose>
-        <mass>0.213</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.26e-05</ixy>
-          <ixz>-6.08e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.76e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hand_collision'>
-        <pose>1.0721e-12 0.0204334 0.000143922 1.53098e-11 2.00656e-11 -3.14159</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hand_visual'>
-        <pose>1.0721e-12 0.0204334 0.000143922 1.53098e-11 2.00656e-11 -3.14159</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_wrist_yaw' type='revolute'>
-      <child>r_hand</child>
-      <parent>r_wrist_1</parent>
-      <axis>
-        <xyz>3.67323e-06 -1 6.27054e-06</xyz>
-        <limit>
-          <lower>-0.436332</lower>
-          <upper>0.436332</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <plugin name='controlboard_torso' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_torso.ini</yarpConfigurationFile>
-      <initialConfiguration> 0 0 0.0824</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_head' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_head.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_left_arm_no_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm_no_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-      <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-      <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-    </plugin>
-    <plugin name="apply_external_wrench" filename="libgazebo_yarp_externalwrench.so" >
-      <robotNamefromConfigFile>model://icub/conf/gazebo_icub_robotname.ini</robotNamefromConfigFile>>
-    </plugin>
-    <pose>0 0 0.7 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/models/icub_on_seesaw/model.config b/models/icub_on_seesaw/model.config
deleted file mode 100644
index f855ec9..0000000
--- a/models/icub_on_seesaw/model.config
+++ /dev/null
@@ -1,16 +0,0 @@
-<?xml version="1.0"?>
-<model>
-    <name>iCub on seesaw</name>
-    
-    <version>1.0</version>
-    <sdf version='1.5'>icub.sdf</sdf>
-    
-    <author>
-        <name>Silvio Traversaro</name>
-        <email>silvio.traversaro@iit.it</email>
-    </author>
-    
-    <description>
-        Model for the iCub humanoid robot. For more information check icub.org .
-    </description>
-</model>
diff --git a/models/icub_standup/icub_standup.sdf b/models/icub_standup/icub_standup.sdf
deleted file mode 100644
index 37aaede..0000000
--- a/models/icub_standup/icub_standup.sdf
+++ /dev/null
@@ -1,2236 +0,0 @@
-<?xml version='1.0'?>
-<sdf version='1.5'>
-  <model name='iCub'>
-    <pose>-0.077649 0.000965 0.389694 -8e-06 0.036487 3.14094</pose>
-    <link name='root_link'>
-      <pose>0 0 0 0 -0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>4.72</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='root_link_collision'>
-        <pose>0 0 0 0 -0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_root_link.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='root_link_visual'>
-        <pose>0 0 0 0 -0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_root_link.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <link name='l_hip_1'>
-      <pose>0 -0.0681 -0.1199 2.65359e-06 1.57079 3.14159</pose>
-      <inertial>
-        <pose>0 -0.0782 0 0 -0 0</pose>
-        <mass>0.754</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>2.059e-06</ixy>
-          <ixz>1.451e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.545e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hip_1_collision'>
-        <pose>0 0 0 -0.785403 1.57079 -2.35619</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hip_1_visual'>
-        <pose>0 0 0 -0.785403 1.57079 -2.35619</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_pitch' type='revolute'>
-      <child>l_hip_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>2.65359e-06 1 -9.74718e-12</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>2.30383</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hip_2'>
-      <pose>0 -0.0681 -0.1199 3.14159 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0 0 0.03045 0 -0 0</pose>
-        <mass>0.526</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-7.4e-08</ixy>
-          <ixz>1.0835e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.2e-08</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hip_2_collision'>
-        <pose>0 0 0 0.785363 -1.57079 2.35623</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hip_2_visual'>
-        <pose>0 0 0 0.785363 -1.57079 2.35623</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_roll' type='revolute'>
-      <child>l_hip_2</child>
-      <parent>l_hip_1</parent>
-      <axis>
-        <xyz>-1 2.65359e-06 7.34641e-06</xyz>
-        <limit>
-          <lower>-0.296706</lower>
-          <upper>2.07694</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hip_3'>
-      <pose>-4.88536e-07 -0.0680998 -0.1864 3.14159 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_leg_ft_sensor' type='revolute'>
-      <child>l_hip_3</child>
-      <parent>l_hip_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_leg_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_leg_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='l_upper_leg'>
-      <pose>0 -0.0681 -0.1199 1.57079 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0.00144 -0.15943 -0.00039 0 -0 0</pose>
-        <mass>2.175</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-6.726e-05</ixy>
-          <ixz>2.267e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.63726e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_upper_leg_collision'>
-        <pose>0 0 0 -2.67797 1.57079 -1.10716</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_thigh.dae</uri>
-          </mesh>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <kp>18000000</kp>
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='l_upper_leg_visual'>
-        <pose>0 0 0 -2.67797 1.57079 -1.10716</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_yaw' type='revolute'>
-      <child>l_upper_leg</child>
-      <parent>l_hip_3</parent>
-      <axis>
-        <xyz>7.34639e-06 -6.3268e-06 1</xyz>
-        <limit>
-          <lower>-1.37881</lower>
-          <upper>1.37881</upper>
-          <effort>40</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_lower_leg'>
-      <pose>-1.64265e-06 -0.0680986 -0.3435 2.09198 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.1071 0.00182 0.00211 0 -0 0</pose>
-        <mass>1.264</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-0.000185699</ixy>
-          <ixz>-6.3147e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>7.86e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_lower_leg_collision'>
-        <pose>5.55112e-17 2.28232e-12 2.2823e-12 -3.14159 -2.33487e-16 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_lower_leg_visual'>
-        <pose>5.55112e-17 2.28232e-12 2.2823e-12 -3.14159 -2.33487e-16 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_knee' type='revolute'>
-      <child>l_lower_leg</child>
-      <parent>l_upper_leg</parent>
-      <axis>
-        <xyz>5.30723e-06 1 6.32676e-06</xyz>
-        <limit>
-          <lower>-2.18166</lower>
-          <upper>0.401426</upper>
-          <effort>30</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_ankle_1'>
-      <pose>-3.98983e-06 -0.0680972 -0.5565 0.52118 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.0054 0.00163 0.0172 0 -0 0</pose>
-        <mass>0.746</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-7.081e-06</ixy>
-          <ixz>4.1421e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>2.0817e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_ankle_1_collision'>
-        <pose>1.11022e-16 -5.54343e-12 4.45647e-12 1.5708 1.53106e-11 1.53105e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_ankle_1_visual'>
-        <pose>1.11022e-16 -5.54343e-12 4.45647e-12 1.5708 1.53106e-11 1.53105e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_ankle_pitch' type='revolute'>
-      <child>l_ankle_1</child>
-      <parent>l_lower_leg</parent>
-      <axis>
-        <xyz>-8.98043e-06 -1 -6.32672e-06</xyz>
-        <limit>
-          <lower>-0.733038</lower>
-          <upper>0.366519</upper>
-          <effort>24</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_ankle_2'>
-      <pose>-3.98983e-06 -0.0680972 -0.5565 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0 0 0.037 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_ankle_roll' type='revolute'>
-      <child>l_ankle_2</child>
-      <parent>l_ankle_1</parent>
-      <axis>
-        <xyz>-1 8.98038e-06 7.34645e-06</xyz>
-        <limit>
-          <lower>-0.418879</lower>
-          <upper>0.418879</upper>
-          <effort>11</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_foot'>
-      <pose>-4.26164e-06 -0.068097 -0.5935 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0.024069 -0.000613931 -0.0284154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.51893e-06</ixy>
-          <ixz>0.000567785</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.62361e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_foot_collision'>
-        <pose>0.03 0 -0.01 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <!--<kp>100000</kp>-->
-              <kp>18000000</kp>
-              <!--<kd>1</kd>-->
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <!--<min_depth>0.001</min_depth>-->
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='l_foot_visual'>
-        <pose>4.45647e-12 -5.54343e-12 -0.037 -0.785403 1.57079 -0.785403</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_foot.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_foot_ft_sensor' type='revolute'>
-      <child>l_foot</child>
-      <parent>l_ankle_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_foot_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_foot_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_foot_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_hip_1'>
-      <pose>0 0.0681 -0.1199 3.14159 1.57079 -3.14159</pose>
-      <inertial>
-        <pose>0 -0.0782 0 0 -0 0</pose>
-        <mass>0.754</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hip_1_collision'>
-        <pose>0 0 0 1.76819 -1.57079 2.76109</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hip_1_visual'>
-        <pose>0 0 0 1.76819 -1.57079 2.76109</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_pitch' type='revolute'>
-      <child>r_hip_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>5.7211e-18 1 -1.22967e-22</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>2.30383</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hip_2'>
-      <pose>0 0.0681 -0.1199 -3.14159 -7.34641e-06 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0.03045 0 -0 0</pose>
-        <mass>0.526</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hip_2_collision'>
-        <pose>0 0 0 -2.35627 -1.57079 2.35627</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hip_2_visual'>
-        <pose>0 0 0 -2.35627 -1.57079 2.35627</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_roll' type='revolute'>
-      <child>r_hip_2</child>
-      <parent>r_hip_1</parent>
-      <axis>
-        <xyz>1 5.55111e-17 -7.34641e-06</xyz>
-        <limit>
-          <lower>-0.296706</lower>
-          <upper>2.07694</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hip_3'>
-      <pose>-4.88536e-07 0.0681 -0.1864 -3.14159 -7.34641e-06 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_leg_ft_sensor' type='revolute'>
-      <child>r_hip_3</child>
-      <parent>r_hip_2</parent>
-      <axis>
-        <xyz>0 0 -0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_leg_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_leg_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_upper_leg'>
-      <pose>0 0.0681 -0.1199 1.57079 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0.00144 -0.15943 0.00039 0 -0 0</pose>
-        <mass>2.175</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-2.71051e-20</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_upper_leg_collision'>
-        <pose>0 0 0 1.57087 -1.57079 -7.26536e-05</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_thigh.dae</uri>
-          </mesh>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <kp>18000000</kp>
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='r_upper_leg_visual'>
-        <pose>0 0 0 1.57087 -1.57079 -7.26536e-05</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_yaw' type='revolute'>
-      <child>r_upper_leg</child>
-      <parent>r_hip_3</parent>
-      <axis>
-        <xyz>-7.34641e-06 3.67321e-06 -1</xyz>
-        <limit>
-          <lower>-1.37881</lower>
-          <upper>1.37881</upper>
-          <effort>40</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_lower_leg'>
-      <pose>-1.64266e-06 0.0681008 -0.3435 1.89254 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.1071 0.00182 -0.00211 0 -0 0</pose>
-        <mass>1.264</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>5.42101e-20</ixy>
-          <ixz>-5.42101e-20</ixz>
-          <iyy>0.01</iyy>
-          <iyz>8.47033e-22</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_lower_leg_collision'>
-        <pose>5.55112e-17 2.28232e-12 -8.32667e-17 3.14159 -1.14426e-17 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_lower_leg_visual'>
-        <pose>5.55112e-17 2.28232e-12 -8.32667e-17 3.14159 -1.14426e-17 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_knee' type='revolute'>
-      <child>r_lower_leg</child>
-      <parent>r_upper_leg</parent>
-      <axis>
-        <xyz>2.65362e-06 1 3.67318e-06</xyz>
-        <limit>
-          <lower>-2.18166</lower>
-          <upper>0.401426</upper>
-          <effort>30</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_ankle_1'>
-      <pose>-3.98984e-06 0.0681016 -0.5565 -2.81984 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.0054 0.00163 -0.0172 0 -0 0</pose>
-        <mass>0.746</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.69407e-21</ixy>
-          <ixz>1.35525e-20</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.38813e-21</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_ankle_1_collision'>
-        <pose>1.11022e-16 -1.08717e-12 -4.45647e-12 -1.5708 -1.53106e-11 5.10376e-12</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_ankle_1_visual'>
-        <pose>1.11022e-16 -1.08717e-12 -4.45647e-12 -1.5708 -1.53106e-11 5.10376e-12</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_ankle_pitch' type='revolute'>
-      <child>r_ankle_1</child>
-      <parent>r_lower_leg</parent>
-      <axis>
-        <xyz>1.01959e-06 -1 -3.67322e-06</xyz>
-        <limit>
-          <lower>-0.733038</lower>
-          <upper>0.366519</upper>
-          <effort>24</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_ankle_2'>
-      <pose>-3.98984e-06 0.0681016 -0.5565 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0.037 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_ankle_roll' type='revolute'>
-      <child>r_ankle_2</child>
-      <parent>r_ankle_1</parent>
-      <axis>
-        <xyz>1 2.79362e-06 -1.4236e-05</xyz>
-        <limit>
-          <lower>-0.418879</lower>
-          <upper>0.418879</upper>
-          <effort>11</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_foot'>
-      <pose>-4.51657e-06 0.0681019 -0.5935 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0.024069 0.000613931 -0.0284154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.51893e-06</ixy>
-          <ixz>0.000567784</ixz>
-          <iyy>0.01</iyy>
-          <iyz>6.62535e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_foot_collision'>
-        <pose>0.03 0 -0.01 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <!--<kp>100000</kp>-->
-              <kp>18000000</kp>
-              <!--<kd>1</kd>-->
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <!--<min_depth>0.001</min_depth>-->
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='r_foot_visual'>
-        <pose>4.45647e-12 1.08717e-12 -0.037 -2.81983 1.57079 0.321763</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_foot.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_foot_ft_sensor' type='revolute'>
-      <child>r_foot</child>
-      <parent>r_ankle_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_foot_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_foot_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_foot_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='torso_1'>
-      <pose>0 0 0 3.14159 -1.57079 -2.65359e-06</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.263e-05</ixy>
-          <ixz>-3.889e-08</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='torso_1_collision'>
-        <pose>0 0 0 -1.5708 5.10314e-12 1.11022e-16</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_lap_belt_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='torso_1_visual'>
-        <pose>0 0 0 -1.5708 5.10314e-12 1.11022e-16</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_lap_belt_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='torso_pitch' type='revolute'>
-      <child>torso_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>-2.65359e-06 -1 -1.05879e-22</xyz>
-        <limit>
-          <lower>-0.383972</lower>
-          <upper>1.46608</upper>
-          <effort>36</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='torso_2'>
-      <pose>1.17543e-07 -3.1191e-13 0.032 -3.14159 3.67321e-06 1.57079</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.923e-06</ixy>
-          <ixz>5.095e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.849e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='torso_roll' type='revolute'>
-      <child>torso_2</child>
-      <parent>torso_1</parent>
-      <axis>
-        <xyz>1 -2.6536e-06 -7.34641e-06</xyz>
-        <limit>
-          <lower>-0.680678</lower>
-          <upper>0.680678</upper>
-          <effort>80</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='chest'>
-      <pose>-0.00549988 1.45945e-08 0.032 1.5708 -7.34642e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00222898 0.075 -1.18362e-17 0 -0 0</pose>
-        <mass>4.81</mass>
-        <inertia>
-          <ixx>0.07472</ixx>
-          <ixy>-3.6e-06</ixy>
-          <ixz>-4.705e-05</ixz>
-          <iyy>0.08145</iyy>
-          <iyz>0.004567</iyz>
-          <izz>0.01306</izz>
-        </inertia>
-      </inertial>
-      <collision name='chest_collision'>
-        <pose>-0.0055 -6.93889e-18 -3.55271e-18 -1.10717 -1.57079 2.67798</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_chest.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='chest_visual'>
-        <pose>-0.0055 -6.93889e-18 -3.55271e-18 -1.10717 -1.57079 2.67798</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_chest.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='torso_yaw' type='revolute'>
-      <child>chest</child>
-      <parent>torso_2</parent>
-      <axis>
-        <xyz>-7.34642e-06 1.94942e-11 -1</xyz>
-        <limit>
-          <lower>-1.02974</lower>
-          <upper>1.02974</upper>
-          <effort>80</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_1'>
-      <pose>-0.0107487 -0.11026 0.1753 -0.261794 -1.57079 -1.42917e-06</pose>
-      <inertial>
-        <pose>-5e-06 0.0187 -0.00119 0 -0 0</pose>
-        <mass>0.189</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>9e-09</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.7e-08</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_1_collision'>
-        <pose>3.08642e-14 0.109285 -0.00521101 -0.301063 1.57079 -1.87186</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_shoulder_1_visual'>
-        <pose>3.08642e-14 0.109285 -0.00521101 -0.301063 1.57079 -1.87186</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_shoulder_pitch' type='revolute'>
-      <child>l_shoulder_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>-0.258815 -0.965927 2.85203e-06</xyz>
-        <limit>
-          <lower>-1.65806</lower>
-          <upper>0.0872665</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_2'>
-      <pose>-0.0107487 -0.11026 0.1753 3.14159 8.21174e-07 -1.83259</pose>
-      <inertial>
-        <pose>9.4e-05 -0.00627 0.0166 0 -0 0</pose>
-        <mass>0.179</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.66e-07</ixy>
-          <ixz>3.65e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-2.0524e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_2_collision'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -2.14033 1.57079 -0.569533</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_shoulder_2_visual'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -2.14033 1.57079 -0.569533</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_shoulder_roll' type='revolute'>
-      <child>l_shoulder_2</child>
-      <parent>l_shoulder_1</parent>
-      <axis>
-        <xyz>-0.965927 0.258815 6.97095e-06</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>2.80649</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_3'>
-      <pose>-0.0107487 -0.11026 0.1753 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00559398 9.77357e-05 0.0501346 0 -0 0</pose>
-        <mass>0.156157</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.662e-08</ixy>
-          <ixz>-4.3618e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-7.42844e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_shoulder_yaw' type='revolute'>
-      <child>l_shoulder_3</child>
-      <parent>l_shoulder_2</parent>
-      <axis>
-        <xyz>-6.5209e-06 2.59738e-06 -1</xyz>
-        <limit>
-          <lower>-0.645772</lower>
-          <upper>1.74533</upper>
-          <effort>34</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_upper_arm'>
-      <pose>-0.0257492 -0.11026 0.1073 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.000156 -9.87e-05 0.0298 0 -0 0</pose>
-        <mass>0.727843</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.08e-06</ixy>
-          <ixz>-2.29e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.57e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_upper_arm_collision'>
-        <pose>-0.0204334 0.000143922 -0.068 1.15274e-11 4.92958e-12 -1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_upper_arm_visual'>
-        <pose>-0.0204334 0.000143922 -0.068 1.15274e-11 4.92958e-12 -1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_arm_ft_sensor' type='revolute'>
-      <child>l_upper_arm</child>
-      <parent>l_shoulder_3</parent>
-      <axis>
-        <xyz>-0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_arm_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_arm_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='l_elbow_1'>
-      <pose>-0.0257497 -0.11026 0.0230202 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.0013 0.00371 -0.00105 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.15e-07</ixy>
-          <ixz>-4.08e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.71e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_elbow' type='revolute'>
-      <child>l_elbow_1</child>
-      <parent>l_upper_arm</parent>
-      <axis>
-        <xyz>-1.01957e-06 1 2.59739e-06</xyz>
-        <limit>
-          <lower>0.0959931</lower>
-          <upper>1.85005</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_forearm'>
-      <pose>-0.0107497 -0.11026 0.0230201 -1.5708 2.59741e-06 -1.57079</pose>
-      <inertial>
-        <pose>0.000347 0.066 0.00476 0 -0 0</pose>
-        <mass>0.525</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.337e-06</ixy>
-          <ixz>2.39e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.9381e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_forearm_collision'>
-        <pose>-0.000143922 -1.25974e-12 0.0204334 -2.66395 1.57079 -1.09315</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_forearm_visual'>
-        <pose>-0.000143922 -1.25974e-12 0.0204334 -2.66395 1.57079 -1.09315</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_wrist_prosup' type='revolute'>
-      <child>l_forearm</child>
-      <parent>l_elbow_1</parent>
-      <axis>
-        <xyz>-2.8477e-06 2.5974e-06 -1</xyz>
-        <limit>
-          <lower>-0.872665</lower>
-          <upper>0.872665</upper>
-          <effort>0.45</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_wrist_1'>
-      <pose>-0.0107501 -0.110259 -0.11428 -2.7153 1.57079 1.99709</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_wrist_pitch' type='revolute'>
-      <child>l_wrist_1</child>
-      <parent>l_forearm</parent>
-      <axis>
-        <xyz>1 1.01958e-06 -2.84771e-06</xyz>
-        <limit>
-          <lower>-1.13446</lower>
-          <upper>0.174533</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hand'>
-      <pose>-0.0107501 -0.110259 -0.11428 -2.7153 1.57079 1.99709</pose>
-      <inertial>
-        <pose>0.07023 -0.00805 -0.007 0 -0 0</pose>
-        <mass>0.213</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.278e-05</ixy>
-          <ixz>4.823e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.8188e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hand_collision'>
-        <pose>-1.25973e-12 0.0204334 -0.000143922 5.10271e-12 -2.64186e-12 1.49622e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hand_visual'>
-        <pose>-1.25973e-12 0.0204334 -0.000143922 5.10271e-12 -2.64186e-12 1.49622e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_wrist_yaw' type='revolute'>
-      <child>l_hand</child>
-      <parent>l_wrist_1</parent>
-      <axis>
-        <xyz>1.01956e-06 -1 -6.27061e-06</xyz>
-        <limit>
-          <lower>-0.436332</lower>
-          <upper>0.436332</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='neck_1'>
-      <pose>-0.00780846 1.22355e-08 0.2253 6.12199e-06 -1.57079 -1.42918e-06</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_1_collision'>
-        <pose>-5.55112e-17 9.62889e-13 -0.0055 3.14159 -1.57079 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_neck_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='neck_1_visual'>
-        <pose>-5.55112e-17 9.62889e-13 -0.0055 3.14159 -1.57079 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_neck_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='neck_pitch' type='revolute'>
-      <child>neck_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>-4.69281e-06 1 6.74621e-11</xyz>
-        <limit>
-          <lower>-0.523599</lower>
-          <upper>0.383972</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-    <link name='neck_2'>
-      <pose>-0.00780836 1.22353e-08 0.2348 -1.46928e-05 3.67327e-06 -1.57079</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_2_collision'>
-        <pose>-9.62885e-13 0.0055 0.0235 -3.14157 -1.57079 -1.57082</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_neck_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='neck_2_visual'>
-        <pose>-9.62885e-13 0.0055 0.0235 -3.14157 -1.57079 -1.57082</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_neck_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='neck_roll' type='revolute'>
-      <child>neck_2</child>
-      <parent>neck_1</parent>
-      <axis>
-        <xyz>-1 -4.69282e-06 1.46928e-05</xyz>
-        <limit>
-          <lower>-0.349066</lower>
-          <upper>0.349066</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-    <link name='head'>
-      <pose>-0.00780836 1.22353e-08 0.2348 -1.5708 1.46928e-05 8.36603e-06</pose>
-      <inertial>
-        <pose>0.0185 -0.1108 6.78454e-18 0 -0 0</pose>
-        <mass>3</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='head_collision'>
-        <pose>0.0045 -0.0235 9.42472e-13 -1.32582 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_head.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='head_visual'>
-     <!--   <pose>0.0045 -0.0235 9.42472e-13 -1.32582 1.57079 -2.89662</pose>-->
-
-       <pose>0.0045 -0.0235 9.42472e-13 -2.89662 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head_without_eyes.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <sensor name='imu_sensor' type='imu'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <visualize>1</visualize>
-        <imu/>
-        <plugin name='iCub_yarp_gazebo_plugin_IMU' filename='libgazebo_yarp_imu.so'>
-          <yarpConfigurationFile>model://icub/conf/gazebo_icub_inertial.ini</yarpConfigurationFile>
-        </plugin>
-        <pose>0.0185 -0.1108 0.0066 1.5708 -0 0</pose>
-      </sensor>
-    </link>
-    <joint name='neck_yaw' type='revolute'>
-      <child>head</child>
-      <parent>neck_2</parent>
-      <axis>
-        <xyz>1.46928e-05 1.48834e-12 1</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>0.767945</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <link name='eyes_tilt_link'>
-      <pose>-62.81e-03 0e-03 340.8e-03 1.57079 0 0</pose>
-      <inertial>
-        <pose>0.0 0.0 0.0 0 0 0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-  <!--     <visual name='eyes_tilt_visual'>
-        <pose>0.0 0.0 0.0 0 0 0</pose>
-        <geometry>
-          <cylinder><radius>0.01</radius><length>0.05</length></cylinder> 
-        </geometry>
-      </visual> -->
-    </link>
-
-    <link name='left_eye_link'>
-      <pose>-62.81e-03 -34e-03 340.8e-03 -3.14159 0 0</pose>
-      <inertial>
-        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-       <visual name='left_eye_visual'>
-         <pose>0.055 0.033 -0.14 0 0 0</pose> 
-        <geometry>
-             <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_eye.dae</uri>
-          </mesh> 
-   <!-- <cylinder><radius>0.01</radius><length>0.05</length></cylinder> -->
-        </geometry>
-      </visual>
-    </link>
-
-    <link name='right_eye_link'>
-      <pose> -62.81e-03 34e-03 340.8e-03 -3.14159 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-      <visual name='right_eye_visual'>
-        <pose>0.055 0.033 -0.14 0 0 0</pose> 
-        <geometry>
- <!-- <cylinder><radius>0.01</radius><length>0.05</length></cylinder>-->
-            <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_eye.dae</uri>
-          </mesh> 
-        </geometry>
-      </visual>
-    </link>
-
-    <joint name='eyes_tilt' type='revolute'>
-      <child>eyes_tilt_link</child>
-      <parent>head</parent>
-      <axis>
-        <xyz>0.0 1.0 0.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <joint name='left_eye' type='revolute'>
-      <child>left_eye_link</child>
-      <parent>eyes_tilt_link</parent>
-      <axis>
-        <xyz>0.0 0.0 1.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <joint name='right_eye' type='revolute'>
-      <child>right_eye_link</child>
-      <parent>eyes_tilt_link</parent>
-      <axis>
-        <xyz>0.0 0.0 1.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <link name='r_shoulder_1'>
-      <pose>-0.0107489 0.11026 0.1753 -2.87979 -1.57079 -1.42916e-06</pose>
-      <inertial>
-        <pose>5e-06 0.0187 0.00119 0 -0 0</pose>
-        <mass>0.189</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>2.1e-08</ixy>
-          <ixz>-1e-09</ixz>
-          <iyy>0.01</iyy>
-          <iyz>4.22e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_1_collision'>
-        <pose>-3.08642e-14 0.109285 0.00521101 3.06003 1.57079 -1.65235</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_shoulder_1_visual'>
-        <pose>-3.08642e-14 0.109285 0.00521101 3.06003 1.57079 -1.65235</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_shoulder_pitch' type='revolute'>
-      <child>r_shoulder_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>0.258817 -0.965926 -2.85208e-06</xyz>
-        <limit>
-          <lower>-1.65806</lower>
-          <upper>0.0872665</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_shoulder_2'>
-      <pose>-0.0107489 0.11026 0.1753 6.97094e-06 8.21122e-07 1.83259</pose>
-      <inertial>
-        <pose>-9.4e-05 -0.00627 -0.0166 0 -0 0</pose>
-        <mass>0.179</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.53e-07</ixy>
-          <ixz>2.03e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>2.07e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_2_collision'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -1.00125 -1.57079 2.57205</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_shoulder_2_visual'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -1.00125 -1.57079 2.57205</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_shoulder_roll' type='revolute'>
-      <child>r_shoulder_2</child>
-      <parent>r_shoulder_1</parent>
-      <axis>
-        <xyz>0.965926 0.258817 -6.97094e-06</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>2.80649</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_shoulder_3'>
-      <pose>-0.0107489 0.11026 0.1753 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00553932 -2.47652e-05 0.0493895 0 -0 0</pose>
-        <mass>0.155</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>3.56977e-07</ixy>
-          <ixz>-3.95372e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.5128e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_shoulder_yaw' type='revolute'>
-      <child>r_shoulder_3</child>
-      <parent>r_shoulder_2</parent>
-      <axis>
-        <xyz>6.5209e-06 -5.62481e-08 1</xyz>
-        <limit>
-          <lower>-0.645772</lower>
-          <upper>1.74533</upper>
-          <effort>34</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_upper_arm'>
-      <pose>-0.0257494 0.11026 0.1073 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.00015906 8.28733e-05 0.0298828 0 -0 0</pose>
-        <mass>0.729</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.08e-06</ixy>
-          <ixz>-2.29e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.57e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_upper_arm_collision'>
-        <pose>-0.0204334 -0.000143922 -0.068 -3.14159 -1.51363e-11 1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_upper_arm_visual'>
-        <pose>-0.0204334 -0.000143922 -0.068 -3.14159 -1.51363e-11 1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_arm_ft_sensor' type='revolute'>
-      <child>r_upper_arm</child>
-      <parent>r_shoulder_3</parent>
-      <axis>
-        <xyz>-0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_arm_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_arm_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_arm_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_elbow_1'>
-      <pose>-0.0257499 0.11026 0.0230202 -2.59732e-06 6.5209e-06 3.6732e-06</pose>
-      <inertial>
-        <pose>0.0013 -0.00371 0.00105 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.02e-07</ixy>
-          <ixz>-3.99e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.71e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_elbow' type='revolute'>
-      <child>r_elbow_1</child>
-      <parent>r_upper_arm</parent>
-      <axis>
-        <xyz>-3.67322e-06 1 -2.59732e-06</xyz>
-        <limit>
-          <lower>0.0959931</lower>
-          <upper>1.85005</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_forearm'>
-      <pose>-0.0107499 0.11026 0.0230201 1.57079 -2.59734e-06 -1.5708</pose>
-      <inertial>
-        <pose>-0.000347 -0.066 -0.00476 0 -0 0</pose>
-        <mass>0.525</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>5.66e-06</ixy>
-          <ixz>1.4e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.82e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_forearm_collision'>
-        <pose>0.000143922 -1.07214e-12 -0.0204334 -1.74165 1.57079 -0.170863</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_forearm_visual'>
-        <pose>0.000143922 -1.07214e-12 -0.0204334 -1.74165 1.57079 -0.170863</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_wrist_prosup' type='revolute'>
-      <child>r_forearm</child>
-      <parent>r_elbow_1</parent>
-      <axis>
-        <xyz>2.84768e-06 2.59734e-06 1</xyz>
-        <limit>
-          <lower>-0.872665</lower>
-          <upper>0.872665</upper>
-          <effort>0.45</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_wrist_1'>
-      <pose>-0.0107503 0.11026 -0.11428 0.426292 -1.57079 1.14451</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_wrist_pitch' type='revolute'>
-      <child>r_wrist_1</child>
-      <parent>r_forearm</parent>
-      <axis>
-        <xyz>-1 -3.67321e-06 2.84771e-06</xyz>
-        <limit>
-          <lower>-1.13446</lower>
-          <upper>0.174533</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hand'>
-      <pose>-0.0107503 0.11026 -0.11428 -0.720145 1.57079 -2.29094</pose>
-      <inertial>
-        <pose>0.07023 -0.00805 0.007 0 -0 0</pose>
-        <mass>0.213</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.26e-05</ixy>
-          <ixz>-6.08e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.76e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hand_collision'>
-        <pose>1.0721e-12 0.0204334 0.000143922 1.53098e-11 2.00656e-11 -3.14159</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hand_visual'>
-        <pose>1.0721e-12 0.0204334 0.000143922 1.53098e-11 2.00656e-11 -3.14159</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_wrist_yaw' type='revolute'>
-      <child>r_hand</child>
-      <parent>r_wrist_1</parent>
-      <axis>
-        <xyz>3.67323e-06 -1 6.27054e-06</xyz>
-        <limit>
-          <lower>-0.436332</lower>
-          <upper>0.436332</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <plugin name='controlboard_torso' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_torso.ini</yarpConfigurationFile>
-      <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_head' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_head.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_left_arm_no_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm_no_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-      <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-      <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
-    </plugin>
-    <plugin name="apply_external_wrench" filename="libgazebo_yarp_externalwrench.so" >
-      <robotNamefromConfigFile>model://icub/conf/gazebo_icub_robotname.ini</robotNamefromConfigFile>>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/models/icub_standup/model.config b/models/icub_standup/model.config
deleted file mode 100644
index 9f7791b..0000000
--- a/models/icub_standup/model.config
+++ /dev/null
@@ -1,16 +0,0 @@
-<?xml version="1.0"?>
-<model>
-    <name>iCub on a chair</name>
-    
-    <version>1.0</version>
-    <sdf version='1.5'>icub_standup.sdf</sdf>
-    
-    <author>
-        <name>Gabriele Nava</name>
-        <email>gabriele.nava@iit.it</email>
-    </author>
-    
-    <description>
-        iCub is sitting on a chair
-    </description>
-</model>
diff --git a/worlds/icub_seesaw_world/icub_seesaw_world b/worlds/icub_seesaw_world/icub_seesaw_world
index 1895332..ce441cc 100644
--- a/worlds/icub_seesaw_world/icub_seesaw_world
+++ b/worlds/icub_seesaw_world/icub_seesaw_world
@@ -18,9 +18,32 @@
     </include>
 
     <!-- iCub above the seesaw -->
-    <include>
-    <uri>model://icub_on_seesaw</uri>
-    </include>
+    <model name="iCub">
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration> 0 0 0.0824</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+      </plugin>
+      <include>
+        <uri>model://icub</uri>
+        <pose>-0.05 0 0.6926 0 0 3.14</pose>
+      </include>
+    </model>
 
   </world>
 </sdf>
diff --git a/worlds/icub_standup_world/icub_standup_world b/worlds/icub_standup_world/icub_standup_world
index 8b06d5e..23153de 100644
--- a/worlds/icub_standup_world/icub_standup_world
+++ b/worlds/icub_standup_world/icub_standup_world
@@ -18,8 +18,31 @@
     </include>
 
     <!-- iCub in the sitting position -->
-    <include>
-    <uri>model://icub_standup</uri>
-    </include>
+    <model name="iCub">
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <include>
+        <uri>model://icub</uri>
+        <pose>-0.077649 0.000965 0.389694 -8e-06 0.036487 3.14094</pose>
+      </include>
+    </model>
   </world>
 </sdf>

From 4e93fabeae075b1a521f67982d73f4de11df42bd Mon Sep 17 00:00:00 2001
From: Lorenzo <lor.rapetti@gmail.com>
Date: Wed, 21 Nov 2018 18:15:27 +0100
Subject: [PATCH 02/20] Add two icubs world

---
 .gitignore                                    |  2 +
 worlds/two_icubs_standup_world/README.md      | 25 ++++++++
 .../two_icub_standup_world                    | 59 +++++++++++++++++++
 3 files changed, 86 insertions(+)
 create mode 100644 worlds/two_icubs_standup_world/README.md
 create mode 100644 worlds/two_icubs_standup_world/two_icub_standup_world

diff --git a/.gitignore b/.gitignore
index 0cc7e4b..54f6c99 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,3 +5,5 @@ Makefile
 cmake_install.cmake
 install_manifest.txt
 CTestTestfile.cmake
+
+.DS_Store
\ No newline at end of file
diff --git a/worlds/two_icubs_standup_world/README.md b/worlds/two_icubs_standup_world/README.md
new file mode 100644
index 0000000..b98e218
--- /dev/null
+++ b/worlds/two_icubs_standup_world/README.md
@@ -0,0 +1,25 @@
+## To load two icubs standup world in Gazebo
+
+Normally, for loading a `world`, Gazebo needs to be launched in the folder where the `world` is. You can avoid this by appending these lines to your `.bashrc`:
+
+```
+source /usr/share/gazebo/setup.sh
+export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/PATH/WHERE/YOUR/WORLD/IS
+```
+
+Furthermore, if you are working with Simulink controllers, it is necessary to sincronize Gazebo with Simulink by running Gazebo with the option `-slibgazebo_yarp_clock.so`. 
+
+Final command to run in the terminal is something like this: `gazebo -slibgazebo_yarp_clock.so nameOfYourWorld`.
+
+For **two_icubs_standup** it is also necessary to avoid using the standard `gazeboYarpPluginsRobotName` for the two robots, otherwise the two models would share the name `icubSim` and the control boards of one of the two model fail.
+This can be done by editing [gazebo_icub_robotname.ini](https://github.com/robotology/icub-gazebo/blob/master/icub/conf/gazebo_icub_robotname.ini) file, and commenting out the following line:
+ ```
+ gazeboYarpPluginsRobotName icubSim
+ ```
+
+ When the world is created the two robots are not linked, but the model `iCub` is created with the [`linkattacher`](https://github.com/robotology/gazebo-yarp-plugins/tree/master/plugins/linkattacher) plugin that can be used to attach the robots hands:
+ ```
+ $ yarp rpc /iCub/linkattacher/rpc:i
+> attachUnscoped iCub l_hand iCub_0 r_hand
+> attachUnscoped iCub r_hand iCub_0 l_hand
+ ```
\ No newline at end of file
diff --git a/worlds/two_icubs_standup_world/two_icub_standup_world b/worlds/two_icubs_standup_world/two_icub_standup_world
new file mode 100644
index 0000000..89ce965
--- /dev/null
+++ b/worlds/two_icubs_standup_world/two_icub_standup_world
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+
+    <!-- Light -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Ground Plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Chair for sittin' on -->
+    <include>
+        <uri>model://chair</uri>
+    </include>
+    
+    <!-- iCub in the sitting position -->
+    <model name="iCub">
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <include>
+          <uri>model://icub</uri>
+      </include>
+      <plugin name='link_attacher' filename='libgazebo_yarp_linkattacher.so'>
+        <yarpConfigurationString>(name /iCub/linkattacher/rpc:i)</yarpConfigurationString>
+      </plugin>
+    </model>
+
+    <!-- iCub standing-->
+    <model name="iCub_0">
+      <include>
+          <uri>model://icub</uri>
+          <pose>0.6 0.0 0.6 0 0 0</pose>
+      </include>
+    </model>
+
+  </world>
+</sdf>
\ No newline at end of file

From 49297c6b5ad6a42057ac964b85fd2b118bbc17a8 Mon Sep 17 00:00:00 2001
From: Lorenzo Rapetti <lor.rapetti@gmail.com>
Date: Thu, 22 Nov 2018 13:08:26 +0100
Subject: [PATCH 03/20] Update README.md

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 81957af..d7b699a 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,7 @@
 # icub-gazebo-wholebody
 Gazebo models and worlds for simulating scenarios related to iCub Whole Body Control. 
 
-The world and models in this repository are mantained by the [Dynamic Interaction Control Group](https://www.iit.it/research/lines/dynamic-interaction-controlicub-gazebo-wholebody) and are mostly related to whole body balancing and walking, related to the FP7 European Projects CoDyCo and Koroibot and the H2020 Project AnDy.
+The world and models in this repository are mantained by the [Dynamic Interaction Control Group](https://www.iit.it/research/lines/dynamic-interaction-control) and are mostly related to whole body balancing and walking, related to the FP7 European Projects CoDyCo and Koroibot and the H2020 Project AnDy.
 
 # Usage 
 - `git clone` the repository on your computer.

From 0a16a9d6d09d1d100da0d22c20e042922fa259e4 Mon Sep 17 00:00:00 2001
From: Lorenzo <lor.rapetti@gmail.com>
Date: Fri, 23 Nov 2018 09:23:41 +0100
Subject: [PATCH 04/20] Fix controlboard name

---
 worlds/icub_seesaw_world/icub_seesaw_world            | 4 ++--
 worlds/icub_standup_world/icub_standup_world          | 4 ++--
 worlds/two_icubs_standup_world/two_icub_standup_world | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/worlds/icub_seesaw_world/icub_seesaw_world b/worlds/icub_seesaw_world/icub_seesaw_world
index ce441cc..d4afff8 100644
--- a/worlds/icub_seesaw_world/icub_seesaw_world
+++ b/worlds/icub_seesaw_world/icub_seesaw_world
@@ -24,11 +24,11 @@
         <initialConfiguration> 0 0 0.0824</initialConfiguration>
       </plugin>
       <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
diff --git a/worlds/icub_standup_world/icub_standup_world b/worlds/icub_standup_world/icub_standup_world
index 23153de..9f88653 100644
--- a/worlds/icub_standup_world/icub_standup_world
+++ b/worlds/icub_standup_world/icub_standup_world
@@ -24,11 +24,11 @@
         <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
       </plugin>
       <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
diff --git a/worlds/two_icubs_standup_world/two_icub_standup_world b/worlds/two_icubs_standup_world/two_icub_standup_world
index 89ce965..95f2346 100644
--- a/worlds/two_icubs_standup_world/two_icub_standup_world
+++ b/worlds/two_icubs_standup_world/two_icub_standup_world
@@ -24,11 +24,11 @@
         <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
       </plugin>
       <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>

From 02e17fc309d1daf09ef55b34c51844f9e7ed9a77 Mon Sep 17 00:00:00 2001
From: Yeshasvi Tirupachuri <yeshasvitvs@gmail.com>
Date: Wed, 9 Jan 2019 15:42:03 +0100
Subject: [PATCH 05/20] Add initial files

---
 models/board/board.sdf                        |  0
 models/board/model.config                     |  0
 worlds/two_icubs_comanipulation/README.md     | 23 ++++++
 .../two_icubs_comanipulation                  | 78 +++++++++++++++++++
 4 files changed, 101 insertions(+)
 create mode 100644 models/board/board.sdf
 create mode 100644 models/board/model.config
 create mode 100644 worlds/two_icubs_comanipulation/README.md
 create mode 100644 worlds/two_icubs_comanipulation/two_icubs_comanipulation

diff --git a/models/board/board.sdf b/models/board/board.sdf
new file mode 100644
index 0000000..e69de29
diff --git a/models/board/model.config b/models/board/model.config
new file mode 100644
index 0000000..e69de29
diff --git a/worlds/two_icubs_comanipulation/README.md b/worlds/two_icubs_comanipulation/README.md
new file mode 100644
index 0000000..330d78a
--- /dev/null
+++ b/worlds/two_icubs_comanipulation/README.md
@@ -0,0 +1,23 @@
+## To load a gazebo world with two icubs comanipulating a board
+
+Normally, for loading a `world`, Gazebo needs to be launched in the folder where the `world` is. You can avoid this by appending these lines to your `.bashrc`:
+
+```
+source /usr/share/gazebo/setup.sh
+export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/PATH/WHERE/YOUR/WORLD/IS
+
+```
+
+Furthermore, if you are working with Simulink controllers, it is necessary to synchronize Gazebo with Simulink by running Gazebo with the option `-slibgazebo_yarp_clock.so`.
+
+Final command to run in the terminal is something like this: `gazebo -slibgazebo_yarp_clock.so nameOfYourWorld`. For seesaw demo, it is also required to start Gazebo in "pause" mode, and therefore final command will be: `gazebo -slibgazebo_yarp_clock.so two_icubs_comanipulation -u`.
+
+Alternatively you can add an alias to your `.bashrc` which will let you run a smaller command from the terminal to launch gazebo with this world. As an example add the following line to your `.bashrc`
+`alias gazebo_prri="gazebo -slibgazebo_yarp_clock.so two_icubs_comanipulation -u --verbose"`
+
+The option `--verbose` enables verbose output on the terminal running gazebo
+
+For **two_icubs_comanipulation** it is also necessary to avoid using the standard `gazeboYarpPluginsRobotName` for the two robots, otherwise the two models would share the name `icubSim` and the control boards of one of the two model fail.
+This can be done by editing [gazebo_icub_robotname.ini](https://github.com/robotology/icub-gazebo/blob/master/icub/conf/gazebo_icub_robotname.ini) file, and commenting out the following line:
+ ```
+ gazeboYarpPluginsRobotName icubSim
diff --git a/worlds/two_icubs_comanipulation/two_icubs_comanipulation b/worlds/two_icubs_comanipulation/two_icubs_comanipulation
new file mode 100644
index 0000000..bd18e48
--- /dev/null
+++ b/worlds/two_icubs_comanipulation/two_icubs_comanipulation
@@ -0,0 +1,78 @@
+<?xml version="1.0"?>
+<sdf version="1.4">
+  <world name="default">
+
+    <!-- Light -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Ground Plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Board -->
+    <!--include>
+        <uri>model://board</uri>
+    </include-->
+
+    <!-- iCub1 -->
+    <model name="iCub1">
+      <!--plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration> 0 0 0.0824</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+      </plugin-->
+      <include>
+        <uri>model://iCubGazeboV2_5</uri>
+        <pose>0 0 0.63 0 0 3.14</pose>
+      </include>
+    </model>
+
+    <!-- iCub2 -->
+    <model name="iCub2">
+      <!--plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration> 0 0 0.0824</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+      </plugin-->
+      <include>
+        <uri>model://iCubGazeboV2_5</uri>
+        <pose>1.0 0 0.63 0 0 0</pose>
+      </include>
+    </model>
+
+
+  </world>
+</sdf>

From ba8e171c659f6b3903ffebbf579db65c8b013ae2 Mon Sep 17 00:00:00 2001
From: Yeshasvi Tirupachuri <yeshasvitvs@gmail.com>
Date: Wed, 9 Jan 2019 17:30:31 +0100
Subject: [PATCH 06/20] Add board model

---
 models/board/board.sdf                        | 210 ++++++++++++++++++
 models/board/model.config                     |  16 ++
 .../two_icubs_comanipulation                  |  11 +-
 3 files changed, 232 insertions(+), 5 deletions(-)

diff --git a/models/board/board.sdf b/models/board/board.sdf
index e69de29..8534816 100644
--- a/models/board/board.sdf
+++ b/models/board/board.sdf
@@ -0,0 +1,210 @@
+<?xml version='1.0'?>
+<sdf version="1.5">
+  <model name="board">
+    <pose>0 0 1.0 0.0 0.0 0.0</pose>
+    <link name="base_link">
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>2</mass>
+        <inertia>
+          <ixx>0.0416</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.0416</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.083</izz>
+        </inertia>
+      </inertial>
+      <collision name='base_link_collision'>
+        <geometry>
+          <box>
+            <size>0.5 0.5 0.025</size>
+          </box>
+        </geometry>
+        <max_contacts>10</max_contacts>
+        <surface>
+          <contact>
+            <ode>
+              <soft_erp>0.2</soft_erp>
+              <soft_cfm>0</soft_cfm>
+              <kp>18000000</kp>
+              <kd>100</kd>
+              <max_vel>100</max_vel>
+              <min_depth>0.0001</min_depth>
+            </ode>
+          </contact>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>100000</threshold>
+          </bounce>
+        </surface>
+      </collision>
+      <visual name="base_link_visual">
+        <geometry>
+          <box>
+            <size>0.5 0.5 0.025</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+    <link name="border_top_link">
+      <pose>0.2375 0.0 0.025 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.000208</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.000001037</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.000208</izz>
+        </inertia>
+      </inertial>
+      <collision name='border_top_link_collision'>
+        <geometry>
+          <box>
+            <size>0.025 0.5 0.025</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="border_top_link_visual">
+        <geometry>
+          <box>
+            <size>0.025 0.5 0.025</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+    <link name="border_bottom_link">
+      <pose>-0.2375 0.0 0.025 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.000208</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.000001037</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.000208</izz>
+        </inertia>
+      </inertial>
+      <collision name='border_bottom_link_collision'>
+        <geometry>
+          <box>
+            <size>0.025 0.5 0.025</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="border_bottom_link_visual">
+        <geometry>
+          <box>
+            <size>0.025 0.5 0.025</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+    <link name="border_left_link">
+      <pose>0.0 0.2375 0.025 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.000001037</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.000208</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.000208</izz>
+        </inertia>
+      </inertial>
+      <collision name='border_left_link_collision'>
+        <geometry>
+          <box>
+            <size>0.5 0.025 0.025</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="border_left_link_visual">
+        <geometry>
+          <box>
+            <size>0.5 0.025 0.025</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+    <link name="border_right_link">
+      <pose>0.0 -0.2375 0.025 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.000001037</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.000208</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.000208</izz>
+        </inertia>
+      </inertial>
+      <collision name='border_right_link_collision'>
+        <geometry>
+          <box>
+            <size>0.5 0.025 0.025</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="border_right_link_visual">
+        <geometry>
+          <box>
+            <size>0.5 0.025 0.025</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <!-- Joints -->
+    <joint name="joint1" type="fixed">
+       <parent>base_link</parent>
+       <child>border_top_link</child>
+       <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+       <axis>
+         <xyz>1.0 0.0 0.0</xyz>
+       </axis>
+     </joint>
+     <joint name="joint2" type="fixed">
+        <parent>base_link</parent>
+        <child>border_bottom_link</child>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <axis>
+          <xyz>1.0 0.0 0.0</xyz>
+        </axis>
+      </joint>
+      <joint name="joint3" type="fixed">
+         <parent>base_link</parent>
+         <child>border_left_link</child>
+         <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+         <axis>
+           <xyz>1.0 0.0 0.0</xyz>
+         </axis>
+       </joint>
+       <joint name="joint4" type="fixed">
+          <parent>base_link</parent>
+          <child>border_right_link</child>
+          <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+          <axis>
+            <xyz>1.0 0.0 0.0</xyz>
+          </axis>
+        </joint>
+  </model>
+</sdf>
diff --git a/models/board/model.config b/models/board/model.config
index e69de29..c4677aa 100644
--- a/models/board/model.config
+++ b/models/board/model.config
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<model>
+    <name>Board</name>
+
+    <version>1.0</version>
+    <sdf version='1.5'>board.sdf</sdf>
+
+    <author>
+        <name>Yeshasvi Tirupachuri</name>
+        <email>yeshasvi.tirupachuri@iit.t</email>
+    </author>
+
+    <description>
+        A simple board used in two icubs comanipulation experiment
+    </description>
+</model>
diff --git a/worlds/two_icubs_comanipulation/two_icubs_comanipulation b/worlds/two_icubs_comanipulation/two_icubs_comanipulation
index bd18e48..216befa 100644
--- a/worlds/two_icubs_comanipulation/two_icubs_comanipulation
+++ b/worlds/two_icubs_comanipulation/two_icubs_comanipulation
@@ -13,9 +13,10 @@
     </include>
 
     <!-- Board -->
-    <!--include>
-        <uri>model://board</uri>
-    </include-->
+    <include>
+      <pose>0.5 0.0 0.1 0.0 0.0 0.0</pose>
+      <uri>model://board</uri>
+    </include>
 
     <!-- iCub1 -->
     <model name="iCub1">
@@ -40,8 +41,8 @@
         <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
       </plugin-->
       <include>
-        <uri>model://iCubGazeboV2_5</uri>
         <pose>0 0 0.63 0 0 3.14</pose>
+        <uri>model://iCubGazeboV2_5</uri>
       </include>
     </model>
 
@@ -68,8 +69,8 @@
         <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
       </plugin-->
       <include>
-        <uri>model://iCubGazeboV2_5</uri>
         <pose>1.0 0 0.63 0 0 0</pose>
+        <uri>model://iCubGazeboV2_5</uri>
       </include>
     </model>
 

From f18ed9f77479041085935fa6f7b6b977c351597d Mon Sep 17 00:00:00 2001
From: Yeshasvi Tirupachuri <yeshasvitvs@gmail.com>
Date: Wed, 9 Jan 2019 18:32:52 +0100
Subject: [PATCH 07/20] Update initial positions

---
 .../two_icubs_comanipulation                  | 45 ++++++++-----------
 1 file changed, 18 insertions(+), 27 deletions(-)

diff --git a/worlds/two_icubs_comanipulation/two_icubs_comanipulation b/worlds/two_icubs_comanipulation/two_icubs_comanipulation
index 216befa..5f32280 100644
--- a/worlds/two_icubs_comanipulation/two_icubs_comanipulation
+++ b/worlds/two_icubs_comanipulation/two_icubs_comanipulation
@@ -14,32 +14,28 @@
 
     <!-- Board -->
     <include>
-      <pose>0.5 0.0 0.1 0.0 0.0 0.0</pose>
+      <pose>0.55 0.0 0.85 0.0 0.0 0.0</pose>
       <uri>model://board</uri>
     </include>
 
     <!-- iCub1 -->
     <model name="iCub1">
-      <!--plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+      <plugin name='head_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_head'> </yarpPluginConfigurationOverride>
+        <initialConfiguration> -0.18046704 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
         <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
-        <initialConfiguration> 0 0 0.0824</initialConfiguration>
+        <initialConfiguration> 0.0 0.0 0.2156194</initialConfiguration>
       </plugin>
       <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
         <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+        <initialConfiguration>-0.6597345 0.4747296 0.4694936 1.0800097 1.0472 0.0 0.0</initialConfiguration>
       </plugin>
       <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
         <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
-      </plugin>
-      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+        <initialConfiguration>-0.6597345 0.4747296 0.4694936 1.0800097 1.0472 0.0 0.0</initialConfiguration>
       </plugin>
-      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-      </plugin-->
       <include>
         <pose>0 0 0.63 0 0 3.14</pose>
         <uri>model://iCubGazeboV2_5</uri>
@@ -48,32 +44,27 @@
 
     <!-- iCub2 -->
     <model name="iCub2">
-      <!--plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+      <plugin name='head_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_head'> </yarpPluginConfigurationOverride>
+        <initialConfiguration> -0.18046704 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
         <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
-        <initialConfiguration> 0 0 0.0824</initialConfiguration>
+        <initialConfiguration> 0.0 0.0 0.2156194</initialConfiguration>
       </plugin>
       <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
         <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+        <initialConfiguration>-0.6597345 0.4747296 0.4694936 1.0800097 1.0472 0.0 0.0</initialConfiguration>
       </plugin>
       <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
         <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+        <initialConfiguration>-0.6597345 0.4747296 0.4694936 1.0800097 1.0472 0.0 0.0</initialConfiguration>
       </plugin>
-      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-      </plugin>
-      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-      </plugin-->
       <include>
-        <pose>1.0 0 0.63 0 0 0</pose>
+        <pose>1.1 0 0.63 0 0 0</pose>
         <uri>model://iCubGazeboV2_5</uri>
       </include>
     </model>
 
-
   </world>
 </sdf>

From f405d2a3e6f8b65f13f2c4f8dd5cc5a7e82f6cb8 Mon Sep 17 00:00:00 2001
From: Yeshasvi Tirupachuri <yeshasvitvs@gmail.com>
Date: Wed, 9 Jan 2019 18:55:40 +0100
Subject: [PATCH 08/20] Update board with handle links

---
 models/board/board.sdf | 158 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 158 insertions(+)

diff --git a/models/board/board.sdf b/models/board/board.sdf
index 8534816..af02017 100644
--- a/models/board/board.sdf
+++ b/models/board/board.sdf
@@ -2,6 +2,8 @@
 <sdf version="1.5">
   <model name="board">
     <pose>0 0 1.0 0.0 0.0 0.0</pose>
+
+    <!-- Base Link -->
     <link name="base_link">
       <inertial>
         <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
@@ -56,6 +58,8 @@
         </geometry>
       </visual>
     </link>
+
+    <!-- Border Links -->
     <link name="border_top_link">
       <pose>0.2375 0.0 0.025 0.0 0.0 0.0</pose>
       <inertial>
@@ -173,6 +177,127 @@
       </visual>
     </link>
 
+    <!-- Handle Dummy Links -->
+    <link name="side1_left_dummy_link">
+      <pose>-0.25 0.125 0.025 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.0000001</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.0000001</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.0000001</izz>
+        </inertia>
+      </inertial>
+      <collision name='side1_left_dummy_link_collision'>
+        <geometry>
+          <box>
+            <size>0.00001 0.00001 0.00001</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="side1_left_dummy_link_visual">
+        <geometry>
+          <box>
+            <size>0.00001 0.00001 0.00001</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="side1_right_dummy_link">
+      <pose>-0.25 -0.125 0.025 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.0000001</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.0000001</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.0000001</izz>
+        </inertia>
+      </inertial>
+      <collision name='side1_right_dummy_link_collision'>
+        <geometry>
+          <box>
+            <size>0.00001 0.00001 0.00001</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="side1_right_dummy_link_visual">
+        <geometry>
+          <box>
+            <size>0.00001 0.00001 0.00001</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="side2_left_dummy_link">
+      <pose>0.25 0.125 0.025 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.0000001</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.0000001</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.0000001</izz>
+        </inertia>
+      </inertial>
+      <collision name='side2_left_dummy_link_collision'>
+        <geometry>
+          <box>
+            <size>0.00001 0.00001 0.00001</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="side2_left_dummy_link_visual">
+        <geometry>
+          <box>
+            <size>0.00001 0.00001 0.00001</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
+    <link name="side2_right_dummy_link">
+      <pose>0.25 -0.125 0.025 0.0 0.0 0.0</pose>
+      <inertial>
+        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+        <mass>0.01</mass>
+        <inertia>
+          <ixx>0.0000001</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.0000001</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.0000001</izz>
+        </inertia>
+      </inertial>
+      <collision name='side2_right_dummy_link_collision'>
+        <geometry>
+          <box>
+            <size>0.00001 0.00001 0.00001</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="side2_right_dummy_link_visual">
+        <geometry>
+          <box>
+            <size>0.00001 0.00001 0.00001</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+
     <!-- Joints -->
     <joint name="joint1" type="fixed">
        <parent>base_link</parent>
@@ -206,5 +331,38 @@
             <xyz>1.0 0.0 0.0</xyz>
           </axis>
         </joint>
+
+        <joint name="joint5" type="fixed">
+           <parent>base_link</parent>
+           <child>side1_left_dummy_link</child>
+           <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+           <axis>
+             <xyz>1.0 0.0 0.0</xyz>
+           </axis>
+         </joint>
+         <joint name="joint6" type="fixed">
+            <parent>base_link</parent>
+            <child>side1_right_dummy_link</child>
+            <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+            <axis>
+              <xyz>1.0 0.0 0.0</xyz>
+            </axis>
+          </joint>
+          <joint name="joint7" type="fixed">
+             <parent>base_link</parent>
+             <child>side2_left_dummy_link</child>
+             <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+             <axis>
+               <xyz>1.0 0.0 0.0</xyz>
+             </axis>
+           </joint>
+           <joint name="joint8" type="fixed">
+              <parent>base_link</parent>
+              <child>side2_right_dummy_link</child>
+              <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+              <axis>
+                <xyz>1.0 0.0 0.0</xyz>
+              </axis>
+            </joint>
   </model>
 </sdf>

From a527b3bd1253528f65bf66fced90503fc63ffb9c Mon Sep 17 00:00:00 2001
From: Yeshasvi Tirupachuri <yeshasvitvs@gmail.com>
Date: Wed, 9 Jan 2019 20:03:11 +0100
Subject: [PATCH 09/20] Update handles inertial parameters and add linkattacher
 plugin

---
 models/board/board.sdf                   | 180 ++++++++++++-----------
 models/board/conf/linkattacher.ini       |   1 +
 models/board/handlesHandsLinkAttacher.sh |  52 +++++++
 3 files changed, 146 insertions(+), 87 deletions(-)
 create mode 100644 models/board/conf/linkattacher.ini
 create mode 100755 models/board/handlesHandsLinkAttacher.sh

diff --git a/models/board/board.sdf b/models/board/board.sdf
index af02017..bc6789b 100644
--- a/models/board/board.sdf
+++ b/models/board/board.sdf
@@ -182,27 +182,27 @@
       <pose>-0.25 0.125 0.025 0.0 0.0 0.0</pose>
       <inertial>
         <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <mass>0.01</mass>
+        <mass>0.1</mass>
         <inertia>
-          <ixx>0.0000001</ixx>
+          <ixx>0,00000166</ixx>
           <ixy>0.0</ixy>
           <ixz>0.0</ixz>
-          <iyy>0.0000001</iyy>
+          <iyy>0,00000166</iyy>
           <iyz>0.0</iyz>
-          <izz>0.0000001</izz>
+          <izz>0,00000166</izz>
         </inertia>
       </inertial>
       <collision name='side1_left_dummy_link_collision'>
         <geometry>
           <box>
-            <size>0.00001 0.00001 0.00001</size>
+            <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </collision>
       <visual name="side1_left_dummy_link_visual">
         <geometry>
           <box>
-            <size>0.00001 0.00001 0.00001</size>
+            <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </visual>
@@ -212,27 +212,27 @@
       <pose>-0.25 -0.125 0.025 0.0 0.0 0.0</pose>
       <inertial>
         <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <mass>0.01</mass>
+        <mass>0.1</mass>
         <inertia>
-          <ixx>0.0000001</ixx>
+          <ixx>0,00000166</ixx>
           <ixy>0.0</ixy>
           <ixz>0.0</ixz>
-          <iyy>0.0000001</iyy>
+          <iyy>0,00000166</iyy>
           <iyz>0.0</iyz>
-          <izz>0.0000001</izz>
+          <izz>0,00000166</izz>
         </inertia>
       </inertial>
       <collision name='side1_right_dummy_link_collision'>
         <geometry>
           <box>
-            <size>0.00001 0.00001 0.00001</size>
+            <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </collision>
       <visual name="side1_right_dummy_link_visual">
         <geometry>
           <box>
-            <size>0.00001 0.00001 0.00001</size>
+            <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </visual>
@@ -242,27 +242,27 @@
       <pose>0.25 0.125 0.025 0.0 0.0 0.0</pose>
       <inertial>
         <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <mass>0.01</mass>
+        <mass>0.1</mass>
         <inertia>
-          <ixx>0.0000001</ixx>
+          <ixx>0,00000166</ixx>
           <ixy>0.0</ixy>
           <ixz>0.0</ixz>
-          <iyy>0.0000001</iyy>
+          <iyy>0,00000166</iyy>
           <iyz>0.0</iyz>
-          <izz>0.0000001</izz>
+          <izz>0,00000166</izz>
         </inertia>
       </inertial>
       <collision name='side2_left_dummy_link_collision'>
         <geometry>
           <box>
-            <size>0.00001 0.00001 0.00001</size>
+            <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </collision>
       <visual name="side2_left_dummy_link_visual">
         <geometry>
           <box>
-            <size>0.00001 0.00001 0.00001</size>
+            <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </visual>
@@ -272,27 +272,27 @@
       <pose>0.25 -0.125 0.025 0.0 0.0 0.0</pose>
       <inertial>
         <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <mass>0.01</mass>
+        <mass>0.1</mass>
         <inertia>
-          <ixx>0.0000001</ixx>
+          <ixx>0,00000166</ixx>
           <ixy>0.0</ixy>
           <ixz>0.0</ixz>
-          <iyy>0.0000001</iyy>
+          <iyy>0,00000166</iyy>
           <iyz>0.0</iyz>
-          <izz>0.0000001</izz>
+          <izz>0,00000166</izz>
         </inertia>
       </inertial>
       <collision name='side2_right_dummy_link_collision'>
         <geometry>
           <box>
-            <size>0.00001 0.00001 0.00001</size>
+            <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </collision>
       <visual name="side2_right_dummy_link_visual">
         <geometry>
           <box>
-            <size>0.00001 0.00001 0.00001</size>
+            <size>0.01 0.01 0.01</size>
           </box>
         </geometry>
       </visual>
@@ -300,69 +300,75 @@
 
     <!-- Joints -->
     <joint name="joint1" type="fixed">
-       <parent>base_link</parent>
-       <child>border_top_link</child>
-       <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-       <axis>
-         <xyz>1.0 0.0 0.0</xyz>
-       </axis>
-     </joint>
-     <joint name="joint2" type="fixed">
-        <parent>base_link</parent>
-        <child>border_bottom_link</child>
-        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <axis>
-          <xyz>1.0 0.0 0.0</xyz>
-        </axis>
-      </joint>
-      <joint name="joint3" type="fixed">
-         <parent>base_link</parent>
-         <child>border_left_link</child>
-         <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-         <axis>
-           <xyz>1.0 0.0 0.0</xyz>
-         </axis>
-       </joint>
-       <joint name="joint4" type="fixed">
-          <parent>base_link</parent>
-          <child>border_right_link</child>
-          <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-          <axis>
-            <xyz>1.0 0.0 0.0</xyz>
-          </axis>
-        </joint>
+      <parent>base_link</parent>
+      <child>border_top_link</child>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <axis>
+        <xyz>1.0 0.0 0.0</xyz>
+      </axis>
+    </joint>
+    <joint name="joint2" type="fixed">
+      <parent>base_link</parent>
+      <child>border_bottom_link</child>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <axis>
+        <xyz>1.0 0.0 0.0</xyz>
+      </axis>
+    </joint>
+    <joint name="joint3" type="fixed">
+      <parent>base_link</parent>
+      <child>border_left_link</child>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <axis>
+        <xyz>1.0 0.0 0.0</xyz>
+      </axis>
+    </joint>
+    <joint name="joint4" type="fixed">
+      <parent>base_link</parent>
+      <child>border_right_link</child>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <axis>
+        <xyz>1.0 0.0 0.0</xyz>
+      </axis>
+    </joint>
+
+    <joint name="joint5" type="fixed">
+      <parent>base_link</parent>
+      <child>side1_left_dummy_link</child>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <axis>
+        <xyz>1.0 0.0 0.0</xyz>
+      </axis>
+    </joint>
+    <joint name="joint6" type="fixed">
+      <parent>base_link</parent>
+      <child>side1_right_dummy_link</child>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <axis>
+        <xyz>1.0 0.0 0.0</xyz>
+      </axis>
+    </joint>
+    <joint name="joint7" type="fixed">
+      <parent>base_link</parent>
+      <child>side2_left_dummy_link</child>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <axis>
+        <xyz>1.0 0.0 0.0</xyz>
+      </axis>
+    </joint>
+    <joint name="joint8" type="fixed">
+      <parent>base_link</parent>
+      <child>side2_right_dummy_link</child>
+      <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
+      <axis>
+        <xyz>1.0 0.0 0.0</xyz>
+      </axis>
+    </joint>
+
+    <!-- Linkattacher Plugin -->
+    <plugin name='link_attacher' filename='libgazebo_yarp_linkattacher.so'>
+      <yarpConfigurationFile>model://board/conf/linkattacher.ini</yarpConfigurationFile>
+    </plugin>
 
-        <joint name="joint5" type="fixed">
-           <parent>base_link</parent>
-           <child>side1_left_dummy_link</child>
-           <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-           <axis>
-             <xyz>1.0 0.0 0.0</xyz>
-           </axis>
-         </joint>
-         <joint name="joint6" type="fixed">
-            <parent>base_link</parent>
-            <child>side1_right_dummy_link</child>
-            <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-            <axis>
-              <xyz>1.0 0.0 0.0</xyz>
-            </axis>
-          </joint>
-          <joint name="joint7" type="fixed">
-             <parent>base_link</parent>
-             <child>side2_left_dummy_link</child>
-             <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-             <axis>
-               <xyz>1.0 0.0 0.0</xyz>
-             </axis>
-           </joint>
-           <joint name="joint8" type="fixed">
-              <parent>base_link</parent>
-              <child>side2_right_dummy_link</child>
-              <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-              <axis>
-                <xyz>1.0 0.0 0.0</xyz>
-              </axis>
-            </joint>
   </model>
 </sdf>
diff --git a/models/board/conf/linkattacher.ini b/models/board/conf/linkattacher.ini
new file mode 100644
index 0000000..4838622
--- /dev/null
+++ b/models/board/conf/linkattacher.ini
@@ -0,0 +1 @@
+name /board/linkattacher/rpc:i
diff --git a/models/board/handlesHandsLinkAttacher.sh b/models/board/handlesHandsLinkAttacher.sh
new file mode 100755
index 0000000..7125c05
--- /dev/null
+++ b/models/board/handlesHandsLinkAttacher.sh
@@ -0,0 +1,52 @@
+usage() {
+cat << EOF
+************************************************************************************************************************************
+ATTACH SCRIPT FOR PRRI SIMULATION EXPERIMENT WITH TWO ICUBS COMANIPULATING A BOARD
+Author:  Yeshasvi Tirupachuri   <yeshasvi.tirupachuri@iit.it>
+This script takes name of the human model and attaches the hands of the human to iCub
+USAGE:
+        $0 options
+************************************************************************************************************************************
+OPTIONS: <command specifier>
+************************************************************************************************************************************
+EXAMPLE USAGE: ./handlesHandsLinkAttacher.sh attach -----> Attach the handle links of the board to the hand links of the two robot
+EXAMPLE USAGE: ./handlesHandsLinkAttacher.sh detach -----> Detach the handle links of the board from the hand links of the two robot
+************************************************************************************************************************************
+EOF
+}
+
+attach() {
+  echo "attachUnscoped board side1_left_dummy_link iCub1 l_hand" | yarp rpc /board/linkattacher/rpc:i
+  echo "attachUnscoped board side1_right_dummy_link iCub1 r_hand" | yarp rpc /board/linkattacher/rpc:i
+  echo "attachUnscoped board side2_left_dummy_link iCub2 r_hand" | yarp rpc /board/linkattacher/rpc:i
+  echo "attachUnscoped board side2_right_dummy_link iCub2 l_hand" | yarp rpc /board/linkattacher/rpc:i
+}
+
+detach() {
+  echo "detachUnscoped board side1_left_dummy_link" | yarp rpc /board/linkattacher/rpc:i
+  echo "detachUnscoped board side1_right_dummy_link" | yarp rpc /board/linkattacher/rpc:i
+  echo "detachUnscoped board side2_left_dummy_link" | yarp rpc /board/linkattacher/rpc:i
+  echo "detachUnscoped board side2_right_dummy_link" | yarp rpc /board/linkattacher/rpc:i
+}
+
+################################################################################
+# "MAIN" FUNCTION:                                                             #
+################################################################################
+if [[ $# -eq 0 || $# -lt 1 ]] ; then
+    echo "Error in options passed!"
+    echo ""
+    usage
+    exit 1
+fi
+
+COMMAND=$1
+
+if [[ ${COMMAND} == "attach" ]] ; then
+  attach
+fi
+
+if [[ ${COMMAND} == "detach" ]] ; then
+  detach
+fi
+
+exit 0

From 51ba393f4a6c3963982a768b7b1a81de109f2079 Mon Sep 17 00:00:00 2001
From: Yeshasvi Tirupachuri <yeshasvitvs@gmail.com>
Date: Wed, 9 Jan 2019 20:03:42 +0100
Subject: [PATCH 10/20] Update board model in world

---
 .../two_icubs_comanipulation                       | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/worlds/two_icubs_comanipulation/two_icubs_comanipulation b/worlds/two_icubs_comanipulation/two_icubs_comanipulation
index 5f32280..1a9d900 100644
--- a/worlds/two_icubs_comanipulation/two_icubs_comanipulation
+++ b/worlds/two_icubs_comanipulation/two_icubs_comanipulation
@@ -12,12 +12,6 @@
       <uri>model://ground_plane</uri>
     </include>
 
-    <!-- Board -->
-    <include>
-      <pose>0.55 0.0 0.85 0.0 0.0 0.0</pose>
-      <uri>model://board</uri>
-    </include>
-
     <!-- iCub1 -->
     <model name="iCub1">
       <plugin name='head_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
@@ -66,5 +60,13 @@
       </include>
     </model>
 
+    <!-- Board -->
+    <model name="board">
+      <include>
+        <pose>0.55 0.0 0.675 0.0 0.0 0.0</pose>
+        <uri>model://board</uri>
+      </include>
+    </model>
+
   </world>
 </sdf>

From a18deb1d7517241489bd790626b74761915df966 Mon Sep 17 00:00:00 2001
From: Gabriele <gabriele.nava@iit.it>
Date: Wed, 6 Mar 2019 11:34:46 +0100
Subject: [PATCH 11/20] cleanup worlds names

---
 worlds/icub_seesaw_world/icub_seesaw.world   | 26 +++++++++++
 worlds/icub_seesaw_world/icub_seesaw_world   | 49 --------------------
 worlds/icub_standup_world/icub_standup.world | 25 ++++++++++
 worlds/icub_standup_world/icub_standup_world | 48 -------------------
 4 files changed, 51 insertions(+), 97 deletions(-)
 create mode 100644 worlds/icub_seesaw_world/icub_seesaw.world
 delete mode 100644 worlds/icub_seesaw_world/icub_seesaw_world
 create mode 100644 worlds/icub_standup_world/icub_standup.world
 delete mode 100644 worlds/icub_standup_world/icub_standup_world

diff --git a/worlds/icub_seesaw_world/icub_seesaw.world b/worlds/icub_seesaw_world/icub_seesaw.world
new file mode 100644
index 0000000..1895332
--- /dev/null
+++ b/worlds/icub_seesaw_world/icub_seesaw.world
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<sdf version="1.4">
+  <world name="default">
+
+    <!-- Light -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Ground Plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Seesaw -->
+    <include>
+        <uri>model://seesaw</uri>
+    </include>
+
+    <!-- iCub above the seesaw -->
+    <include>
+    <uri>model://icub_on_seesaw</uri>
+    </include>
+
+  </world>
+</sdf>
diff --git a/worlds/icub_seesaw_world/icub_seesaw_world b/worlds/icub_seesaw_world/icub_seesaw_world
deleted file mode 100644
index d4afff8..0000000
--- a/worlds/icub_seesaw_world/icub_seesaw_world
+++ /dev/null
@@ -1,49 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.4">
-  <world name="default">
-
-    <!-- Light -->
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <!-- Ground Plane -->
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-
-    <!-- Seesaw -->
-    <include>
-        <uri>model://seesaw</uri>
-    </include>
-
-    <!-- iCub above the seesaw -->
-    <model name="iCub">
-      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
-        <initialConfiguration> 0 0 0.0824</initialConfiguration>
-      </plugin>
-      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
-      </plugin>
-      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
-      </plugin>
-      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-      </plugin>
-      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-      </plugin>
-      <include>
-        <uri>model://icub</uri>
-        <pose>-0.05 0 0.6926 0 0 3.14</pose>
-      </include>
-    </model>
-
-  </world>
-</sdf>
diff --git a/worlds/icub_standup_world/icub_standup.world b/worlds/icub_standup_world/icub_standup.world
new file mode 100644
index 0000000..8b06d5e
--- /dev/null
+++ b/worlds/icub_standup_world/icub_standup.world
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<sdf version="1.4">
+  <world name="default">
+
+    <!-- Light -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Ground Plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Chair for sittin' on -->
+    <include>
+        <uri>model://chair</uri>
+    </include>
+
+    <!-- iCub in the sitting position -->
+    <include>
+    <uri>model://icub_standup</uri>
+    </include>
+  </world>
+</sdf>
diff --git a/worlds/icub_standup_world/icub_standup_world b/worlds/icub_standup_world/icub_standup_world
deleted file mode 100644
index 9f88653..0000000
--- a/worlds/icub_standup_world/icub_standup_world
+++ /dev/null
@@ -1,48 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.4">
-  <world name="default">
-
-    <!-- Light -->
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <!-- Ground Plane -->
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-
-    <!-- Chair for sittin' on -->
-    <include>
-        <uri>model://chair</uri>
-    </include>
-
-    <!-- iCub in the sitting position -->
-    <model name="iCub">
-      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
-      </plugin>
-      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
-      </plugin>
-      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
-      </plugin>
-      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
-      </plugin>
-      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
-        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
-      </plugin>
-      <include>
-        <uri>model://icub</uri>
-        <pose>-0.077649 0.000965 0.389694 -8e-06 0.036487 3.14094</pose>
-      </include>
-    </model>
-  </world>
-</sdf>

From c10a7e0f7415583925f0c30f5d07ff2fcff676ef Mon Sep 17 00:00:00 2001
From: Gabriele <gabriele.nava@iit.it>
Date: Wed, 6 Mar 2019 11:40:56 +0100
Subject: [PATCH 12/20] fixed worlds names

---
 worlds/icub_seesaw_world/README.md  | 4 ++--
 worlds/icub_standup_world/README.md | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/worlds/icub_seesaw_world/README.md b/worlds/icub_seesaw_world/README.md
index 0451739..50fb509 100644
--- a/worlds/icub_seesaw_world/README.md
+++ b/worlds/icub_seesaw_world/README.md
@@ -4,12 +4,12 @@ Normally, for loading a `world`, Gazebo needs to be launched in the folder where
 
 ```
 source /usr/share/gazebo/setup.sh
-export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/PATH/WHERE/YOUR/WORLD/IS
+export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/PATH/TO/WORLD/FOLDER
 
 ```
 
 Furthermore, if you are working with Simulink controllers, it is necessary to sincronize Gazebo with Simulink by running Gazebo with the option `-slibgazebo_yarp_clock.so`. 
 
-Final command to run in the terminal is something like this: `gazebo -slibgazebo_yarp_clock.so nameOfYourWorld`. For seesaw demo, it is also required to start Gazebo in "pause" mode, and therefore final command will be: `gazebo -slibgazebo_yarp_clock.so icub_seesaw_world -u`.
+Final command to run in the terminal is something like this: `gazebo -slibgazebo_yarp_clock.so worldFolderName/nameOfYourWorld`. For seesaw demo, it is also required to start Gazebo in "pause" mode, and therefore final command will be: `gazebo -slibgazebo_yarp_clock.so icub_seesaw_world/icub_seesaw.world -u`.
 
 
diff --git a/worlds/icub_standup_world/README.md b/worlds/icub_standup_world/README.md
index eb14518..1249618 100644
--- a/worlds/icub_standup_world/README.md
+++ b/worlds/icub_standup_world/README.md
@@ -4,12 +4,12 @@ Normally, for loading a `world`, Gazebo needs to be launched in the folder where
 
 ```
 source /usr/share/gazebo/setup.sh
-export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/PATH/WHERE/YOUR/WORLD/IS
+export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/PATH/TO/WORLD/FOLDER
 
 ```
 
 Furthermore, if you are working with Simulink controllers, it is necessary to sincronize Gazebo with Simulink by running Gazebo with the option `-slibgazebo_yarp_clock.so`. 
 
-Final command to run in the terminal is something like this: `gazebo -slibgazebo_yarp_clock.so nameOfYourWorld` where for iCub standup demo `nameOfYourWorld = icub_standup_world`.
+Final command to run in the terminal is something like this: `gazebo -slibgazebo_yarp_clock.so worldFolderName/nameOfYourWorld` where for iCub standup demo `worldFolderName/nameOfYourWorld = icub_standup_world/icub_standup.world`.
 
 

From 55f5594810b8b79ec908c14ac9acc7c30a13bdc4 Mon Sep 17 00:00:00 2001
From: Gabriele <gabriele.nava@iit.it>
Date: Wed, 21 Nov 2018 11:48:00 +0100
Subject: [PATCH 13/20] removed icub standup/seesaw and added standup/seesaw
 config directly in the world file

---
 models/icub_on_seesaw/icub.sdf               | 2184 -----------------
 models/icub_on_seesaw/model.config           |   16 -
 models/icub_standup/icub_standup.sdf         | 2236 ------------------
 models/icub_standup/model.config             |   16 -
 worlds/icub_seesaw_world/icub_seesaw.world   |   29 +-
 worlds/icub_standup_world/icub_standup.world |   29 +-
 6 files changed, 52 insertions(+), 4458 deletions(-)
 delete mode 100644 models/icub_on_seesaw/icub.sdf
 delete mode 100644 models/icub_on_seesaw/model.config
 delete mode 100644 models/icub_standup/icub_standup.sdf
 delete mode 100644 models/icub_standup/model.config

diff --git a/models/icub_on_seesaw/icub.sdf b/models/icub_on_seesaw/icub.sdf
deleted file mode 100644
index dd8cc67..0000000
--- a/models/icub_on_seesaw/icub.sdf
+++ /dev/null
@@ -1,2184 +0,0 @@
-<?xml version='1.0'?>
-<sdf version='1.5'>
-  <model name='iCub'>
-    <pose>-0.05 0 0.6926 0 0 3.14</pose>
-    <link name='root_link'>
-      <pose>0 0 0 0 -0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>4.72</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='root_link_collision'>
-        <pose>0 0 0 0 -0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_root_link.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='root_link_visual'>
-        <pose>0 0 0 0 -0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_root_link.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <link name='l_hip_1'>
-      <pose>0 -0.0681 -0.1199 2.65359e-06 1.57079 3.14159</pose>
-      <inertial>
-        <pose>0 -0.0782 0 0 -0 0</pose>
-        <mass>0.754</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>2.059e-06</ixy>
-          <ixz>1.451e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.545e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hip_1_collision'>
-        <pose>0 0 0 -0.785403 1.57079 -2.35619</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hip_1_visual'>
-        <pose>0 0 0 -0.785403 1.57079 -2.35619</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_pitch' type='revolute'>
-      <child>l_hip_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>2.65359e-06 1 -9.74718e-12</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>2.30383</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hip_2'>
-      <pose>0 -0.0681 -0.1199 3.14159 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0 0 0.03045 0 -0 0</pose>
-        <mass>0.526</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-7.4e-08</ixy>
-          <ixz>1.0835e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.2e-08</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hip_2_collision'>
-        <pose>0 0 0 0.785363 -1.57079 2.35623</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hip_2_visual'>
-        <pose>0 0 0 0.785363 -1.57079 2.35623</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_roll' type='revolute'>
-      <child>l_hip_2</child>
-      <parent>l_hip_1</parent>
-      <axis>
-        <xyz>-1 2.65359e-06 7.34641e-06</xyz>
-        <limit>
-          <lower>-0.296706</lower>
-          <upper>2.07694</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hip_3'>
-      <pose>-4.88536e-07 -0.0680998 -0.1864 3.14159 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_leg_ft_sensor' type='revolute'>
-      <child>l_hip_3</child>
-      <parent>l_hip_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_leg_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_leg_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='l_upper_leg'>
-      <pose>0 -0.0681 -0.1199 1.57079 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0.00144 -0.15943 -0.00039 0 -0 0</pose>
-        <mass>2.175</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-6.726e-05</ixy>
-          <ixz>2.267e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.63726e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_upper_leg_collision'>
-        <pose>0 0 0 -2.67797 1.57079 -1.10716</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_upper_leg_visual'>
-        <pose>0 0 0 -2.67797 1.57079 -1.10716</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_yaw' type='revolute'>
-      <child>l_upper_leg</child>
-      <parent>l_hip_3</parent>
-      <axis>
-        <xyz>7.34639e-06 -6.3268e-06 1</xyz>
-        <limit>
-          <lower>-1.37881</lower>
-          <upper>1.37881</upper>
-          <effort>40</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_lower_leg'>
-      <pose>-1.64265e-06 -0.0680986 -0.3435 2.09198 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.1071 0.00182 0.00211 0 -0 0</pose>
-        <mass>1.264</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-0.000185699</ixy>
-          <ixz>-6.3147e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>7.86e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_lower_leg_collision'>
-        <pose>5.55112e-17 2.28232e-12 2.2823e-12 -3.14159 -2.33487e-16 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_lower_leg_visual'>
-        <pose>5.55112e-17 2.28232e-12 2.2823e-12 -3.14159 -2.33487e-16 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_knee' type='revolute'>
-      <child>l_lower_leg</child>
-      <parent>l_upper_leg</parent>
-      <axis>
-        <xyz>5.30723e-06 1 6.32676e-06</xyz>
-        <limit>
-          <lower>-2.18166</lower>
-          <upper>0.401426</upper>
-          <effort>30</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_ankle_1'>
-      <pose>-3.98983e-06 -0.0680972 -0.5565 0.52118 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.0054 0.00163 0.0172 0 -0 0</pose>
-        <mass>0.746</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-7.081e-06</ixy>
-          <ixz>4.1421e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>2.0817e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_ankle_1_collision'>
-        <pose>1.11022e-16 -5.54343e-12 4.45647e-12 1.5708 1.53106e-11 1.53105e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_ankle_1_visual'>
-        <pose>1.11022e-16 -5.54343e-12 4.45647e-12 1.5708 1.53106e-11 1.53105e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_ankle_pitch' type='revolute'>
-      <child>l_ankle_1</child>
-      <parent>l_lower_leg</parent>
-      <axis>
-        <xyz>-8.98043e-06 -1 -6.32672e-06</xyz>
-        <limit>
-          <lower>-0.733038</lower>
-          <upper>0.366519</upper>
-          <effort>24</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_ankle_2'>
-      <pose>-3.98983e-06 -0.0680972 -0.5565 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0 0 0.037 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_ankle_roll' type='revolute'>
-      <child>l_ankle_2</child>
-      <parent>l_ankle_1</parent>
-      <axis>
-        <xyz>-1 8.98038e-06 7.34645e-06</xyz>
-        <limit>
-          <lower>-0.418879</lower>
-          <upper>0.418879</upper>
-          <effort>11</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_foot'>
-      <pose>-4.26164e-06 -0.068097 -0.5935 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0.024069 -0.000613931 -0.0284154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.51893e-06</ixy>
-          <ixz>0.000567785</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.62361e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_foot_collision'>
-        <pose>0.03 0 -0.01 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <!--<kp>100000</kp>-->
-              <kp>18000000</kp>
-              <!--<kd>1</kd>-->
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <!--<min_depth>0.001</min_depth>-->
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='l_foot_visual'>
-        <pose>4.45647e-12 -5.54343e-12 -0.037 -0.785403 1.57079 -0.785403</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_foot.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_foot_ft_sensor' type='revolute'>
-      <child>l_foot</child>
-      <parent>l_ankle_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_foot_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_foot_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_foot_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_hip_1'>
-      <pose>0 0.0681 -0.1199 3.14159 1.57079 -3.14159</pose>
-      <inertial>
-        <pose>0 -0.0782 0 0 -0 0</pose>
-        <mass>0.754</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hip_1_collision'>
-        <pose>0 0 0 1.76819 -1.57079 2.76109</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hip_1_visual'>
-        <pose>0 0 0 1.76819 -1.57079 2.76109</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_pitch' type='revolute'>
-      <child>r_hip_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>5.7211e-18 1 -1.22967e-22</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>2.30383</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hip_2'>
-      <pose>0 0.0681 -0.1199 -3.14159 -7.34641e-06 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0.03045 0 -0 0</pose>
-        <mass>0.526</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hip_2_collision'>
-        <pose>0 0 0 -2.35627 -1.57079 2.35627</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hip_2_visual'>
-        <pose>0 0 0 -2.35627 -1.57079 2.35627</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_roll' type='revolute'>
-      <child>r_hip_2</child>
-      <parent>r_hip_1</parent>
-      <axis>
-        <xyz>1 5.55111e-17 -7.34641e-06</xyz>
-        <limit>
-          <lower>-0.296706</lower>
-          <upper>2.07694</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hip_3'>
-      <pose>-4.88536e-07 0.0681 -0.1864 -3.14159 -7.34641e-06 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_leg_ft_sensor' type='revolute'>
-      <child>r_hip_3</child>
-      <parent>r_hip_2</parent>
-      <axis>
-        <xyz>0 0 -0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_leg_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_leg_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_upper_leg'>
-      <pose>0 0.0681 -0.1199 1.57079 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0.00144 -0.15943 0.00039 0 -0 0</pose>
-        <mass>2.175</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-2.71051e-20</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_upper_leg_collision'>
-        <pose>0 0 0 1.57087 -1.57079 -7.26536e-05</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_upper_leg_visual'>
-        <pose>0 0 0 1.57087 -1.57079 -7.26536e-05</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_yaw' type='revolute'>
-      <child>r_upper_leg</child>
-      <parent>r_hip_3</parent>
-      <axis>
-        <xyz>-7.34641e-06 3.67321e-06 -1</xyz>
-        <limit>
-          <lower>-1.37881</lower>
-          <upper>1.37881</upper>
-          <effort>40</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_lower_leg'>
-      <pose>-1.64266e-06 0.0681008 -0.3435 1.89254 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.1071 0.00182 -0.00211 0 -0 0</pose>
-        <mass>1.264</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>5.42101e-20</ixy>
-          <ixz>-5.42101e-20</ixz>
-          <iyy>0.01</iyy>
-          <iyz>8.47033e-22</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_lower_leg_collision'>
-        <pose>5.55112e-17 2.28232e-12 -8.32667e-17 3.14159 -1.14426e-17 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_lower_leg_visual'>
-        <pose>5.55112e-17 2.28232e-12 -8.32667e-17 3.14159 -1.14426e-17 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_knee' type='revolute'>
-      <child>r_lower_leg</child>
-      <parent>r_upper_leg</parent>
-      <axis>
-        <xyz>2.65362e-06 1 3.67318e-06</xyz>
-        <limit>
-          <lower>-2.18166</lower>
-          <upper>0.401426</upper>
-          <effort>30</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_ankle_1'>
-      <pose>-3.98984e-06 0.0681016 -0.5565 -2.81984 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.0054 0.00163 -0.0172 0 -0 0</pose>
-        <mass>0.746</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.69407e-21</ixy>
-          <ixz>1.35525e-20</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.38813e-21</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_ankle_1_collision'>
-        <pose>1.11022e-16 -1.08717e-12 -4.45647e-12 -1.5708 -1.53106e-11 5.10376e-12</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_ankle_1_visual'>
-        <pose>1.11022e-16 -1.08717e-12 -4.45647e-12 -1.5708 -1.53106e-11 5.10376e-12</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_ankle_pitch' type='revolute'>
-      <child>r_ankle_1</child>
-      <parent>r_lower_leg</parent>
-      <axis>
-        <xyz>1.01959e-06 -1 -3.67322e-06</xyz>
-        <limit>
-          <lower>-0.733038</lower>
-          <upper>0.366519</upper>
-          <effort>24</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_ankle_2'>
-      <pose>-3.98984e-06 0.0681016 -0.5565 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0.037 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_ankle_roll' type='revolute'>
-      <child>r_ankle_2</child>
-      <parent>r_ankle_1</parent>
-      <axis>
-        <xyz>1 2.79362e-06 -1.4236e-05</xyz>
-        <limit>
-          <lower>-0.418879</lower>
-          <upper>0.418879</upper>
-          <effort>11</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_foot'>
-      <pose>-4.51657e-06 0.0681019 -0.5935 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0.024069 0.000613931 -0.0284154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.51893e-06</ixy>
-          <ixz>0.000567784</ixz>
-          <iyy>0.01</iyy>
-          <iyz>6.62535e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_foot_collision'>
-        <pose>0.03 0 -0.01 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <!--<kp>100000</kp>-->
-              <kp>18000000</kp>
-              <!--<kd>1</kd>-->
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <!--<min_depth>0.001</min_depth>-->
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='r_foot_visual'>
-        <pose>4.45647e-12 1.08717e-12 -0.037 -2.81983 1.57079 0.321763</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_foot.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_foot_ft_sensor' type='revolute'>
-      <child>r_foot</child>
-      <parent>r_ankle_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_foot_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_foot_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_foot_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='torso_1'>
-      <pose>0 0 0 3.14159 -1.57079 -2.65359e-06</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.263e-05</ixy>
-          <ixz>-3.889e-08</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='torso_1_collision'>
-        <pose>0 0 0 -1.5708 5.10314e-12 1.11022e-16</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_lap_belt_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='torso_1_visual'>
-        <pose>0 0 0 -1.5708 5.10314e-12 1.11022e-16</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_lap_belt_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='torso_pitch' type='revolute'>
-      <child>torso_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>-2.65359e-06 -1 -1.05879e-22</xyz>
-        <limit>
-          <lower>-0.383972</lower>
-          <upper>1.46608</upper>
-          <effort>36</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='torso_2'>
-      <pose>1.17543e-07 -3.1191e-13 0.032 -3.14159 3.67321e-06 1.57079</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.923e-06</ixy>
-          <ixz>5.095e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.849e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='torso_roll' type='revolute'>
-      <child>torso_2</child>
-      <parent>torso_1</parent>
-      <axis>
-        <xyz>1 -2.6536e-06 -7.34641e-06</xyz>
-        <limit>
-          <lower>-0.680678</lower>
-          <upper>0.680678</upper>
-          <effort>80</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='chest'>
-      <pose>-0.00549988 1.45945e-08 0.032 1.5708 -7.34642e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00222898 0.075 -1.18362e-17 0 -0 0</pose>
-        <mass>4.81</mass>
-        <inertia>
-          <ixx>0.07472</ixx>
-          <ixy>-3.6e-06</ixy>
-          <ixz>-4.705e-05</ixz>
-          <iyy>0.08145</iyy>
-          <iyz>0.004567</iyz>
-          <izz>0.01306</izz>
-        </inertia>
-      </inertial>
-      <collision name='chest_collision'>
-        <pose>-0.0055 -6.93889e-18 -3.55271e-18 -1.10717 -1.57079 2.67798</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_chest.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='chest_visual'>
-        <pose>-0.0055 -6.93889e-18 -3.55271e-18 -1.10717 -1.57079 2.67798</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_chest.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='torso_yaw' type='revolute'>
-      <child>chest</child>
-      <parent>torso_2</parent>
-      <axis>
-        <xyz>-7.34642e-06 1.94942e-11 -1</xyz>
-        <limit>
-          <lower>-1.02974</lower>
-          <upper>1.02974</upper>
-          <effort>80</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_1'>
-      <pose>-0.0107487 -0.11026 0.1753 -0.261794 -1.57079 -1.42917e-06</pose>
-      <inertial>
-        <pose>-5e-06 0.0187 -0.00119 0 -0 0</pose>
-        <mass>0.189</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>9e-09</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.7e-08</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_1_collision'>
-        <pose>3.08642e-14 0.109285 -0.00521101 -0.301063 1.57079 -1.87186</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_shoulder_1_visual'>
-        <pose>3.08642e-14 0.109285 -0.00521101 -0.301063 1.57079 -1.87186</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_shoulder_pitch' type='revolute'>
-      <child>l_shoulder_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>-0.258815 -0.965927 2.85203e-06</xyz>
-        <limit>
-          <lower>-1.65806</lower>
-          <upper>0.0872665</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_2'>
-      <pose>-0.0107487 -0.11026 0.1753 3.14159 8.21174e-07 -1.83259</pose>
-      <inertial>
-        <pose>9.4e-05 -0.00627 0.0166 0 -0 0</pose>
-        <mass>0.179</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.66e-07</ixy>
-          <ixz>3.65e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-2.0524e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_2_collision'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -2.14033 1.57079 -0.569533</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_shoulder_2_visual'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -2.14033 1.57079 -0.569533</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_shoulder_roll' type='revolute'>
-      <child>l_shoulder_2</child>
-      <parent>l_shoulder_1</parent>
-      <axis>
-        <xyz>-0.965927 0.258815 6.97095e-06</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>2.80649</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_3'>
-      <pose>-0.0107487 -0.11026 0.1753 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00559398 9.77357e-05 0.0501346 0 -0 0</pose>
-        <mass>0.156157</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.662e-08</ixy>
-          <ixz>-4.3618e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-7.42844e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_shoulder_yaw' type='revolute'>
-      <child>l_shoulder_3</child>
-      <parent>l_shoulder_2</parent>
-      <axis>
-        <xyz>-6.5209e-06 2.59738e-06 -1</xyz>
-        <limit>
-          <lower>-0.645772</lower>
-          <upper>1.74533</upper>
-          <effort>34</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_upper_arm'>
-      <pose>-0.0257492 -0.11026 0.1073 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.000156 -9.87e-05 0.0298 0 -0 0</pose>
-        <mass>0.727843</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.08e-06</ixy>
-          <ixz>-2.29e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.57e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_upper_arm_collision'>
-        <pose>-0.0204334 0.000143922 -0.068 1.15274e-11 4.92958e-12 -1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_upper_arm_visual'>
-        <pose>-0.0204334 0.000143922 -0.068 1.15274e-11 4.92958e-12 -1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_arm_ft_sensor' type='revolute'>
-      <child>l_upper_arm</child>
-      <parent>l_shoulder_3</parent>
-      <axis>
-        <xyz>-0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_arm_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_arm_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='l_elbow_1'>
-      <pose>-0.0257497 -0.11026 0.0230202 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.0013 0.00371 -0.00105 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.15e-07</ixy>
-          <ixz>-4.08e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.71e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_elbow' type='revolute'>
-      <child>l_elbow_1</child>
-      <parent>l_upper_arm</parent>
-      <axis>
-        <xyz>-1.01957e-06 1 2.59739e-06</xyz>
-        <limit>
-          <lower>0.0959931</lower>
-          <upper>1.85005</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_forearm'>
-      <pose>-0.0107497 -0.11026 0.0230201 -1.5708 2.59741e-06 -1.57079</pose>
-      <inertial>
-        <pose>0.000347 0.066 0.00476 0 -0 0</pose>
-        <mass>0.525</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.337e-06</ixy>
-          <ixz>2.39e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.9381e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_forearm_collision'>
-        <pose>-0.000143922 -1.25974e-12 0.0204334 -2.66395 1.57079 -1.09315</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_forearm_visual'>
-        <pose>-0.000143922 -1.25974e-12 0.0204334 -2.66395 1.57079 -1.09315</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_wrist_prosup' type='revolute'>
-      <child>l_forearm</child>
-      <parent>l_elbow_1</parent>
-      <axis>
-        <xyz>-2.8477e-06 2.5974e-06 -1</xyz>
-        <limit>
-          <lower>-0.872665</lower>
-          <upper>0.872665</upper>
-          <effort>0.45</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_wrist_1'>
-      <pose>-0.0107501 -0.110259 -0.11428 -2.7153 1.57079 1.99709</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_wrist_pitch' type='revolute'>
-      <child>l_wrist_1</child>
-      <parent>l_forearm</parent>
-      <axis>
-        <xyz>1 1.01958e-06 -2.84771e-06</xyz>
-        <limit>
-          <lower>-1.13446</lower>
-          <upper>0.174533</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hand'>
-      <pose>-0.0107501 -0.110259 -0.11428 -2.7153 1.57079 1.99709</pose>
-      <inertial>
-        <pose>0.07023 -0.00805 -0.007 0 -0 0</pose>
-        <mass>0.213</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.278e-05</ixy>
-          <ixz>4.823e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.8188e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hand_collision'>
-        <pose>-1.25973e-12 0.0204334 -0.000143922 5.10271e-12 -2.64186e-12 1.49622e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hand_visual'>
-        <pose>-1.25973e-12 0.0204334 -0.000143922 5.10271e-12 -2.64186e-12 1.49622e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_wrist_yaw' type='revolute'>
-      <child>l_hand</child>
-      <parent>l_wrist_1</parent>
-      <axis>
-        <xyz>1.01956e-06 -1 -6.27061e-06</xyz>
-        <limit>
-          <lower>-0.436332</lower>
-          <upper>0.436332</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='neck_1'>
-      <pose>-0.00780846 1.22355e-08 0.2253 6.12199e-06 -1.57079 -1.42918e-06</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_1_collision'>
-        <pose>-5.55112e-17 9.62889e-13 -0.0055 3.14159 -1.57079 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_neck_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='neck_1_visual'>
-        <pose>-5.55112e-17 9.62889e-13 -0.0055 3.14159 -1.57079 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_neck_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='neck_pitch' type='revolute'>
-      <child>neck_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>-4.69281e-06 1 6.74621e-11</xyz>
-        <limit>
-          <lower>-0.523599</lower>
-          <upper>0.383972</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-    <link name='neck_2'>
-      <pose>-0.00780836 1.22353e-08 0.2348 -1.46928e-05 3.67327e-06 -1.57079</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_2_collision'>
-        <pose>-9.62885e-13 0.0055 0.0235 -3.14157 -1.57079 -1.57082</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_neck_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='neck_2_visual'>
-        <pose>-9.62885e-13 0.0055 0.0235 -3.14157 -1.57079 -1.57082</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_neck_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='neck_roll' type='revolute'>
-      <child>neck_2</child>
-      <parent>neck_1</parent>
-      <axis>
-        <xyz>-1 -4.69282e-06 1.46928e-05</xyz>
-        <limit>
-          <lower>-0.349066</lower>
-          <upper>0.349066</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-    <link name='head'>
-      <pose>-0.00780836 1.22353e-08 0.2348 -1.5708 1.46928e-05 8.36603e-06</pose>
-      <inertial>
-        <pose>0.0185 -0.1108 6.78454e-18 0 -0 0</pose>
-        <mass>3</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='head_collision'>
-        <pose>0.0045 -0.0235 9.42472e-13 -1.32582 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_head.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='head_visual'>
-     <!--   <pose>0.0045 -0.0235 9.42472e-13 -1.32582 1.57079 -2.89662</pose>-->
-
-       <pose>0.0045 -0.0235 9.42472e-13 -2.89662 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head_without_eyes.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <sensor name='imu_sensor' type='imu'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <visualize>1</visualize>
-        <imu/>
-        <plugin name='iCub_yarp_gazebo_plugin_IMU' filename='libgazebo_yarp_imu.so'>
-          <yarpConfigurationFile>model://icub/conf/gazebo_icub_inertial.ini</yarpConfigurationFile>
-        </plugin>
-        <pose>0.0185 -0.1108 0.0066 1.5708 -0 0</pose>
-      </sensor>
-    </link>
-    <joint name='neck_yaw' type='revolute'>
-      <child>head</child>
-      <parent>neck_2</parent>
-      <axis>
-        <xyz>1.46928e-05 1.48834e-12 1</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>0.767945</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <link name='eyes_tilt_link'>
-      <pose>-62.81e-03 0e-03 340.8e-03 1.57079 0 0</pose>
-      <inertial>
-        <pose>0.0 0.0 0.0 0 0 0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-  <!--     <visual name='eyes_tilt_visual'>
-        <pose>0.0 0.0 0.0 0 0 0</pose>
-        <geometry>
-          <cylinder><radius>0.01</radius><length>0.05</length></cylinder> 
-        </geometry>
-      </visual> -->
-    </link>
-
-    <link name='left_eye_link'>
-      <pose>-62.81e-03 -34e-03 340.8e-03 -3.14159 0 0</pose>
-      <inertial>
-        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-       <visual name='left_eye_visual'>
-         <pose>0.055 0.033 -0.14 0 0 0</pose> 
-        <geometry>
-             <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_eye.dae</uri>
-          </mesh> 
-   <!-- <cylinder><radius>0.01</radius><length>0.05</length></cylinder> -->
-        </geometry>
-      </visual>
-    </link>
-
-    <link name='right_eye_link'>
-      <pose> -62.81e-03 34e-03 340.8e-03 -3.14159 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-      <visual name='right_eye_visual'>
-        <pose>0.055 0.033 -0.14 0 0 0</pose> 
-        <geometry>
- <!-- <cylinder><radius>0.01</radius><length>0.05</length></cylinder>-->
-            <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_eye.dae</uri>
-          </mesh> 
-        </geometry>
-      </visual>
-    </link>
-
-    <joint name='eyes_tilt' type='revolute'>
-      <child>eyes_tilt_link</child>
-      <parent>head</parent>
-      <axis>
-        <xyz>0.0 1.0 0.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <joint name='left_eye' type='revolute'>
-      <child>left_eye_link</child>
-      <parent>eyes_tilt_link</parent>
-      <axis>
-        <xyz>0.0 0.0 1.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <joint name='right_eye' type='revolute'>
-      <child>right_eye_link</child>
-      <parent>eyes_tilt_link</parent>
-      <axis>
-        <xyz>0.0 0.0 1.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <link name='r_shoulder_1'>
-      <pose>-0.0107489 0.11026 0.1753 -2.87979 -1.57079 -1.42916e-06</pose>
-      <inertial>
-        <pose>5e-06 0.0187 0.00119 0 -0 0</pose>
-        <mass>0.189</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>2.1e-08</ixy>
-          <ixz>-1e-09</ixz>
-          <iyy>0.01</iyy>
-          <iyz>4.22e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_1_collision'>
-        <pose>-3.08642e-14 0.109285 0.00521101 3.06003 1.57079 -1.65235</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_shoulder_1_visual'>
-        <pose>-3.08642e-14 0.109285 0.00521101 3.06003 1.57079 -1.65235</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_shoulder_pitch' type='revolute'>
-      <child>r_shoulder_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>0.258817 -0.965926 -2.85208e-06</xyz>
-        <limit>
-          <lower>-1.65806</lower>
-          <upper>0.0872665</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_shoulder_2'>
-      <pose>-0.0107489 0.11026 0.1753 6.97094e-06 8.21122e-07 1.83259</pose>
-      <inertial>
-        <pose>-9.4e-05 -0.00627 -0.0166 0 -0 0</pose>
-        <mass>0.179</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.53e-07</ixy>
-          <ixz>2.03e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>2.07e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_2_collision'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -1.00125 -1.57079 2.57205</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_shoulder_2_visual'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -1.00125 -1.57079 2.57205</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_shoulder_roll' type='revolute'>
-      <child>r_shoulder_2</child>
-      <parent>r_shoulder_1</parent>
-      <axis>
-        <xyz>0.965926 0.258817 -6.97094e-06</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>2.80649</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_shoulder_3'>
-      <pose>-0.0107489 0.11026 0.1753 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00553932 -2.47652e-05 0.0493895 0 -0 0</pose>
-        <mass>0.155</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>3.56977e-07</ixy>
-          <ixz>-3.95372e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.5128e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_shoulder_yaw' type='revolute'>
-      <child>r_shoulder_3</child>
-      <parent>r_shoulder_2</parent>
-      <axis>
-        <xyz>6.5209e-06 -5.62481e-08 1</xyz>
-        <limit>
-          <lower>-0.645772</lower>
-          <upper>1.74533</upper>
-          <effort>34</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_upper_arm'>
-      <pose>-0.0257494 0.11026 0.1073 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.00015906 8.28733e-05 0.0298828 0 -0 0</pose>
-        <mass>0.729</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.08e-06</ixy>
-          <ixz>-2.29e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.57e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_upper_arm_collision'>
-        <pose>-0.0204334 -0.000143922 -0.068 -3.14159 -1.51363e-11 1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_upper_arm_visual'>
-        <pose>-0.0204334 -0.000143922 -0.068 -3.14159 -1.51363e-11 1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_arm_ft_sensor' type='revolute'>
-      <child>r_upper_arm</child>
-      <parent>r_shoulder_3</parent>
-      <axis>
-        <xyz>-0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_arm_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_arm_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_arm_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_elbow_1'>
-      <pose>-0.0257499 0.11026 0.0230202 -2.59732e-06 6.5209e-06 3.6732e-06</pose>
-      <inertial>
-        <pose>0.0013 -0.00371 0.00105 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.02e-07</ixy>
-          <ixz>-3.99e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.71e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_elbow' type='revolute'>
-      <child>r_elbow_1</child>
-      <parent>r_upper_arm</parent>
-      <axis>
-        <xyz>-3.67322e-06 1 -2.59732e-06</xyz>
-        <limit>
-          <lower>0.0959931</lower>
-          <upper>1.85005</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_forearm'>
-      <pose>-0.0107499 0.11026 0.0230201 1.57079 -2.59734e-06 -1.5708</pose>
-      <inertial>
-        <pose>-0.000347 -0.066 -0.00476 0 -0 0</pose>
-        <mass>0.525</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>5.66e-06</ixy>
-          <ixz>1.4e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.82e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_forearm_collision'>
-        <pose>0.000143922 -1.07214e-12 -0.0204334 -1.74165 1.57079 -0.170863</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_forearm_visual'>
-        <pose>0.000143922 -1.07214e-12 -0.0204334 -1.74165 1.57079 -0.170863</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_wrist_prosup' type='revolute'>
-      <child>r_forearm</child>
-      <parent>r_elbow_1</parent>
-      <axis>
-        <xyz>2.84768e-06 2.59734e-06 1</xyz>
-        <limit>
-          <lower>-0.872665</lower>
-          <upper>0.872665</upper>
-          <effort>0.45</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_wrist_1'>
-      <pose>-0.0107503 0.11026 -0.11428 0.426292 -1.57079 1.14451</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_wrist_pitch' type='revolute'>
-      <child>r_wrist_1</child>
-      <parent>r_forearm</parent>
-      <axis>
-        <xyz>-1 -3.67321e-06 2.84771e-06</xyz>
-        <limit>
-          <lower>-1.13446</lower>
-          <upper>0.174533</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hand'>
-      <pose>-0.0107503 0.11026 -0.11428 -0.720145 1.57079 -2.29094</pose>
-      <inertial>
-        <pose>0.07023 -0.00805 0.007 0 -0 0</pose>
-        <mass>0.213</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.26e-05</ixy>
-          <ixz>-6.08e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.76e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hand_collision'>
-        <pose>1.0721e-12 0.0204334 0.000143922 1.53098e-11 2.00656e-11 -3.14159</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hand_visual'>
-        <pose>1.0721e-12 0.0204334 0.000143922 1.53098e-11 2.00656e-11 -3.14159</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_wrist_yaw' type='revolute'>
-      <child>r_hand</child>
-      <parent>r_wrist_1</parent>
-      <axis>
-        <xyz>3.67323e-06 -1 6.27054e-06</xyz>
-        <limit>
-          <lower>-0.436332</lower>
-          <upper>0.436332</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <plugin name='controlboard_torso' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_torso.ini</yarpConfigurationFile>
-      <initialConfiguration> 0 0 0.0824</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_head' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_head.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_left_arm_no_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm_no_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-      <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-      <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
-    </plugin>
-    <plugin name="apply_external_wrench" filename="libgazebo_yarp_externalwrench.so" >
-      <robotNamefromConfigFile>model://icub/conf/gazebo_icub_robotname.ini</robotNamefromConfigFile>>
-    </plugin>
-    <pose>0 0 0.7 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/models/icub_on_seesaw/model.config b/models/icub_on_seesaw/model.config
deleted file mode 100644
index f855ec9..0000000
--- a/models/icub_on_seesaw/model.config
+++ /dev/null
@@ -1,16 +0,0 @@
-<?xml version="1.0"?>
-<model>
-    <name>iCub on seesaw</name>
-    
-    <version>1.0</version>
-    <sdf version='1.5'>icub.sdf</sdf>
-    
-    <author>
-        <name>Silvio Traversaro</name>
-        <email>silvio.traversaro@iit.it</email>
-    </author>
-    
-    <description>
-        Model for the iCub humanoid robot. For more information check icub.org .
-    </description>
-</model>
diff --git a/models/icub_standup/icub_standup.sdf b/models/icub_standup/icub_standup.sdf
deleted file mode 100644
index 37aaede..0000000
--- a/models/icub_standup/icub_standup.sdf
+++ /dev/null
@@ -1,2236 +0,0 @@
-<?xml version='1.0'?>
-<sdf version='1.5'>
-  <model name='iCub'>
-    <pose>-0.077649 0.000965 0.389694 -8e-06 0.036487 3.14094</pose>
-    <link name='root_link'>
-      <pose>0 0 0 0 -0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>4.72</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='root_link_collision'>
-        <pose>0 0 0 0 -0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_root_link.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='root_link_visual'>
-        <pose>0 0 0 0 -0 0</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_root_link.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <link name='l_hip_1'>
-      <pose>0 -0.0681 -0.1199 2.65359e-06 1.57079 3.14159</pose>
-      <inertial>
-        <pose>0 -0.0782 0 0 -0 0</pose>
-        <mass>0.754</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>2.059e-06</ixy>
-          <ixz>1.451e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.545e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hip_1_collision'>
-        <pose>0 0 0 -0.785403 1.57079 -2.35619</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hip_1_visual'>
-        <pose>0 0 0 -0.785403 1.57079 -2.35619</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_pitch' type='revolute'>
-      <child>l_hip_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>2.65359e-06 1 -9.74718e-12</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>2.30383</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hip_2'>
-      <pose>0 -0.0681 -0.1199 3.14159 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0 0 0.03045 0 -0 0</pose>
-        <mass>0.526</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-7.4e-08</ixy>
-          <ixz>1.0835e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.2e-08</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hip_2_collision'>
-        <pose>0 0 0 0.785363 -1.57079 2.35623</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hip_2_visual'>
-        <pose>0 0 0 0.785363 -1.57079 2.35623</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_roll' type='revolute'>
-      <child>l_hip_2</child>
-      <parent>l_hip_1</parent>
-      <axis>
-        <xyz>-1 2.65359e-06 7.34641e-06</xyz>
-        <limit>
-          <lower>-0.296706</lower>
-          <upper>2.07694</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hip_3'>
-      <pose>-4.88536e-07 -0.0680998 -0.1864 3.14159 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_leg_ft_sensor' type='revolute'>
-      <child>l_hip_3</child>
-      <parent>l_hip_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_leg_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_leg_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='l_upper_leg'>
-      <pose>0 -0.0681 -0.1199 1.57079 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0.00144 -0.15943 -0.00039 0 -0 0</pose>
-        <mass>2.175</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-6.726e-05</ixy>
-          <ixz>2.267e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.63726e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_upper_leg_collision'>
-        <pose>0 0 0 -2.67797 1.57079 -1.10716</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_thigh.dae</uri>
-          </mesh>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <kp>18000000</kp>
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='l_upper_leg_visual'>
-        <pose>0 0 0 -2.67797 1.57079 -1.10716</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_hip_yaw' type='revolute'>
-      <child>l_upper_leg</child>
-      <parent>l_hip_3</parent>
-      <axis>
-        <xyz>7.34639e-06 -6.3268e-06 1</xyz>
-        <limit>
-          <lower>-1.37881</lower>
-          <upper>1.37881</upper>
-          <effort>40</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_lower_leg'>
-      <pose>-1.64265e-06 -0.0680986 -0.3435 2.09198 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.1071 0.00182 0.00211 0 -0 0</pose>
-        <mass>1.264</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-0.000185699</ixy>
-          <ixz>-6.3147e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>7.86e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_lower_leg_collision'>
-        <pose>5.55112e-17 2.28232e-12 2.2823e-12 -3.14159 -2.33487e-16 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_lower_leg_visual'>
-        <pose>5.55112e-17 2.28232e-12 2.2823e-12 -3.14159 -2.33487e-16 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_knee' type='revolute'>
-      <child>l_lower_leg</child>
-      <parent>l_upper_leg</parent>
-      <axis>
-        <xyz>5.30723e-06 1 6.32676e-06</xyz>
-        <limit>
-          <lower>-2.18166</lower>
-          <upper>0.401426</upper>
-          <effort>30</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_ankle_1'>
-      <pose>-3.98983e-06 -0.0680972 -0.5565 0.52118 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.0054 0.00163 0.0172 0 -0 0</pose>
-        <mass>0.746</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-7.081e-06</ixy>
-          <ixz>4.1421e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>2.0817e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_ankle_1_collision'>
-        <pose>1.11022e-16 -5.54343e-12 4.45647e-12 1.5708 1.53106e-11 1.53105e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_ankle_1_visual'>
-        <pose>1.11022e-16 -5.54343e-12 4.45647e-12 1.5708 1.53106e-11 1.53105e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_ankle_pitch' type='revolute'>
-      <child>l_ankle_1</child>
-      <parent>l_lower_leg</parent>
-      <axis>
-        <xyz>-8.98043e-06 -1 -6.32672e-06</xyz>
-        <limit>
-          <lower>-0.733038</lower>
-          <upper>0.366519</upper>
-          <effort>24</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_ankle_2'>
-      <pose>-3.98983e-06 -0.0680972 -0.5565 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0 0 0.037 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_ankle_roll' type='revolute'>
-      <child>l_ankle_2</child>
-      <parent>l_ankle_1</parent>
-      <axis>
-        <xyz>-1 8.98038e-06 7.34645e-06</xyz>
-        <limit>
-          <lower>-0.418879</lower>
-          <upper>0.418879</upper>
-          <effort>11</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_foot'>
-      <pose>-4.26164e-06 -0.068097 -0.5935 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0.024069 -0.000613931 -0.0284154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.51893e-06</ixy>
-          <ixz>0.000567785</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.62361e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_foot_collision'>
-        <pose>0.03 0 -0.01 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <!--<kp>100000</kp>-->
-              <kp>18000000</kp>
-              <!--<kd>1</kd>-->
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <!--<min_depth>0.001</min_depth>-->
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='l_foot_visual'>
-        <pose>4.45647e-12 -5.54343e-12 -0.037 -0.785403 1.57079 -0.785403</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_foot.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_foot_ft_sensor' type='revolute'>
-      <child>l_foot</child>
-      <parent>l_ankle_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_foot_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_foot_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_foot_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_hip_1'>
-      <pose>0 0.0681 -0.1199 3.14159 1.57079 -3.14159</pose>
-      <inertial>
-        <pose>0 -0.0782 0 0 -0 0</pose>
-        <mass>0.754</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hip_1_collision'>
-        <pose>0 0 0 1.76819 -1.57079 2.76109</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hip_1_visual'>
-        <pose>0 0 0 1.76819 -1.57079 2.76109</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hip_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_pitch' type='revolute'>
-      <child>r_hip_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>5.7211e-18 1 -1.22967e-22</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>2.30383</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hip_2'>
-      <pose>0 0.0681 -0.1199 -3.14159 -7.34641e-06 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0.03045 0 -0 0</pose>
-        <mass>0.526</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hip_2_collision'>
-        <pose>0 0 0 -2.35627 -1.57079 2.35627</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hip_2_visual'>
-        <pose>0 0 0 -2.35627 -1.57079 2.35627</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hip_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_roll' type='revolute'>
-      <child>r_hip_2</child>
-      <parent>r_hip_1</parent>
-      <axis>
-        <xyz>1 5.55111e-17 -7.34641e-06</xyz>
-        <limit>
-          <lower>-0.296706</lower>
-          <upper>2.07694</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hip_3'>
-      <pose>-4.88536e-07 0.0681 -0.1864 -3.14159 -7.34641e-06 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_leg_ft_sensor' type='revolute'>
-      <child>r_hip_3</child>
-      <parent>r_hip_2</parent>
-      <axis>
-        <xyz>0 0 -0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_leg_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_leg_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_upper_leg'>
-      <pose>0 0.0681 -0.1199 1.57079 -7.34641e-06 3.14159</pose>
-      <inertial>
-        <pose>0.00144 -0.15943 0.00039 0 -0 0</pose>
-        <mass>2.175</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-2.71051e-20</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_upper_leg_collision'>
-        <pose>0 0 0 1.57087 -1.57079 -7.26536e-05</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_thigh.dae</uri>
-          </mesh>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <kp>18000000</kp>
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='r_upper_leg_visual'>
-        <pose>0 0 0 1.57087 -1.57079 -7.26536e-05</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_thigh.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_hip_yaw' type='revolute'>
-      <child>r_upper_leg</child>
-      <parent>r_hip_3</parent>
-      <axis>
-        <xyz>-7.34641e-06 3.67321e-06 -1</xyz>
-        <limit>
-          <lower>-1.37881</lower>
-          <upper>1.37881</upper>
-          <effort>40</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_lower_leg'>
-      <pose>-1.64266e-06 0.0681008 -0.3435 1.89254 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.1071 0.00182 -0.00211 0 -0 0</pose>
-        <mass>1.264</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>5.42101e-20</ixy>
-          <ixz>-5.42101e-20</ixz>
-          <iyy>0.01</iyy>
-          <iyz>8.47033e-22</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_lower_leg_collision'>
-        <pose>5.55112e-17 2.28232e-12 -8.32667e-17 3.14159 -1.14426e-17 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_lower_leg_visual'>
-        <pose>5.55112e-17 2.28232e-12 -8.32667e-17 3.14159 -1.14426e-17 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shank.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_knee' type='revolute'>
-      <child>r_lower_leg</child>
-      <parent>r_upper_leg</parent>
-      <axis>
-        <xyz>2.65362e-06 1 3.67318e-06</xyz>
-        <limit>
-          <lower>-2.18166</lower>
-          <upper>0.401426</upper>
-          <effort>30</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_ankle_1'>
-      <pose>-3.98984e-06 0.0681016 -0.5565 -2.81984 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.0054 0.00163 -0.0172 0 -0 0</pose>
-        <mass>0.746</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.69407e-21</ixy>
-          <ixz>1.35525e-20</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.38813e-21</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_ankle_1_collision'>
-        <pose>1.11022e-16 -1.08717e-12 -4.45647e-12 -1.5708 -1.53106e-11 5.10376e-12</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_ankle_1_visual'>
-        <pose>1.11022e-16 -1.08717e-12 -4.45647e-12 -1.5708 -1.53106e-11 5.10376e-12</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_ankle_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_ankle_pitch' type='revolute'>
-      <child>r_ankle_1</child>
-      <parent>r_lower_leg</parent>
-      <axis>
-        <xyz>1.01959e-06 -1 -3.67322e-06</xyz>
-        <limit>
-          <lower>-0.733038</lower>
-          <upper>0.366519</upper>
-          <effort>24</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_ankle_2'>
-      <pose>-3.98984e-06 0.0681016 -0.5565 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0 0 0.037 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_ankle_roll' type='revolute'>
-      <child>r_ankle_2</child>
-      <parent>r_ankle_1</parent>
-      <axis>
-        <xyz>1 2.79362e-06 -1.4236e-05</xyz>
-        <limit>
-          <lower>-0.418879</lower>
-          <upper>0.418879</upper>
-          <effort>11</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_foot'>
-      <pose>-4.51657e-06 0.0681019 -0.5935 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0.024069 0.000613931 -0.0284154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.51893e-06</ixy>
-          <ixz>0.000567784</ixz>
-          <iyy>0.01</iyy>
-          <iyz>6.62535e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_foot_collision'>
-        <pose>0.03 0 -0.01 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-        <max_contacts>4</max_contacts>
-        <surface>
-          <contact>
-            <ode>
-              <soft_erp>0.2</soft_erp>
-              <soft_cfm>0</soft_cfm>
-              <!--<kp>100000</kp>-->
-              <kp>18000000</kp>
-              <!--<kd>1</kd>-->
-              <kd>100</kd>
-              <max_vel>100</max_vel>
-              <!--<min_depth>0.001</min_depth>-->
-              <min_depth>0.0001</min_depth>
-            </ode>
-          </contact>
-          <friction>
-            <ode>
-              <mu>1</mu>
-              <mu2>1</mu2>
-              <fdir1>0 0 0</fdir1>
-              <slip1>0</slip1>
-              <slip2>0</slip2>
-            </ode>
-          </friction>
-          <bounce>
-            <restitution_coefficient>0</restitution_coefficient>
-            <threshold>100000</threshold>
-          </bounce>
-        </surface>
-      </collision>
-      <visual name='r_foot_visual'>
-        <pose>4.45647e-12 1.08717e-12 -0.037 -2.81983 1.57079 0.321763</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_foot.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_foot_ft_sensor' type='revolute'>
-      <child>r_foot</child>
-      <parent>r_ankle_2</parent>
-      <axis>
-        <xyz>0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_foot_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_foot_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_foot_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='torso_1'>
-      <pose>0 0 0 3.14159 -1.57079 -2.65359e-06</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.263e-05</ixy>
-          <ixz>-3.889e-08</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='torso_1_collision'>
-        <pose>0 0 0 -1.5708 5.10314e-12 1.11022e-16</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_lap_belt_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='torso_1_visual'>
-        <pose>0 0 0 -1.5708 5.10314e-12 1.11022e-16</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_lap_belt_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='torso_pitch' type='revolute'>
-      <child>torso_1</child>
-      <parent>root_link</parent>
-      <axis>
-        <xyz>-2.65359e-06 -1 -1.05879e-22</xyz>
-        <limit>
-          <lower>-0.383972</lower>
-          <upper>1.46608</upper>
-          <effort>36</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='torso_2'>
-      <pose>1.17543e-07 -3.1191e-13 0.032 -3.14159 3.67321e-06 1.57079</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.923e-06</ixy>
-          <ixz>5.095e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.849e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='torso_roll' type='revolute'>
-      <child>torso_2</child>
-      <parent>torso_1</parent>
-      <axis>
-        <xyz>1 -2.6536e-06 -7.34641e-06</xyz>
-        <limit>
-          <lower>-0.680678</lower>
-          <upper>0.680678</upper>
-          <effort>80</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='chest'>
-      <pose>-0.00549988 1.45945e-08 0.032 1.5708 -7.34642e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00222898 0.075 -1.18362e-17 0 -0 0</pose>
-        <mass>4.81</mass>
-        <inertia>
-          <ixx>0.07472</ixx>
-          <ixy>-3.6e-06</ixy>
-          <ixz>-4.705e-05</ixz>
-          <iyy>0.08145</iyy>
-          <iyz>0.004567</iyz>
-          <izz>0.01306</izz>
-        </inertia>
-      </inertial>
-      <collision name='chest_collision'>
-        <pose>-0.0055 -6.93889e-18 -3.55271e-18 -1.10717 -1.57079 2.67798</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_chest.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='chest_visual'>
-        <pose>-0.0055 -6.93889e-18 -3.55271e-18 -1.10717 -1.57079 2.67798</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_chest.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='torso_yaw' type='revolute'>
-      <child>chest</child>
-      <parent>torso_2</parent>
-      <axis>
-        <xyz>-7.34642e-06 1.94942e-11 -1</xyz>
-        <limit>
-          <lower>-1.02974</lower>
-          <upper>1.02974</upper>
-          <effort>80</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_1'>
-      <pose>-0.0107487 -0.11026 0.1753 -0.261794 -1.57079 -1.42917e-06</pose>
-      <inertial>
-        <pose>-5e-06 0.0187 -0.00119 0 -0 0</pose>
-        <mass>0.189</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>9e-09</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.7e-08</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_1_collision'>
-        <pose>3.08642e-14 0.109285 -0.00521101 -0.301063 1.57079 -1.87186</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_shoulder_1_visual'>
-        <pose>3.08642e-14 0.109285 -0.00521101 -0.301063 1.57079 -1.87186</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_shoulder_pitch' type='revolute'>
-      <child>l_shoulder_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>-0.258815 -0.965927 2.85203e-06</xyz>
-        <limit>
-          <lower>-1.65806</lower>
-          <upper>0.0872665</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_2'>
-      <pose>-0.0107487 -0.11026 0.1753 3.14159 8.21174e-07 -1.83259</pose>
-      <inertial>
-        <pose>9.4e-05 -0.00627 0.0166 0 -0 0</pose>
-        <mass>0.179</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.66e-07</ixy>
-          <ixz>3.65e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-2.0524e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_2_collision'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -2.14033 1.57079 -0.569533</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_shoulder_2_visual'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -2.14033 1.57079 -0.569533</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_shoulder_roll' type='revolute'>
-      <child>l_shoulder_2</child>
-      <parent>l_shoulder_1</parent>
-      <axis>
-        <xyz>-0.965927 0.258815 6.97095e-06</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>2.80649</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_shoulder_3'>
-      <pose>-0.0107487 -0.11026 0.1753 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00559398 9.77357e-05 0.0501346 0 -0 0</pose>
-        <mass>0.156157</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.662e-08</ixy>
-          <ixz>-4.3618e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-7.42844e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_shoulder_yaw' type='revolute'>
-      <child>l_shoulder_3</child>
-      <parent>l_shoulder_2</parent>
-      <axis>
-        <xyz>-6.5209e-06 2.59738e-06 -1</xyz>
-        <limit>
-          <lower>-0.645772</lower>
-          <upper>1.74533</upper>
-          <effort>34</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_upper_arm'>
-      <pose>-0.0257492 -0.11026 0.1073 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.000156 -9.87e-05 0.0298 0 -0 0</pose>
-        <mass>0.727843</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.08e-06</ixy>
-          <ixz>-2.29e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.57e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_upper_arm_collision'>
-        <pose>-0.0204334 0.000143922 -0.068 1.15274e-11 4.92958e-12 -1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_upper_arm_visual'>
-        <pose>-0.0204334 0.000143922 -0.068 1.15274e-11 4.92958e-12 -1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_arm_ft_sensor' type='revolute'>
-      <child>l_upper_arm</child>
-      <parent>l_shoulder_3</parent>
-      <axis>
-        <xyz>-0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='left_arm_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='left_arm_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='l_elbow_1'>
-      <pose>-0.0257497 -0.11026 0.0230202 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.0013 0.00371 -0.00105 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.15e-07</ixy>
-          <ixz>-4.08e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.71e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_elbow' type='revolute'>
-      <child>l_elbow_1</child>
-      <parent>l_upper_arm</parent>
-      <axis>
-        <xyz>-1.01957e-06 1 2.59739e-06</xyz>
-        <limit>
-          <lower>0.0959931</lower>
-          <upper>1.85005</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_forearm'>
-      <pose>-0.0107497 -0.11026 0.0230201 -1.5708 2.59741e-06 -1.57079</pose>
-      <inertial>
-        <pose>0.000347 0.066 0.00476 0 -0 0</pose>
-        <mass>0.525</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.337e-06</ixy>
-          <ixz>2.39e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.9381e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_forearm_collision'>
-        <pose>-0.000143922 -1.25974e-12 0.0204334 -2.66395 1.57079 -1.09315</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_forearm_visual'>
-        <pose>-0.000143922 -1.25974e-12 0.0204334 -2.66395 1.57079 -1.09315</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_wrist_prosup' type='revolute'>
-      <child>l_forearm</child>
-      <parent>l_elbow_1</parent>
-      <axis>
-        <xyz>-2.8477e-06 2.5974e-06 -1</xyz>
-        <limit>
-          <lower>-0.872665</lower>
-          <upper>0.872665</upper>
-          <effort>0.45</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_wrist_1'>
-      <pose>-0.0107501 -0.110259 -0.11428 -2.7153 1.57079 1.99709</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='l_wrist_pitch' type='revolute'>
-      <child>l_wrist_1</child>
-      <parent>l_forearm</parent>
-      <axis>
-        <xyz>1 1.01958e-06 -2.84771e-06</xyz>
-        <limit>
-          <lower>-1.13446</lower>
-          <upper>0.174533</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='l_hand'>
-      <pose>-0.0107501 -0.110259 -0.11428 -2.7153 1.57079 1.99709</pose>
-      <inertial>
-        <pose>0.07023 -0.00805 -0.007 0 -0 0</pose>
-        <mass>0.213</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.278e-05</ixy>
-          <ixz>4.823e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.8188e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_hand_collision'>
-        <pose>-1.25973e-12 0.0204334 -0.000143922 5.10271e-12 -2.64186e-12 1.49622e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_l_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='l_hand_visual'>
-        <pose>-1.25973e-12 0.0204334 -0.000143922 5.10271e-12 -2.64186e-12 1.49622e-11</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_l_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='l_wrist_yaw' type='revolute'>
-      <child>l_hand</child>
-      <parent>l_wrist_1</parent>
-      <axis>
-        <xyz>1.01956e-06 -1 -6.27061e-06</xyz>
-        <limit>
-          <lower>-0.436332</lower>
-          <upper>0.436332</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='neck_1'>
-      <pose>-0.00780846 1.22355e-08 0.2253 6.12199e-06 -1.57079 -1.42918e-06</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_1_collision'>
-        <pose>-5.55112e-17 9.62889e-13 -0.0055 3.14159 -1.57079 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_neck_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='neck_1_visual'>
-        <pose>-5.55112e-17 9.62889e-13 -0.0055 3.14159 -1.57079 1.5708</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_neck_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='neck_pitch' type='revolute'>
-      <child>neck_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>-4.69281e-06 1 6.74621e-11</xyz>
-        <limit>
-          <lower>-0.523599</lower>
-          <upper>0.383972</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-    <link name='neck_2'>
-      <pose>-0.00780836 1.22353e-08 0.2348 -1.46928e-05 3.67327e-06 -1.57079</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_2_collision'>
-        <pose>-9.62885e-13 0.0055 0.0235 -3.14157 -1.57079 -1.57082</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_neck_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='neck_2_visual'>
-        <pose>-9.62885e-13 0.0055 0.0235 -3.14157 -1.57079 -1.57082</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_neck_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='neck_roll' type='revolute'>
-      <child>neck_2</child>
-      <parent>neck_1</parent>
-      <axis>
-        <xyz>-1 -4.69282e-06 1.46928e-05</xyz>
-        <limit>
-          <lower>-0.349066</lower>
-          <upper>0.349066</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-    <link name='head'>
-      <pose>-0.00780836 1.22353e-08 0.2348 -1.5708 1.46928e-05 8.36603e-06</pose>
-      <inertial>
-        <pose>0.0185 -0.1108 6.78454e-18 0 -0 0</pose>
-        <mass>3</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='head_collision'>
-        <pose>0.0045 -0.0235 9.42472e-13 -1.32582 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_head.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='head_visual'>
-     <!--   <pose>0.0045 -0.0235 9.42472e-13 -1.32582 1.57079 -2.89662</pose>-->
-
-       <pose>0.0045 -0.0235 9.42472e-13 -2.89662 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head_without_eyes.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <sensor name='imu_sensor' type='imu'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <visualize>1</visualize>
-        <imu/>
-        <plugin name='iCub_yarp_gazebo_plugin_IMU' filename='libgazebo_yarp_imu.so'>
-          <yarpConfigurationFile>model://icub/conf/gazebo_icub_inertial.ini</yarpConfigurationFile>
-        </plugin>
-        <pose>0.0185 -0.1108 0.0066 1.5708 -0 0</pose>
-      </sensor>
-    </link>
-    <joint name='neck_yaw' type='revolute'>
-      <child>head</child>
-      <parent>neck_2</parent>
-      <axis>
-        <xyz>1.46928e-05 1.48834e-12 1</xyz>
-        <limit>
-          <lower>-0.767945</lower>
-          <upper>0.767945</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <link name='eyes_tilt_link'>
-      <pose>-62.81e-03 0e-03 340.8e-03 1.57079 0 0</pose>
-      <inertial>
-        <pose>0.0 0.0 0.0 0 0 0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-  <!--     <visual name='eyes_tilt_visual'>
-        <pose>0.0 0.0 0.0 0 0 0</pose>
-        <geometry>
-          <cylinder><radius>0.01</radius><length>0.05</length></cylinder> 
-        </geometry>
-      </visual> -->
-    </link>
-
-    <link name='left_eye_link'>
-      <pose>-62.81e-03 -34e-03 340.8e-03 -3.14159 0 0</pose>
-      <inertial>
-        <pose>0.0 0.0 0.0 0.0 0.0 0.0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-       <visual name='left_eye_visual'>
-         <pose>0.055 0.033 -0.14 0 0 0</pose> 
-        <geometry>
-             <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_eye.dae</uri>
-          </mesh> 
-   <!-- <cylinder><radius>0.01</radius><length>0.05</length></cylinder> -->
-        </geometry>
-      </visual>
-    </link>
-
-    <link name='right_eye_link'>
-      <pose> -62.81e-03 34e-03 340.8e-03 -3.14159 0 0</pose>
-      <inertial>
-        <pose>0 0 0 0 0 0</pose>
-        <mass>0.01</mass>
-        <inertia>
-          <ixx>0.001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.001</iyy>
-          <iyz>0</iyz>
-          <izz>0.001</izz>
-        </inertia>
-      </inertial>
-      <visual name='right_eye_visual'>
-        <pose>0.055 0.033 -0.14 0 0 0</pose> 
-        <geometry>
- <!-- <cylinder><radius>0.01</radius><length>0.05</length></cylinder>-->
-            <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_eye.dae</uri>
-          </mesh> 
-        </geometry>
-      </visual>
-    </link>
-
-    <joint name='eyes_tilt' type='revolute'>
-      <child>eyes_tilt_link</child>
-      <parent>head</parent>
-      <axis>
-        <xyz>0.0 1.0 0.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <joint name='left_eye' type='revolute'>
-      <child>left_eye_link</child>
-      <parent>eyes_tilt_link</parent>
-      <axis>
-        <xyz>0.0 0.0 1.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <joint name='right_eye' type='revolute'>
-      <child>right_eye_link</child>
-      <parent>eyes_tilt_link</parent>
-      <axis>
-        <xyz>0.0 0.0 1.0</xyz>
-        <limit>
-          <lower>-1.5708</lower>
-          <upper>1.5708</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <physics>
-        <ode>
-          <implicit_spring_damper>1</implicit_spring_damper>
-          <cfm_damping>1</cfm_damping>
-          <limit>
-            <cfm>0</cfm>
-            <erp>0.2</erp>
-          </limit>
-        </ode>
-      </physics>
-    </joint>
-
-    <link name='r_shoulder_1'>
-      <pose>-0.0107489 0.11026 0.1753 -2.87979 -1.57079 -1.42916e-06</pose>
-      <inertial>
-        <pose>5e-06 0.0187 0.00119 0 -0 0</pose>
-        <mass>0.189</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>2.1e-08</ixy>
-          <ixz>-1e-09</ixz>
-          <iyy>0.01</iyy>
-          <iyz>4.22e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_1_collision'>
-        <pose>-3.08642e-14 0.109285 0.00521101 3.06003 1.57079 -1.65235</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_shoulder_1_visual'>
-        <pose>-3.08642e-14 0.109285 0.00521101 3.06003 1.57079 -1.65235</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shoulder_1.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_shoulder_pitch' type='revolute'>
-      <child>r_shoulder_1</child>
-      <parent>chest</parent>
-      <axis>
-        <xyz>0.258817 -0.965926 -2.85208e-06</xyz>
-        <limit>
-          <lower>-1.65806</lower>
-          <upper>0.0872665</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_shoulder_2'>
-      <pose>-0.0107489 0.11026 0.1753 6.97094e-06 8.21122e-07 1.83259</pose>
-      <inertial>
-        <pose>-9.4e-05 -0.00627 -0.0166 0 -0 0</pose>
-        <mass>0.179</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-4.53e-07</ixy>
-          <ixz>2.03e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>2.07e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_2_collision'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -1.00125 -1.57079 2.57205</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_shoulder_2_visual'>
-        <pose>-0.00154529 -0.00521101 -1.11181e-12 -1.00125 -1.57079 2.57205</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_shoulder_2.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_shoulder_roll' type='revolute'>
-      <child>r_shoulder_2</child>
-      <parent>r_shoulder_1</parent>
-      <axis>
-        <xyz>0.965926 0.258817 -6.97094e-06</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>2.80649</upper>
-          <effort>84</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_shoulder_3'>
-      <pose>-0.0107489 0.11026 0.1753 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>0.00553932 -2.47652e-05 0.0493895 0 -0 0</pose>
-        <mass>0.155</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>3.56977e-07</ixy>
-          <ixz>-3.95372e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-6.5128e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_shoulder_yaw' type='revolute'>
-      <child>r_shoulder_3</child>
-      <parent>r_shoulder_2</parent>
-      <axis>
-        <xyz>6.5209e-06 -5.62481e-08 1</xyz>
-        <limit>
-          <lower>-0.645772</lower>
-          <upper>1.74533</upper>
-          <effort>34</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_upper_arm'>
-      <pose>-0.0257494 0.11026 0.1073 3.14159 -6.5209e-06 -3.14159</pose>
-      <inertial>
-        <pose>-0.00015906 8.28733e-05 0.0298828 0 -0 0</pose>
-        <mass>0.729</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.08e-06</ixy>
-          <ixz>-2.29e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.57e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_upper_arm_collision'>
-        <pose>-0.0204334 -0.000143922 -0.068 -3.14159 -1.51363e-11 1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_upper_arm_visual'>
-        <pose>-0.0204334 -0.000143922 -0.068 -3.14159 -1.51363e-11 1.309</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_arm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_arm_ft_sensor' type='revolute'>
-      <child>r_upper_arm</child>
-      <parent>r_shoulder_3</parent>
-      <axis>
-        <xyz>-0 0 0</xyz>
-        <limit>
-          <lower>0</lower>
-          <upper>0</upper>
-          <effort>0</effort>
-          <velocity>0</velocity>
-        </limit>
-        <dynamics>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-      <sensor name='right_arm_ft' type='force_torque'>
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin name='right_arm_ft_plugin' filename='libgazebo_yarp_forcetorque.so'>
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_right_arm_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-    </joint>
-    <link name='r_elbow_1'>
-      <pose>-0.0257499 0.11026 0.0230202 -2.59732e-06 6.5209e-06 3.6732e-06</pose>
-      <inertial>
-        <pose>0.0013 -0.00371 0.00105 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.02e-07</ixy>
-          <ixz>-3.99e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.71e-07</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_elbow' type='revolute'>
-      <child>r_elbow_1</child>
-      <parent>r_upper_arm</parent>
-      <axis>
-        <xyz>-3.67322e-06 1 -2.59732e-06</xyz>
-        <limit>
-          <lower>0.0959931</lower>
-          <upper>1.85005</upper>
-          <effort>20</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_forearm'>
-      <pose>-0.0107499 0.11026 0.0230201 1.57079 -2.59734e-06 -1.5708</pose>
-      <inertial>
-        <pose>-0.000347 -0.066 -0.00476 0 -0 0</pose>
-        <mass>0.525</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>5.66e-06</ixy>
-          <ixz>1.4e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.82e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_forearm_collision'>
-        <pose>0.000143922 -1.07214e-12 -0.0204334 -1.74165 1.57079 -0.170863</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_forearm_visual'>
-        <pose>0.000143922 -1.07214e-12 -0.0204334 -1.74165 1.57079 -0.170863</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_forearm.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_wrist_prosup' type='revolute'>
-      <child>r_forearm</child>
-      <parent>r_elbow_1</parent>
-      <axis>
-        <xyz>2.84768e-06 2.59734e-06 1</xyz>
-        <limit>
-          <lower>-0.872665</lower>
-          <upper>0.872665</upper>
-          <effort>0.45</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_wrist_1'>
-      <pose>-0.0107503 0.11026 -0.11428 0.426292 -1.57079 1.14451</pose>
-      <inertial>
-        <pose>0 0 0 0 -0 0</pose>
-        <mass>0.1</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-    </link>
-    <joint name='r_wrist_pitch' type='revolute'>
-      <child>r_wrist_1</child>
-      <parent>r_forearm</parent>
-      <axis>
-        <xyz>-1 -3.67321e-06 2.84771e-06</xyz>
-        <limit>
-          <lower>-1.13446</lower>
-          <upper>0.174533</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <link name='r_hand'>
-      <pose>-0.0107503 0.11026 -0.11428 -0.720145 1.57079 -2.29094</pose>
-      <inertial>
-        <pose>0.07023 -0.00805 0.007 0 -0 0</pose>
-        <mass>0.213</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.26e-05</ixy>
-          <ixz>-6.08e-06</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.76e-05</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_hand_collision'>
-        <pose>1.0721e-12 0.0204334 0.000143922 1.53098e-11 2.00656e-11 -3.14159</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/collision/icub_simple_collision_r_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-      <visual name='r_hand_visual'>
-        <pose>1.0721e-12 0.0204334 0.000143922 1.53098e-11 2.00656e-11 -3.14159</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_r_hand.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-    </link>
-    <joint name='r_wrist_yaw' type='revolute'>
-      <child>r_hand</child>
-      <parent>r_wrist_1</parent>
-      <axis>
-        <xyz>3.67323e-06 -1 6.27054e-06</xyz>
-        <limit>
-          <lower>-0.436332</lower>
-          <upper>0.436332</upper>
-          <effort>0.65</effort>
-          <velocity>100</velocity>
-        </limit>
-        <dynamics>
-          <damping>1</damping>
-          <friction>0</friction>
-          <spring_reference>0</spring_reference>
-          <spring_stiffness>0</spring_stiffness>
-        </dynamics>
-        <use_parent_model_frame>1</use_parent_model_frame>
-      </axis>
-    </joint>
-    <plugin name='controlboard_torso' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_torso.ini</yarpConfigurationFile>
-      <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_head' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_head.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_left_arm_no_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm_no_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-      <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-      <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
-    </plugin>
-    <plugin name="apply_external_wrench" filename="libgazebo_yarp_externalwrench.so" >
-      <robotNamefromConfigFile>model://icub/conf/gazebo_icub_robotname.ini</robotNamefromConfigFile>>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/models/icub_standup/model.config b/models/icub_standup/model.config
deleted file mode 100644
index 9f7791b..0000000
--- a/models/icub_standup/model.config
+++ /dev/null
@@ -1,16 +0,0 @@
-<?xml version="1.0"?>
-<model>
-    <name>iCub on a chair</name>
-    
-    <version>1.0</version>
-    <sdf version='1.5'>icub_standup.sdf</sdf>
-    
-    <author>
-        <name>Gabriele Nava</name>
-        <email>gabriele.nava@iit.it</email>
-    </author>
-    
-    <description>
-        iCub is sitting on a chair
-    </description>
-</model>
diff --git a/worlds/icub_seesaw_world/icub_seesaw.world b/worlds/icub_seesaw_world/icub_seesaw.world
index 1895332..ce441cc 100644
--- a/worlds/icub_seesaw_world/icub_seesaw.world
+++ b/worlds/icub_seesaw_world/icub_seesaw.world
@@ -18,9 +18,32 @@
     </include>
 
     <!-- iCub above the seesaw -->
-    <include>
-    <uri>model://icub_on_seesaw</uri>
-    </include>
+    <model name="iCub">
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration> 0 0 0.0824</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.2094 0.0873 0 -0.1745 0.0479 -0.0898</initialConfiguration>
+      </plugin>
+      <include>
+        <uri>model://icub</uri>
+        <pose>-0.05 0 0.6926 0 0 3.14</pose>
+      </include>
+    </model>
 
   </world>
 </sdf>
diff --git a/worlds/icub_standup_world/icub_standup.world b/worlds/icub_standup_world/icub_standup.world
index 8b06d5e..23153de 100644
--- a/worlds/icub_standup_world/icub_standup.world
+++ b/worlds/icub_standup_world/icub_standup.world
@@ -18,8 +18,31 @@
     </include>
 
     <!-- iCub in the sitting position -->
-    <include>
-    <uri>model://icub_standup</uri>
-    </include>
+    <model name="iCub">
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <include>
+        <uri>model://icub</uri>
+        <pose>-0.077649 0.000965 0.389694 -8e-06 0.036487 3.14094</pose>
+      </include>
+    </model>
   </world>
 </sdf>

From 92f074f7e587c369415d47b2488d64e59ef89336 Mon Sep 17 00:00:00 2001
From: Lorenzo <lor.rapetti@gmail.com>
Date: Wed, 21 Nov 2018 18:15:27 +0100
Subject: [PATCH 14/20] Add two icubs world

---
 .gitignore                                    |  2 +
 worlds/two_icubs_standup_world/README.md      | 25 ++++++++
 .../two_icub_standup_world                    | 59 +++++++++++++++++++
 3 files changed, 86 insertions(+)
 create mode 100644 worlds/two_icubs_standup_world/README.md
 create mode 100644 worlds/two_icubs_standup_world/two_icub_standup_world

diff --git a/.gitignore b/.gitignore
index 0cc7e4b..54f6c99 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,3 +5,5 @@ Makefile
 cmake_install.cmake
 install_manifest.txt
 CTestTestfile.cmake
+
+.DS_Store
\ No newline at end of file
diff --git a/worlds/two_icubs_standup_world/README.md b/worlds/two_icubs_standup_world/README.md
new file mode 100644
index 0000000..b98e218
--- /dev/null
+++ b/worlds/two_icubs_standup_world/README.md
@@ -0,0 +1,25 @@
+## To load two icubs standup world in Gazebo
+
+Normally, for loading a `world`, Gazebo needs to be launched in the folder where the `world` is. You can avoid this by appending these lines to your `.bashrc`:
+
+```
+source /usr/share/gazebo/setup.sh
+export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/PATH/WHERE/YOUR/WORLD/IS
+```
+
+Furthermore, if you are working with Simulink controllers, it is necessary to sincronize Gazebo with Simulink by running Gazebo with the option `-slibgazebo_yarp_clock.so`. 
+
+Final command to run in the terminal is something like this: `gazebo -slibgazebo_yarp_clock.so nameOfYourWorld`.
+
+For **two_icubs_standup** it is also necessary to avoid using the standard `gazeboYarpPluginsRobotName` for the two robots, otherwise the two models would share the name `icubSim` and the control boards of one of the two model fail.
+This can be done by editing [gazebo_icub_robotname.ini](https://github.com/robotology/icub-gazebo/blob/master/icub/conf/gazebo_icub_robotname.ini) file, and commenting out the following line:
+ ```
+ gazeboYarpPluginsRobotName icubSim
+ ```
+
+ When the world is created the two robots are not linked, but the model `iCub` is created with the [`linkattacher`](https://github.com/robotology/gazebo-yarp-plugins/tree/master/plugins/linkattacher) plugin that can be used to attach the robots hands:
+ ```
+ $ yarp rpc /iCub/linkattacher/rpc:i
+> attachUnscoped iCub l_hand iCub_0 r_hand
+> attachUnscoped iCub r_hand iCub_0 l_hand
+ ```
\ No newline at end of file
diff --git a/worlds/two_icubs_standup_world/two_icub_standup_world b/worlds/two_icubs_standup_world/two_icub_standup_world
new file mode 100644
index 0000000..89ce965
--- /dev/null
+++ b/worlds/two_icubs_standup_world/two_icub_standup_world
@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+
+    <!-- Light -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <!-- Ground Plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <!-- Chair for sittin' on -->
+    <include>
+        <uri>model://chair</uri>
+    </include>
+    
+    <!-- iCub in the sitting position -->
+    <model name="iCub">
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <include>
+          <uri>model://icub</uri>
+      </include>
+      <plugin name='link_attacher' filename='libgazebo_yarp_linkattacher.so'>
+        <yarpConfigurationString>(name /iCub/linkattacher/rpc:i)</yarpConfigurationString>
+      </plugin>
+    </model>
+
+    <!-- iCub standing-->
+    <model name="iCub_0">
+      <include>
+          <uri>model://icub</uri>
+          <pose>0.6 0.0 0.6 0 0 0</pose>
+      </include>
+    </model>
+
+  </world>
+</sdf>
\ No newline at end of file

From 820f38261099107d495aecbd6b324b3aeb1c8bb4 Mon Sep 17 00:00:00 2001
From: Lorenzo Rapetti <lor.rapetti@gmail.com>
Date: Thu, 22 Nov 2018 13:08:26 +0100
Subject: [PATCH 15/20] Update README.md

---
 README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 81957af..d7b699a 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,7 @@
 # icub-gazebo-wholebody
 Gazebo models and worlds for simulating scenarios related to iCub Whole Body Control. 
 
-The world and models in this repository are mantained by the [Dynamic Interaction Control Group](https://www.iit.it/research/lines/dynamic-interaction-controlicub-gazebo-wholebody) and are mostly related to whole body balancing and walking, related to the FP7 European Projects CoDyCo and Koroibot and the H2020 Project AnDy.
+The world and models in this repository are mantained by the [Dynamic Interaction Control Group](https://www.iit.it/research/lines/dynamic-interaction-control) and are mostly related to whole body balancing and walking, related to the FP7 European Projects CoDyCo and Koroibot and the H2020 Project AnDy.
 
 # Usage 
 - `git clone` the repository on your computer.

From 65be68a751f93188252db303e9ff95d4c28946db Mon Sep 17 00:00:00 2001
From: Lorenzo <lor.rapetti@gmail.com>
Date: Fri, 23 Nov 2018 09:23:41 +0100
Subject: [PATCH 16/20] Fix controlboard name

---
 worlds/icub_seesaw_world/icub_seesaw.world            | 4 ++--
 worlds/icub_standup_world/icub_standup.world          | 4 ++--
 worlds/two_icubs_standup_world/two_icub_standup_world | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/worlds/icub_seesaw_world/icub_seesaw.world b/worlds/icub_seesaw_world/icub_seesaw.world
index ce441cc..d4afff8 100644
--- a/worlds/icub_seesaw_world/icub_seesaw.world
+++ b/worlds/icub_seesaw_world/icub_seesaw.world
@@ -24,11 +24,11 @@
         <initialConfiguration> 0 0 0.0824</initialConfiguration>
       </plugin>
       <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6278 0.5231 0.0010 0.8727 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
diff --git a/worlds/icub_standup_world/icub_standup.world b/worlds/icub_standup_world/icub_standup.world
index 23153de..9f88653 100644
--- a/worlds/icub_standup_world/icub_standup.world
+++ b/worlds/icub_standup_world/icub_standup.world
@@ -24,11 +24,11 @@
         <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
       </plugin>
       <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
diff --git a/worlds/two_icubs_standup_world/two_icub_standup_world b/worlds/two_icubs_standup_world/two_icub_standup_world
index 89ce965..95f2346 100644
--- a/worlds/two_icubs_standup_world/two_icub_standup_world
+++ b/worlds/two_icubs_standup_world/two_icub_standup_world
@@ -24,11 +24,11 @@
         <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
       </plugin>
       <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
-        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_arm'> </yarpPluginConfigurationOverride>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
         <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
       </plugin>
       <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>

From 1cbc0e3929cb411b5e7b3fb54f6d6bb29cf0c94c Mon Sep 17 00:00:00 2001
From: Gabriele <gabriele.nava@iit.it>
Date: Wed, 6 Mar 2019 14:14:39 +0100
Subject: [PATCH 17/20] worlds seesaw and standup now point iCubGazeboV2_5

---
 worlds/icub_seesaw_world/icub_seesaw.world   | 31 ++++++++++++----
 worlds/icub_standup_world/icub_standup.world | 37 +++++++++++++++++---
 2 files changed, 57 insertions(+), 11 deletions(-)

diff --git a/worlds/icub_seesaw_world/icub_seesaw.world b/worlds/icub_seesaw_world/icub_seesaw.world
index 1895332..ee073ae 100644
--- a/worlds/icub_seesaw_world/icub_seesaw.world
+++ b/worlds/icub_seesaw_world/icub_seesaw.world
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<sdf version="1.4">
+<sdf version="1.6">
   <world name="default">
 
     <!-- Light -->
@@ -12,15 +12,34 @@
       <uri>model://ground_plane</uri>
     </include>
 
-    <!-- Seesaw -->
+     <!-- Seesaw -->
     <include>
         <uri>model://seesaw</uri>
     </include>
+    
+    <!-- iCub in the sitting position -->
+    <model name="iCub">
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.0 0.0 0.0 0.0 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.0 0.0 0.0 0.0 0.0 0.0</initialConfiguration>
+      </plugin>
+      <include>
+          <uri>model://iCubGazeboV2_5</uri>
+          <pose>0.0 0.0 0.75 0.0 0.0 3.14</pose>
+      </include>
+      <plugin name='link_attacher' filename='libgazebo_yarp_linkattacher.so'>
+        <yarpConfigurationString>(name /iCub/linkattacher/rpc:i)</yarpConfigurationString>
+      </plugin>
 
-    <!-- iCub above the seesaw -->
-    <include>
-    <uri>model://icub_on_seesaw</uri>
-    </include>
+    </model>
 
   </world>
 </sdf>
diff --git a/worlds/icub_standup_world/icub_standup.world b/worlds/icub_standup_world/icub_standup.world
index 8b06d5e..a478f7a 100644
--- a/worlds/icub_standup_world/icub_standup.world
+++ b/worlds/icub_standup_world/icub_standup.world
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<sdf version="1.4">
+<sdf version="1.6">
   <world name="default">
 
     <!-- Light -->
@@ -16,10 +16,37 @@
     <include>
         <uri>model://chair</uri>
     </include>
-
+    
     <!-- iCub in the sitting position -->
-    <include>
-    <uri>model://icub_standup</uri>
-    </include>
+    <model name="iCub">
+      <plugin name='torso_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_torso'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>0.0 0.0 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='larm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_arm_no_hand'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='rarm_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_arm_no_hand'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>-0.6981 0.2094 1.3963 0.5585 0 0 0</initialConfiguration>
+      </plugin>
+      <plugin name='lleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_left_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <plugin name='rleg_configuration_override' filename='libgazebo_yarp_configurationoverride.so'>
+        <yarpPluginConfigurationOverride plugin_name='controlboard_right_leg'> </yarpPluginConfigurationOverride>
+        <initialConfiguration>1.5908 0.2141 0 -1.8176  -0.2142 0.0</initialConfiguration>
+      </plugin>
+      <include>
+          <uri>model://iCubGazeboV2_5</uri>
+          <pose>0.0 0.0 0.39 0.0 0.0 3.14</pose>
+      </include>
+      <plugin name='link_attacher' filename='libgazebo_yarp_linkattacher.so'>
+        <yarpConfigurationString>(name /iCub/linkattacher/rpc:i)</yarpConfigurationString>
+      </plugin>
+    </model>
+
   </world>
 </sdf>

From 2b4d6c004368c0ac0b26f590b9ddec0a1ffaf197 Mon Sep 17 00:00:00 2001
From: Gabriele Nava <gabriele.nava@iit.it>
Date: Wed, 6 Mar 2019 14:18:15 +0100
Subject: [PATCH 18/20] Update icub_seesaw.world

---
 worlds/icub_seesaw_world/icub_seesaw.world | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/worlds/icub_seesaw_world/icub_seesaw.world b/worlds/icub_seesaw_world/icub_seesaw.world
index ee073ae..3c88e71 100644
--- a/worlds/icub_seesaw_world/icub_seesaw.world
+++ b/worlds/icub_seesaw_world/icub_seesaw.world
@@ -35,10 +35,6 @@
           <uri>model://iCubGazeboV2_5</uri>
           <pose>0.0 0.0 0.75 0.0 0.0 3.14</pose>
       </include>
-      <plugin name='link_attacher' filename='libgazebo_yarp_linkattacher.so'>
-        <yarpConfigurationString>(name /iCub/linkattacher/rpc:i)</yarpConfigurationString>
-      </plugin>
-
     </model>
 
   </world>

From b599de284d5e38c0ae577668b984e745107c06ac Mon Sep 17 00:00:00 2001
From: Gabriele Nava <gabriele.nava@iit.it>
Date: Wed, 6 Mar 2019 14:18:47 +0100
Subject: [PATCH 19/20] Update icub_standup.world

---
 worlds/icub_standup_world/icub_standup.world | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/worlds/icub_standup_world/icub_standup.world b/worlds/icub_standup_world/icub_standup.world
index a478f7a..cc2a6af 100644
--- a/worlds/icub_standup_world/icub_standup.world
+++ b/worlds/icub_standup_world/icub_standup.world
@@ -43,9 +43,6 @@
           <uri>model://iCubGazeboV2_5</uri>
           <pose>0.0 0.0 0.39 0.0 0.0 3.14</pose>
       </include>
-      <plugin name='link_attacher' filename='libgazebo_yarp_linkattacher.so'>
-        <yarpConfigurationString>(name /iCub/linkattacher/rpc:i)</yarpConfigurationString>
-      </plugin>
     </model>
 
   </world>

From fb410927ccb77f295698e63bd80b837705c5c86d Mon Sep 17 00:00:00 2001
From: Yeshasvitvs <yeshasvitvs@gmail.com>
Date: Thu, 14 Mar 2019 19:41:19 +0100
Subject: [PATCH 20/20] Update readme with linkattacher bash script details

---
 worlds/two_icubs_comanipulation/README.md | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/worlds/two_icubs_comanipulation/README.md b/worlds/two_icubs_comanipulation/README.md
index 330d78a..90224b4 100644
--- a/worlds/two_icubs_comanipulation/README.md
+++ b/worlds/two_icubs_comanipulation/README.md
@@ -19,5 +19,18 @@ The option `--verbose` enables verbose output on the terminal running gazebo
 
 For **two_icubs_comanipulation** it is also necessary to avoid using the standard `gazeboYarpPluginsRobotName` for the two robots, otherwise the two models would share the name `icubSim` and the control boards of one of the two model fail.
 This can be done by editing [gazebo_icub_robotname.ini](https://github.com/robotology/icub-gazebo/blob/master/icub/conf/gazebo_icub_robotname.ini) file, and commenting out the following line:
+
+ ```
+gazeboYarpPluginsRobotName icubSim
+
  ```
- gazeboYarpPluginsRobotName icubSim
+ When the world is created the two robots are spawned along with the board on top of their hands. The board and the hands of the robots are not linked at this state, but the `board` model is created with the [linkattacher](https://github.com/robotology/gazebo-yarp-plugins/tree/master/plugins/linkattacher) gazebo-yarp-plugin which is be used to attach the robots hands and the board. The bash script `handlesHandsLinkAttacher.sh` inside the `board` model directory will achieve this. The usage is as following:
+
+```
+./handlesHandsLinkAttacher.sh attach -----> Attach the handle links of the board to the hand links of the two robot
+./handlesHandsLinkAttacher.sh detach -----> Detach the handle links of the board from the hand links of the two robot
+ ```
+
+ **NOTE:** If the bash script is throwing error there may be a possible naming issue of the icub models spawned in gazebo. The default names of the two iCub robots assumed for this bash script are `iCub1` and `iCub2`.
+
+