From db7c03bb082faf73916ecc93ea8b71f51607d043 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 27 May 2021 16:15:55 +0200
Subject: [PATCH 1/5] Update README.md

---
 README.md | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/README.md b/README.md
index 6962f22..27296f6 100644
--- a/README.md
+++ b/README.md
@@ -4,12 +4,15 @@ Resources and programs to generate models (URDF, SDF) of the iCub robot.
 The models automatically generated by the software and the resources in this repository
 are available at https://github.com/robotology/icub-models .
 
-**Note: this repository is meant to streamline the process of producing iCub URDF/SDF models by iCub mantainers. It is not meant to be used directly by users of iCub. For offical info on the kinematic parameters of the iCub, please see [the documentation in iCub's wiki](http://wiki.icub.org/wiki/ICubForwardKinematics).**
+**Note: this repository is meant to streamline the process of producing iCub URDF/SDF models by iCub mantainers. It is not meant to be used directly by users of iCub. For offical info on the kinematic parameters of the iCub, please see [the documentation in iCub's documentation](https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-forward-kinematics/icub-forward-kinematics/).**
 
 ## Dependencies
-- [orocos_kdl](http://www.orocos.org/kdl)
+- [iDynTree](https://github.com/robotology/idyntree)
+- [YARP](https://github.com/robotology/yarp)
+- [simmechanics-to-urdf](https://github.com/robotology/simmechanics-to-urdf)
+
+Detailed documentation on how to install the softare is current work in progress. In the meanwhile, you can check the exact way in which the software is installing by inspecting the [`CI` GitHub Actions workflow](.github/workflows/ci.yml) that is used to automatically generate the models of iCub using this repo and commit it to the [icub-models](https://github.com/robotology/icub-models) repo.
 
-Required only if you wish to generate DH models (`ICUB_MODEL_GENERATE_DH = ON`). Make sure you have compiled iDynTree with the option `IDYNTREE_USES_KDL = ON`.
 
 ## Usage
 To generate the models, please ensure that you have all the necessary dependencies.
@@ -21,10 +24,10 @@ Currently, it is possible to generate iCub models by means of two different pipe
 
 1) The generation is achieved by using Denavit Hartenberg parametsrs based on robot-model hardcoded data in the iKin/iDyn source code, and meshes postprocessed by ISIR at UPMC, Paris, available in `dh` directory.
 
-2) The generation is achieved from CAD models converted to simmechanics file, as described in http://wiki.icub.org/wiki/Creo_Mechanism_to_URDF , available in the `simmechanics` directory.
+2) The generation is achieved from CAD models converted to simmechanics file, as described in https://github.com/icub-tech-iit/cad-mechanics-public/wiki/Creo-mechanism-to-URDF, available in the `simmechanics` directory.
 
 As the two pipeline process data from different sources, **the link frames are not guaranteed to have the same orientation
-or location across different models**. The only frames (that are represented in URDF as massless links) whose position is guaranteed to be consistent across different models are the one documented in http://wiki.icub.org/wiki/ICub_Model_naming_conventions#Frames .
+or location across different models**. The only frames (that are represented in URDF as massless links) whose position is guaranteed to be consistent across different models are the one documented in https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-model-naming-conventions/icub-model-naming-conventions/ .
 
 Both generation pipelines are still in `a work in progress` state, and several issues need to be properly solved, e.g.:
 * [Meshes are not properly handled #28](https://github.com/robotology/icub-model-generator/issues/28)

From 8450982e4493b527060651d0db19c126a69c8068 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 27 May 2021 16:17:15 +0200
Subject: [PATCH 2/5] Delete generate.sh

---
 dh/generate.sh | 1 -
 1 file changed, 1 deletion(-)
 delete mode 100755 dh/generate.sh

diff --git a/dh/generate.sh b/dh/generate.sh
deleted file mode 100755
index 6aa2a41..0000000
--- a/dh/generate.sh
+++ /dev/null
@@ -1 +0,0 @@
-icub_urdf_sdf_generator --data_directory ./data/ --output_directory ./generated/

From a27646a37d7080d516d12f2d3939e086fa43fa91 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 27 May 2021 16:17:23 +0200
Subject: [PATCH 3/5] Delete generateCoDyCoSuperbuild.sh

---
 dh/generateCoDyCoSuperbuild.sh | 8 --------
 1 file changed, 8 deletions(-)
 delete mode 100755 dh/generateCoDyCoSuperbuild.sh

diff --git a/dh/generateCoDyCoSuperbuild.sh b/dh/generateCoDyCoSuperbuild.sh
deleted file mode 100755
index f338e5f..0000000
--- a/dh/generateCoDyCoSuperbuild.sh
+++ /dev/null
@@ -1,8 +0,0 @@
-../../build/misc/icub-model-generator/src/icub_urdf_sdf_generator --data_directory ./data/ --output_directory ./generated
-cp ./generated/urdf/iCubDarmstad01/icub.urdf ../../libraries/yarpWholeBodyInterface/app/robots/iCubDarmstadt01/model.urdf
-cp ./generated/gazebo_models/icubGazeboSim/icub_simulation.urdf ../../libraries/yarpWholeBodyInterface/app/robots/icubGazeboSim/model.urdf
-cp ./generated/urdf/iCubGenova01/icub.urdf ../../libraries/yarpWholeBodyInterface/app/robots/iCubGenova01/model.urdf
-cp ./generated/urdf/iCubGenova03/icub.urdf ../../libraries/yarpWholeBodyInterface/app/robots/iCubGenova03/model.urdf
-cp ./generated/urdf/iCubParis01/icub.urdf ../../libraries/yarpWholeBodyInterface/app/robots/iCubParis01/model.urdf
-cp ./generated/urdf/iCubParis02/icub.urdf ../../libraries/yarpWholeBodyInterface/app/robots/iCubParis02/model.urdf
-cp ./generated/urdf/iCubDarmstadt01/icub.urdf ../../libraries/yarpWholeBodyInterface/app/robots/iCubDarmstadt01/model.urdf

From 56e644009bae12e183189b315778064bf65d0749 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 27 May 2021 16:18:34 +0200
Subject: [PATCH 4/5] Delete README.md

---
 dh/README.md | 38 --------------------------------------
 1 file changed, 38 deletions(-)
 delete mode 100644 dh/README.md

diff --git a/dh/README.md b/dh/README.md
deleted file mode 100644
index f281808..0000000
--- a/dh/README.md
+++ /dev/null
@@ -1,38 +0,0 @@
-DH iCub Model Generator
-==================
-
-This software repository is still in alpha stage, not ready for normal use.
-
-Resources (./data/ folder) and programs (./src/) folder to generated models (URDF,SDF) of the iCub robot from DH parameters.
-
-Installation
-------------
-
-##### Dependencies
-- [YARP](https://github.com/robotology/yarp)
-- [ICUB](https://github.com/robotology/icub-main)
-- [iDynTree](https://github.com/robotology/idyntree)
-- [SDFormat](https://bitbucket.org/osrf/sdformat)
-
-The easiest way to install this repository with almost all its dependencies is to use the [codyco-superbuild](https://github.com/robotology/codyco-superbuild). The SDFormat dependency is not
-installed by the codyco-superbuild, but it should be already if you have installed the development files Gazebo (the
-one that you need to compile [gazebo-yarp-plugins](https://github.com/robotology/gazebo-yarp-plugins).
-
-
-Example
--------
-
-### Model generation
-After compiling and installing this repository, you can run:
-```
-icub_urdf_sdf_generator --data_directory /path/to/codyco-superbuild/data/ --output_directory /path/to/icub-models/
-```
-Where the `/path/to/codyco-superbuild/data/ ` directory is the path to the data subdirectory of this repository.
-`/path/to/icub-models/` is an arbitrary directory where you want to store the generated URDF and SDF models for the various iCubs (iCubGenova01, iCubGenova03, etc, etc).
-The models available in `/path/to/icub-models/icub-models/urdf` are the one extracted directly from the iDyn models.
-
-### Use of generated models in Gazebo
-In the directory `/path/to/icub-models/icub-models/gazebo-models` you will find the SDF models generated for use with the Gazebo simulator, and the URDF models that match the kinematic and dynamic parameters of the SDF models. As the `icub_urdf_sdf_generator` generates also the proper configuration files to create a Gazebo model database, you can simply add `/path/to/icub-models/icub-models/gazebo-models` to the enviroment variable `GAZEBO_MODEL_PATH`.
-
-**Warning: currently to use the database of generated models of the iCub you should also install the [icub-gazebo](https://github.com/robotology-playground/icub-gazebo) repository, as the generated models take configuration files and meshes from the `iCub` model present in the `icub-gazebo` repository.**
-

From a07c1b870ed8cd4546bb4fe6a0b6cf5b0e0bc696 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 27 May 2021 16:20:57 +0200
Subject: [PATCH 5/5] Removed unused files

---
 dh/generated/gazebo_models/database.config    |   13 -
 .../gazebo_models/iCubDarmstadt01/icub.sdf    | 1970 -----------------
 .../iCubDarmstadt01/icub_simulation.urdf      | 1230 ----------
 .../iCubDarmstadt01/model.config              |    4 -
 .../gazebo_models/iCubGenova01/icub.sdf       | 1970 -----------------
 .../iCubGenova01/icub_simulation.urdf         | 1230 ----------
 .../gazebo_models/iCubGenova01/model.config   |    4 -
 .../gazebo_models/iCubGenova03/icub.sdf       | 1970 -----------------
 .../iCubGenova03/icub_simulation.urdf         | 1230 ----------
 .../gazebo_models/iCubGenova03/model.config   |    4 -
 .../gazebo_models/iCubParis01/icub.sdf        | 1970 -----------------
 .../iCubParis01/icub_simulation.urdf          | 1230 ----------
 .../gazebo_models/iCubParis01/model.config    |    4 -
 .../gazebo_models/iCubParis02/icub.sdf        | 1970 -----------------
 .../iCubParis02/icub_simulation.urdf          | 1230 ----------
 .../gazebo_models/iCubParis02/model.config    |    4 -
 .../gazebo_models/icubGazeboSim/icub.sdf      | 1970 -----------------
 .../icubGazeboSim/icub_simulation.urdf        | 1230 ----------
 .../gazebo_models/icubGazeboSim/model.config  |    4 -
 .../gazebo_models/icubGazeboSimNoFT/icub.sdf  | 1672 --------------
 .../icubGazeboSimNoFT/icub_simulation.urdf    | 1230 ----------
 .../icubGazeboSimNoFT/model.config            |    4 -
 dh/generated/urdf/iCubDarmstadt01/icub.urdf   | 1290 -----------
 dh/generated/urdf/iCubGenova01/icub.urdf      | 1290 -----------
 dh/generated/urdf/iCubGenova03/icub.urdf      | 1290 -----------
 dh/generated/urdf/iCubParis01/icub.urdf       | 1290 -----------
 dh/generated/urdf/iCubParis02/icub.urdf       | 1290 -----------
 dh/generated/urdf/icubGazeboSim/icub.urdf     | 1290 -----------
 dh/generated/urdf/icubGazeboSimNoFT/icub.urdf | 1290 -----------
 dh/generator/CMakeLists.txt                   |   44 -
 dh/generator/CMakeLists2.txt                  |   48 -
 dh/generator/README                           |   14 -
 dh/generator/cmake/FindEigen3.cmake           |   81 -
 dh/generator/cmake/FindTinyXML.cmake          |   40 -
 .../deprecated_icub_urdf_sdf_generator.cpp    |  112 -
 dh/generator/icub_dh_to_urdf_sdf_utils.cpp    |  115 -
 dh/generator/icub_urdf_from_iDyn.cpp          |  114 -
 .../icub_urdf_sdf_from_dh_generator.cpp       |  135 --
 dh/generator/urdf_gazebo_cleanup.cpp          |  152 --
 dh/generator/urdf_get_limits.cpp              |   80 -
 dh/generator/urdf_get_meshes.cpp              |   81 -
 dh/generator/urdf_get_total_mass.cpp          |   63 -
 ...urdf_print_homogeneous_transformations.cpp |   66 -
 dh/generator/urdf_sdf_from_dh_utils.cpp       |  899 --------
 dh/generator/urdf_sdf_from_dh_utils.h         |  163 --
 dh/generator/urdf_utils.cpp                   |  492 ----
 dh/generator/urdf_utils.h                     |   87 -
 dh/toBeIntegrated/README.md                   |    1 -
 .../icub2gazeboPIDconverters/CMakeLists.txt   |   48 -
 .../config/icub_head.xml                      |   58 -
 .../config/icub_left_arm.xml                  |  171 --
 .../config/icub_left_hand.xml                 |   67 -
 .../config/icub_left_leg.xml                  |  119 -
 .../config/icub_right_arm.xml                 |  136 --
 .../config/icub_right_hand.xml                |   12 -
 .../config/icub_right_leg.xml                 |  123 -
 .../config/icub_torso.xml                     |  116 -
 .../include/Constants.h                       |   61 -
 .../include/ICUBConfigParser.h                |  112 -
 .../src/ICUBConfigParser.cpp                  |  401 ----
 .../icub2gazeboPIDconverters/src/main.cpp     |   81 -
 61 files changed, 35465 deletions(-)
 delete mode 100644 dh/generated/gazebo_models/database.config
 delete mode 100644 dh/generated/gazebo_models/iCubDarmstadt01/icub.sdf
 delete mode 100644 dh/generated/gazebo_models/iCubDarmstadt01/icub_simulation.urdf
 delete mode 100644 dh/generated/gazebo_models/iCubDarmstadt01/model.config
 delete mode 100644 dh/generated/gazebo_models/iCubGenova01/icub.sdf
 delete mode 100644 dh/generated/gazebo_models/iCubGenova01/icub_simulation.urdf
 delete mode 100644 dh/generated/gazebo_models/iCubGenova01/model.config
 delete mode 100644 dh/generated/gazebo_models/iCubGenova03/icub.sdf
 delete mode 100644 dh/generated/gazebo_models/iCubGenova03/icub_simulation.urdf
 delete mode 100644 dh/generated/gazebo_models/iCubGenova03/model.config
 delete mode 100644 dh/generated/gazebo_models/iCubParis01/icub.sdf
 delete mode 100644 dh/generated/gazebo_models/iCubParis01/icub_simulation.urdf
 delete mode 100644 dh/generated/gazebo_models/iCubParis01/model.config
 delete mode 100644 dh/generated/gazebo_models/iCubParis02/icub.sdf
 delete mode 100644 dh/generated/gazebo_models/iCubParis02/icub_simulation.urdf
 delete mode 100644 dh/generated/gazebo_models/iCubParis02/model.config
 delete mode 100644 dh/generated/gazebo_models/icubGazeboSim/icub.sdf
 delete mode 100644 dh/generated/gazebo_models/icubGazeboSim/icub_simulation.urdf
 delete mode 100644 dh/generated/gazebo_models/icubGazeboSim/model.config
 delete mode 100644 dh/generated/gazebo_models/icubGazeboSimNoFT/icub.sdf
 delete mode 100644 dh/generated/gazebo_models/icubGazeboSimNoFT/icub_simulation.urdf
 delete mode 100644 dh/generated/gazebo_models/icubGazeboSimNoFT/model.config
 delete mode 100644 dh/generated/urdf/iCubDarmstadt01/icub.urdf
 delete mode 100644 dh/generated/urdf/iCubGenova01/icub.urdf
 delete mode 100644 dh/generated/urdf/iCubGenova03/icub.urdf
 delete mode 100644 dh/generated/urdf/iCubParis01/icub.urdf
 delete mode 100644 dh/generated/urdf/iCubParis02/icub.urdf
 delete mode 100644 dh/generated/urdf/icubGazeboSim/icub.urdf
 delete mode 100644 dh/generated/urdf/icubGazeboSimNoFT/icub.urdf
 delete mode 100644 dh/generator/CMakeLists.txt
 delete mode 100644 dh/generator/CMakeLists2.txt
 delete mode 100644 dh/generator/README
 delete mode 100644 dh/generator/cmake/FindEigen3.cmake
 delete mode 100644 dh/generator/cmake/FindTinyXML.cmake
 delete mode 100644 dh/generator/deprecated_icub_urdf_sdf_generator.cpp
 delete mode 100644 dh/generator/icub_dh_to_urdf_sdf_utils.cpp
 delete mode 100644 dh/generator/icub_urdf_from_iDyn.cpp
 delete mode 100644 dh/generator/icub_urdf_sdf_from_dh_generator.cpp
 delete mode 100644 dh/generator/urdf_gazebo_cleanup.cpp
 delete mode 100644 dh/generator/urdf_get_limits.cpp
 delete mode 100644 dh/generator/urdf_get_meshes.cpp
 delete mode 100644 dh/generator/urdf_get_total_mass.cpp
 delete mode 100644 dh/generator/urdf_print_homogeneous_transformations.cpp
 delete mode 100644 dh/generator/urdf_sdf_from_dh_utils.cpp
 delete mode 100644 dh/generator/urdf_sdf_from_dh_utils.h
 delete mode 100644 dh/generator/urdf_utils.cpp
 delete mode 100644 dh/generator/urdf_utils.h
 delete mode 100644 dh/toBeIntegrated/README.md
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/CMakeLists.txt
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_head.xml
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_arm.xml
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_hand.xml
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_leg.xml
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_arm.xml
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_hand.xml
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_leg.xml
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_torso.xml
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/include/Constants.h
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/include/ICUBConfigParser.h
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/src/ICUBConfigParser.cpp
 delete mode 100644 dh/toBeIntegrated/icub2gazeboPIDconverters/src/main.cpp

diff --git a/dh/generated/gazebo_models/database.config b/dh/generated/gazebo_models/database.config
deleted file mode 100644
index c7fcdf7..0000000
--- a/dh/generated/gazebo_models/database.config
+++ /dev/null
@@ -1,13 +0,0 @@
-<?xml version='1.0'?>
-<database>
-<name>icub-model-generator Database</name>
-<models><uri>file://iCubGenova01</uri>
-<uri>file://iCubGenova03</uri>
-<uri>file://iCubParis01</uri>
-<uri>file://iCubParis02</uri>
-<uri>file://iCubDarmstad01</uri>
-<uri>file://icubGazeboSim</uri>
-<uri>file://icubGazeboSimNoFT</uri>
-</models>
-</database>
-
diff --git a/dh/generated/gazebo_models/iCubDarmstadt01/icub.sdf b/dh/generated/gazebo_models/iCubDarmstadt01/icub.sdf
deleted file mode 100644
index 9d6d6e8..0000000
--- a/dh/generated/gazebo_models/iCubDarmstadt01/icub.sdf
+++ /dev/null
@@ -1,1970 +0,0 @@
-<sdf version='1.5'>
-  <model name='iCub'>
-    <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>8.0431e-05 -0.00691643 -0.0023047 0 -0 0</pose>
-        <mass>0.695</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-2.2209e-05</ixy>
-          <ixz>1.39712e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>4.77629e-05</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.0010964 -3.13229e-05 0.0598624 0 -0 0</pose>
-        <mass>0.982</mass>
-        <inertia>
-          <ixx>0.0253646</ixx>
-          <ixy>1.2116e-06</ixy>
-          <ixz>0.000491081</ixz>
-          <iyy>0.0272714</iyy>
-          <iyz>1.15713e-05</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>-2.07694</lower>
-          <upper>0.296706</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.0021968 -0.174502 0.0013437 0 -0 0</pose>
-        <mass>1.522</mass>
-        <inertia>
-          <ixx>0.0454917</ixx>
-          <ixy>-0.000940971</ixy>
-          <ixz>5.37405e-05</ixz>
-          <iyy>0.01004</iyy>
-          <iyz>0.001203</iyz>
-          <izz>0.0458255</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>0.000915777 -0.0680985 -0.354445 2.09198 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.0761044 -4.50091e-05 0.0062408 0 -0 0</pose>
-        <mass>2.032</mass>
-        <inertia>
-          <ixx>0.0154559</ixx>
-          <ixy>-0.0013903</ixy>
-          <ixz>-0.0039454</ixz>
-          <iyy>0.070277</iyy>
-          <iyz>0.00071995</iyz>
-          <izz>0.0659645</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_lower_leg_collision'>
-        <pose>0.010945 0.0009175 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>0.010945 0.0009175 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>0.000913568 -0.0680972 -0.554945 0.52118 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>2.65892e-05 -0.00082253 0.0102574 0 -0 0</pose>
-        <mass>0.643</mass>
-        <inertia>
-          <ixx>4.76593</ixx>
-          <ixy>9.7426e-06</ixy>
-          <ixz>2.68085e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-0.000353796</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_ankle_1_collision'>
-        <pose>-0.001555 -5.54343e-12 0.0009175 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>-0.001555 -5.54343e-12 0.0009175 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>0.000913568 -0.0680972 -0.554945 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>-0.0035 0 0.0645 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>0.00441309 -0.0680969 -0.619445 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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>0.0044175 -5.54341e-12 -0.062945 -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>8.0431e-05 -0.00691643 0.0023047 0 -0 0</pose>
-        <mass>0.695</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-2.2209e-05</ixy>
-          <ixz>-1.39712e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-4.77629e-05</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.0010964 -3.13229e-05 0.0598624 0 -0 0</pose>
-        <mass>0.982</mass>
-        <inertia>
-          <ixx>0.0253646</ixx>
-          <ixy>1.2116e-06</ixy>
-          <ixz>0.000491081</ixz>
-          <iyy>0.0272714</iyy>
-          <iyz>1.15713e-05</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>-2.07694</lower>
-          <upper>0.296706</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.0021968 -0.174502 -0.0013437 0 -0 0</pose>
-        <mass>1.522</mass>
-        <inertia>
-          <ixx>0.0454917</ixx>
-          <ixy>-0.000940971</ixy>
-          <ixz>-5.37405e-05</ixz>
-          <iyy>0.01004</iyy>
-          <iyz>-0.001203</iyz>
-          <izz>0.0458255</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>0.000915777 0.0681009 -0.354445 1.89254 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.0761044 -4.50091e-05 -0.0062408 0 -0 0</pose>
-        <mass>2.032</mass>
-        <inertia>
-          <ixx>0.0154559</ixx>
-          <ixy>-0.0013903</ixy>
-          <ixz>0.0039454</ixz>
-          <iyy>0.070277</iyy>
-          <iyz>-0.00071995</iyz>
-          <izz>0.0659645</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_lower_leg_collision'>
-        <pose>0.010945 0.0009175 -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>0.010945 0.0009175 -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>0.000913568 0.0681016 -0.554945 -2.81984 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-2.65892e-05 -0.00082253 -0.0102574 0 -0 0</pose>
-        <mass>0.643</mass>
-        <inertia>
-          <ixx>4.76593</ixx>
-          <ixy>9.7426e-06</ixy>
-          <ixz>-2.68085e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0.000353796</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_ankle_1_collision'>
-        <pose>-0.001555 -1.08717e-12 -0.0009175 -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>-0.001555 -1.08717e-12 -0.0009175 -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>0.000913568 0.0681016 -0.554945 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>-0.0035 4.28626e-19 0.0645 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.70198e-38</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>0.00441265 0.0681021 -0.619445 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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>0.0044175 1.08716e-12 -0.062945 -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.698132</lower>
-          <upper>0.523599</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='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>-1.22173</lower>
-          <upper>1.0472</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='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>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head.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.959931</lower>
-          <upper>0.959931</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_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>
-    </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' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/dh/generated/gazebo_models/iCubDarmstadt01/icub_simulation.urdf b/dh/generated/gazebo_models/iCubDarmstadt01/icub_simulation.urdf
deleted file mode 100644
index e5bd998..0000000
--- a/dh/generated/gazebo_models/iCubDarmstadt01/icub_simulation.urdf
+++ /dev/null
@@ -1,1230 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.643" />
-            <origin xyz="2.65892e-05 -0.00082253 0.0102574" rpy="0 -0 0" />
-            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="2.68085e-05" iyy="0.01" iyz="-0.000353796" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.15e-07" ixz="-4.08e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.01" iyz="-6.62361e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.337e-06" ixz="2.39e-07" iyy="0.01" iyz="1.9381e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.278e-05" ixz="4.823e-06" iyy="0.01" iyz="-1.8188e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.695" />
-            <origin xyz="8.0431e-05 -0.00691643 -0.0023047" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-2.2209e-05" ixz="1.39712e-05" iyy="0.01" iyz="4.77629e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.982" />
-            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
-            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="2.032" />
-            <origin xyz="-0.0761044 -4.50091e-05 0.0062408" rpy="0 -0 0" />
-            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="-0.0039454" iyy="0.070277" iyz="0.00071995" izz="0.0659645" />
-        </inertial>
-        <visual>
-            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="9e-09" ixz="0" iyy="0.01" iyz="-1.7e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.66e-07" ixz="3.65e-07" iyy="0.01" iyz="-2.0524e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="0.01" iyz="-7.42844e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="1.522" />
-            <origin xyz="0.0021968 -0.174502 0.0013437" rpy="0 -0 0" />
-            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="5.37405e-05" iyy="0.01004" iyz="0.001203" izz="0.0458255" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.643" />
-            <origin xyz="-2.65892e-05 -0.00082253 -0.0102574" rpy="0 -0 0" />
-            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="-2.68085e-05" iyy="0.01" iyz="0.000353796" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.70198e-38" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.02e-07" ixz="-3.99e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.51893e-06" ixz="0.000567784" iyy="0.01" iyz="6.62535e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.66e-06" ixz="1.4e-06" iyy="0.01" iyz="1.82e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.01" iyz="1.76e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.695" />
-            <origin xyz="8.0431e-05 -0.00691643 0.0023047" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-2.2209e-05" ixz="-1.39712e-05" iyy="0.01" iyz="-4.77629e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.982" />
-            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
-            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="2.032" />
-            <origin xyz="-0.0761044 -4.50091e-05 -0.0062408" rpy="0 -0 0" />
-            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="0.0039454" iyy="0.070277" iyz="-0.00071995" izz="0.0659645" />
-        </inertial>
-        <visual>
-            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.1e-08" ixz="-1e-09" iyy="0.01" iyz="4.22e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.53e-07" ixz="2.03e-07" iyy="0.01" iyz="2.07e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="0.01" iyz="-6.5128e-06" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="1.522" />
-            <origin xyz="0.0021968 -0.174502 -0.0013437" rpy="0 -0 0" />
-            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="-5.37405e-05" iyy="0.01004" iyz="-0.001203" izz="0.0458255" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.01" iyz="-3.849e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.2005 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="-0.0009175 -0.234545 -1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 -1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.2005 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="-0.0009175 -0.234545 1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-</robot>
diff --git a/dh/generated/gazebo_models/iCubDarmstadt01/model.config b/dh/generated/gazebo_models/iCubDarmstadt01/model.config
deleted file mode 100644
index e263555..0000000
--- a/dh/generated/gazebo_models/iCubDarmstadt01/model.config
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0"?>
-<model>
-<name>iCubDarmstadt01</name><version>1.0</version><sdf version='1.4'>icub.sdf</sdf>
-</model>
diff --git a/dh/generated/gazebo_models/iCubGenova01/icub.sdf b/dh/generated/gazebo_models/iCubGenova01/icub.sdf
deleted file mode 100644
index 9d6d6e8..0000000
--- a/dh/generated/gazebo_models/iCubGenova01/icub.sdf
+++ /dev/null
@@ -1,1970 +0,0 @@
-<sdf version='1.5'>
-  <model name='iCub'>
-    <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>8.0431e-05 -0.00691643 -0.0023047 0 -0 0</pose>
-        <mass>0.695</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-2.2209e-05</ixy>
-          <ixz>1.39712e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>4.77629e-05</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.0010964 -3.13229e-05 0.0598624 0 -0 0</pose>
-        <mass>0.982</mass>
-        <inertia>
-          <ixx>0.0253646</ixx>
-          <ixy>1.2116e-06</ixy>
-          <ixz>0.000491081</ixz>
-          <iyy>0.0272714</iyy>
-          <iyz>1.15713e-05</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>-2.07694</lower>
-          <upper>0.296706</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.0021968 -0.174502 0.0013437 0 -0 0</pose>
-        <mass>1.522</mass>
-        <inertia>
-          <ixx>0.0454917</ixx>
-          <ixy>-0.000940971</ixy>
-          <ixz>5.37405e-05</ixz>
-          <iyy>0.01004</iyy>
-          <iyz>0.001203</iyz>
-          <izz>0.0458255</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>0.000915777 -0.0680985 -0.354445 2.09198 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>-0.0761044 -4.50091e-05 0.0062408 0 -0 0</pose>
-        <mass>2.032</mass>
-        <inertia>
-          <ixx>0.0154559</ixx>
-          <ixy>-0.0013903</ixy>
-          <ixz>-0.0039454</ixz>
-          <iyy>0.070277</iyy>
-          <iyz>0.00071995</iyz>
-          <izz>0.0659645</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_lower_leg_collision'>
-        <pose>0.010945 0.0009175 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>0.010945 0.0009175 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>0.000913568 -0.0680972 -0.554945 0.52118 -1.57078 -0.521189</pose>
-      <inertial>
-        <pose>2.65892e-05 -0.00082253 0.0102574 0 -0 0</pose>
-        <mass>0.643</mass>
-        <inertia>
-          <ixx>4.76593</ixx>
-          <ixy>9.7426e-06</ixy>
-          <ixz>2.68085e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-0.000353796</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_ankle_1_collision'>
-        <pose>-0.001555 -5.54343e-12 0.0009175 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>-0.001555 -5.54343e-12 0.0009175 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>0.000913568 -0.0680972 -0.554945 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>-0.0035 0 0.0645 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>0.00441309 -0.0680969 -0.619445 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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>0.0044175 -5.54341e-12 -0.062945 -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>8.0431e-05 -0.00691643 0.0023047 0 -0 0</pose>
-        <mass>0.695</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-2.2209e-05</ixy>
-          <ixz>-1.39712e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-4.77629e-05</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.0010964 -3.13229e-05 0.0598624 0 -0 0</pose>
-        <mass>0.982</mass>
-        <inertia>
-          <ixx>0.0253646</ixx>
-          <ixy>1.2116e-06</ixy>
-          <ixz>0.000491081</ixz>
-          <iyy>0.0272714</iyy>
-          <iyz>1.15713e-05</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>-2.07694</lower>
-          <upper>0.296706</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.0021968 -0.174502 -0.0013437 0 -0 0</pose>
-        <mass>1.522</mass>
-        <inertia>
-          <ixx>0.0454917</ixx>
-          <ixy>-0.000940971</ixy>
-          <ixz>-5.37405e-05</ixz>
-          <iyy>0.01004</iyy>
-          <iyz>-0.001203</iyz>
-          <izz>0.0458255</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>0.000915777 0.0681009 -0.354445 1.89254 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-0.0761044 -4.50091e-05 -0.0062408 0 -0 0</pose>
-        <mass>2.032</mass>
-        <inertia>
-          <ixx>0.0154559</ixx>
-          <ixy>-0.0013903</ixy>
-          <ixz>0.0039454</ixz>
-          <iyy>0.070277</iyy>
-          <iyz>-0.00071995</iyz>
-          <izz>0.0659645</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_lower_leg_collision'>
-        <pose>0.010945 0.0009175 -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>0.010945 0.0009175 -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>0.000913568 0.0681016 -0.554945 -2.81984 -1.57078 -0.321751</pose>
-      <inertial>
-        <pose>-2.65892e-05 -0.00082253 -0.0102574 0 -0 0</pose>
-        <mass>0.643</mass>
-        <inertia>
-          <ixx>4.76593</ixx>
-          <ixy>9.7426e-06</ixy>
-          <ixz>-2.68085e-05</ixz>
-          <iyy>0.01</iyy>
-          <iyz>0.000353796</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_ankle_1_collision'>
-        <pose>-0.001555 -1.08717e-12 -0.0009175 -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>-0.001555 -1.08717e-12 -0.0009175 -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>0.000913568 0.0681016 -0.554945 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>-0.0035 4.28626e-19 0.0645 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>4.70198e-38</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>0.00441265 0.0681021 -0.619445 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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>0.0044175 1.08716e-12 -0.062945 -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.698132</lower>
-          <upper>0.523599</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='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>-1.22173</lower>
-          <upper>1.0472</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='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>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head.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.959931</lower>
-          <upper>0.959931</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_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>
-    </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' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/dh/generated/gazebo_models/iCubGenova01/icub_simulation.urdf b/dh/generated/gazebo_models/iCubGenova01/icub_simulation.urdf
deleted file mode 100644
index e5bd998..0000000
--- a/dh/generated/gazebo_models/iCubGenova01/icub_simulation.urdf
+++ /dev/null
@@ -1,1230 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.643" />
-            <origin xyz="2.65892e-05 -0.00082253 0.0102574" rpy="0 -0 0" />
-            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="2.68085e-05" iyy="0.01" iyz="-0.000353796" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.15e-07" ixz="-4.08e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.01" iyz="-6.62361e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.337e-06" ixz="2.39e-07" iyy="0.01" iyz="1.9381e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.278e-05" ixz="4.823e-06" iyy="0.01" iyz="-1.8188e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.695" />
-            <origin xyz="8.0431e-05 -0.00691643 -0.0023047" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-2.2209e-05" ixz="1.39712e-05" iyy="0.01" iyz="4.77629e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.982" />
-            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
-            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="2.032" />
-            <origin xyz="-0.0761044 -4.50091e-05 0.0062408" rpy="0 -0 0" />
-            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="-0.0039454" iyy="0.070277" iyz="0.00071995" izz="0.0659645" />
-        </inertial>
-        <visual>
-            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="9e-09" ixz="0" iyy="0.01" iyz="-1.7e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.66e-07" ixz="3.65e-07" iyy="0.01" iyz="-2.0524e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="0.01" iyz="-7.42844e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="1.522" />
-            <origin xyz="0.0021968 -0.174502 0.0013437" rpy="0 -0 0" />
-            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="5.37405e-05" iyy="0.01004" iyz="0.001203" izz="0.0458255" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.643" />
-            <origin xyz="-2.65892e-05 -0.00082253 -0.0102574" rpy="0 -0 0" />
-            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="-2.68085e-05" iyy="0.01" iyz="0.000353796" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.70198e-38" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.02e-07" ixz="-3.99e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.51893e-06" ixz="0.000567784" iyy="0.01" iyz="6.62535e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.66e-06" ixz="1.4e-06" iyy="0.01" iyz="1.82e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.01" iyz="1.76e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.695" />
-            <origin xyz="8.0431e-05 -0.00691643 0.0023047" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-2.2209e-05" ixz="-1.39712e-05" iyy="0.01" iyz="-4.77629e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.982" />
-            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
-            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="2.032" />
-            <origin xyz="-0.0761044 -4.50091e-05 -0.0062408" rpy="0 -0 0" />
-            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="0.0039454" iyy="0.070277" iyz="-0.00071995" izz="0.0659645" />
-        </inertial>
-        <visual>
-            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.1e-08" ixz="-1e-09" iyy="0.01" iyz="4.22e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.53e-07" ixz="2.03e-07" iyy="0.01" iyz="2.07e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="0.01" iyz="-6.5128e-06" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="1.522" />
-            <origin xyz="0.0021968 -0.174502 -0.0013437" rpy="0 -0 0" />
-            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="-5.37405e-05" iyy="0.01004" iyz="-0.001203" izz="0.0458255" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.01" iyz="-3.849e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.2005 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="-0.0009175 -0.234545 -1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 -1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.2005 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="-0.0009175 -0.234545 1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-</robot>
diff --git a/dh/generated/gazebo_models/iCubGenova01/model.config b/dh/generated/gazebo_models/iCubGenova01/model.config
deleted file mode 100644
index ce56c77..0000000
--- a/dh/generated/gazebo_models/iCubGenova01/model.config
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0"?>
-<model>
-<name>iCubGenova01</name><version>1.0</version><sdf version='1.4'>icub.sdf</sdf>
-</model>
diff --git a/dh/generated/gazebo_models/iCubGenova03/icub.sdf b/dh/generated/gazebo_models/iCubGenova03/icub.sdf
deleted file mode 100644
index 125325b..0000000
--- a/dh/generated/gazebo_models/iCubGenova03/icub.sdf
+++ /dev/null
@@ -1,1970 +0,0 @@
-<sdf version='1.5'>
-  <model name='iCub'>
-    <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>-2.07694</lower>
-          <upper>0.296706</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.017 0 0.042 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>-0.0170043 -0.0680968 -0.5985 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0.007069 -0.000613931 -0.0334154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.3505e-05</ixy>
-          <ixz>0.0001823</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.30533e-06</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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>-0.017 -5.54343e-12 -0.042 -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>-2.07694</lower>
-          <upper>0.296706</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.017 0 0.042 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>-0.0170046 0.0681019 -0.5985 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0.007069 0.000613931 -0.0334154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.3505e-05</ixy>
-          <ixz>0.0001823</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.30551e-06</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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>-0.017 1.08717e-12 -0.042 -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.698132</lower>
-          <upper>0.523599</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='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>-1.22173</lower>
-          <upper>1.0472</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='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>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head.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.959931</lower>
-          <upper>0.959931</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_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>
-    </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' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/dh/generated/gazebo_models/iCubGenova03/icub_simulation.urdf b/dh/generated/gazebo_models/iCubGenova03/icub_simulation.urdf
deleted file mode 100644
index fd6fc49..0000000
--- a/dh/generated/gazebo_models/iCubGenova03/icub_simulation.urdf
+++ /dev/null
@@ -1,1230 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.01" iyz="2.0817e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.15e-07" ixz="-4.08e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.007069 -0.000613931 -0.0334154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.3505e-05" ixz="0.0001823" iyy="0.01" iyz="-3.30533e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.017 -5.54343e-12 -0.042" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.017 -5.54343e-12 -0.042" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.337e-06" ixz="2.39e-07" iyy="0.01" iyz="1.9381e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.278e-05" ixz="4.823e-06" iyy="0.01" iyz="-1.8188e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.059e-06" ixz="1.451e-06" iyy="0.01" iyz="1.545e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.01" iyz="-6.2e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.01" iyz="7.86e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="9e-09" ixz="0" iyy="0.01" iyz="-1.7e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.66e-07" ixz="3.65e-07" iyy="0.01" iyz="-2.0524e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="0.01" iyz="-7.42844e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.01" iyz="3.63726e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="0.01" iyz="3.38813e-21" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.02e-07" ixz="-3.99e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.007069 0.000613931 -0.0334154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.3505e-05" ixz="0.0001823" iyy="0.01" iyz="3.30551e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.017 1.08717e-12 -0.042" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.017 1.08717e-12 -0.042" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.66e-06" ixz="1.4e-06" iyy="0.01" iyz="1.82e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.01" iyz="1.76e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="0.01" iyz="8.47033e-22" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.1e-08" ixz="-1e-09" iyy="0.01" iyz="4.22e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.53e-07" ixz="2.03e-07" iyy="0.01" iyz="2.07e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="0.01" iyz="-6.5128e-06" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="-2.71051e-20" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.01" iyz="-3.849e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -7.60894e-35 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 0 -0.001" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -7.60894e-35 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 0 -0.001" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -1.38778e-17 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-</robot>
diff --git a/dh/generated/gazebo_models/iCubGenova03/model.config b/dh/generated/gazebo_models/iCubGenova03/model.config
deleted file mode 100644
index a929191..0000000
--- a/dh/generated/gazebo_models/iCubGenova03/model.config
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0"?>
-<model>
-<name>iCubGenova03</name><version>1.0</version><sdf version='1.4'>icub.sdf</sdf>
-</model>
diff --git a/dh/generated/gazebo_models/iCubParis01/icub.sdf b/dh/generated/gazebo_models/iCubParis01/icub.sdf
deleted file mode 100644
index f599e95..0000000
--- a/dh/generated/gazebo_models/iCubParis01/icub.sdf
+++ /dev/null
@@ -1,1970 +0,0 @@
-<sdf version='1.5'>
-  <model name='iCub'>
-    <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>-2.07694</lower>
-          <upper>0.296706</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.017 0 0.042 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>-0.0170043 -0.0680968 -0.5985 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0.007069 -0.000613931 -0.0334154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.3505e-05</ixy>
-          <ixz>0.0001823</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-3.30533e-06</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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>-0.017 -5.54343e-12 -0.042 -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>-2.07694</lower>
-          <upper>0.296706</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.017 0 0.042 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>-0.0170046 0.0681019 -0.5985 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0.007069 0.000613931 -0.0334154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>1.3505e-05</ixy>
-          <ixz>0.0001823</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.30551e-06</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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>-0.017 1.08717e-12 -0.042 -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.00246408 -2.13821e-50 -0.000235713 0 -0 0</pose>
-        <mass>0.270176</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-1.77658e-07</ixy>
-          <ixz>4.49143e-07</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.26829e-07</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.698132</lower>
-          <upper>0.523599</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='neck_2'>
-      <pose>-0.0078081 1.22349e-08 0.2583 -1.46928e-05 3.67327e-06 -1.57079</pose>
-      <inertial>
-        <pose>0 0.00337529 0.00545442 0 -0 0</pose>
-        <mass>0.272306</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-5.92615e-09</ixy>
-          <ixz>-2.20067e-09</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-9.13211e-06</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='neck_2_collision'>
-        <pose>-9.62885e-13 0.0055 -5.55112e-17 -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 -5.55112e-17 -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>-1.22173</lower>
-          <upper>1.0472</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='head'>
-      <pose>-0.0088081 7.54212e-09 0.2583 -1.5708 1.46928e-05 8.36603e-06</pose>
-      <inertial>
-        <pose>0.0225 -0.1005 6.15385e-18 0 -0 0</pose>
-        <mass>3</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>-4.81482e-35</ixz>
-          <iyy>0.01</iyy>
-          <iyz>3.85186e-34</iyz>
-          <izz>0.01</izz>
-        </inertia>
-      </inertial>
-      <collision name='head_collision'>
-        <pose>0.0055 -5.05151e-15 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.0055 -5.05151e-15 9.42472e-13 -1.32582 1.57079 -2.89662</pose>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head.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.959931</lower>
-          <upper>0.959931</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_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>
-    </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' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/dh/generated/gazebo_models/iCubParis01/icub_simulation.urdf b/dh/generated/gazebo_models/iCubParis01/icub_simulation.urdf
deleted file mode 100644
index 17c1f64..0000000
--- a/dh/generated/gazebo_models/iCubParis01/icub_simulation.urdf
+++ /dev/null
@@ -1,1230 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0225 -0.1005 6.15385e-18" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="-4.81482e-35" iyy="0.01" iyz="3.85186e-34" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0055 -5.05151e-15 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0055 -5.05151e-15 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.01" iyz="2.0817e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.15e-07" ixz="-4.08e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.007069 -0.000613931 -0.0334154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.3505e-05" ixz="0.0001823" iyy="0.01" iyz="-3.30533e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.017 -5.54343e-12 -0.042" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.017 -5.54343e-12 -0.042" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.337e-06" ixz="2.39e-07" iyy="0.01" iyz="1.9381e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.278e-05" ixz="4.823e-06" iyy="0.01" iyz="-1.8188e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.059e-06" ixz="1.451e-06" iyy="0.01" iyz="1.545e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.01" iyz="-6.2e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.01" iyz="7.86e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="9e-09" ixz="0" iyy="0.01" iyz="-1.7e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.66e-07" ixz="3.65e-07" iyy="0.01" iyz="-2.0524e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="0.01" iyz="-7.42844e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.01" iyz="3.63726e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0.270176" />
-            <origin xyz="0.00246408 -2.13821e-50 -0.000235713" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.77658e-07" ixz="4.49143e-07" iyy="0.01" iyz="-1.26829e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0.272306" />
-            <origin xyz="0 0.00337529 0.00545442" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.92615e-09" ixz="-2.20067e-09" iyy="0.01" iyz="-9.13211e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 -5.55112e-17" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 -5.55112e-17" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="0.01" iyz="3.38813e-21" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.02e-07" ixz="-3.99e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.007069 0.000613931 -0.0334154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.3505e-05" ixz="0.0001823" iyy="0.01" iyz="3.30551e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.017 1.08717e-12 -0.042" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.017 1.08717e-12 -0.042" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.66e-06" ixz="1.4e-06" iyy="0.01" iyz="1.82e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.01" iyz="1.76e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="0.01" iyz="8.47033e-22" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.1e-08" ixz="-1e-09" iyy="0.01" iyz="4.22e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.53e-07" ixz="2.03e-07" iyy="0.01" iyz="2.07e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="0.01" iyz="-6.5128e-06" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="-2.71051e-20" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.01" iyz="-3.849e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -7.60894e-35 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="0 -0.001 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0225 -0.1005 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 0 -0.001" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -7.60894e-35 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.033 -2.13821e-50 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 0 -0.001" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -1.38778e-17 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-</robot>
diff --git a/dh/generated/gazebo_models/iCubParis01/model.config b/dh/generated/gazebo_models/iCubParis01/model.config
deleted file mode 100644
index 0bd8fe2..0000000
--- a/dh/generated/gazebo_models/iCubParis01/model.config
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0"?>
-<model>
-<name>iCubParis01</name><version>1.0</version><sdf version='1.4'>icub.sdf</sdf>
-</model>
diff --git a/dh/generated/gazebo_models/iCubParis02/icub.sdf b/dh/generated/gazebo_models/iCubParis02/icub.sdf
deleted file mode 100644
index da868cc..0000000
--- a/dh/generated/gazebo_models/iCubParis02/icub.sdf
+++ /dev/null
@@ -1,1970 +0,0 @@
-<sdf version='1.5'>
-  <model name='iCub'>
-    <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>-2.07694</lower>
-          <upper>0.296706</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.008 0 0.0553 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>1.35525e-20</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>-0.0080044 -0.0680968 -0.6118 3.14159 -7.34645e-06 3.14158</pose>
-      <inertial>
-        <pose>0.016069 -0.000613931 -0.0467154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>-8.74769e-06</ixy>
-          <ixz>0.000625247</ixz>
-          <iyy>0.01</iyy>
-          <iyz>-1.03356e-05</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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>-0.008 -5.54343e-12 -0.0553 -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>-2.07694</lower>
-          <upper>0.296706</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.008 0 0.0553 0 -0 0</pose>
-        <mass>0.2675</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>0</ixy>
-          <ixz>1.35525e-20</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>-0.00800478 0.068102 -0.6118 3.14159 -1.4236e-05 -3.14159</pose>
-      <inertial>
-        <pose>0.016069 0.000613931 -0.0467154 0 -0 0</pose>
-        <mass>0.5935</mass>
-        <inertia>
-          <ixx>0.01</ixx>
-          <ixy>8.74769e-06</ixy>
-          <ixz>0.000625247</ixz>
-          <iyy>0.01</iyy>
-          <iyz>1.03358e-05</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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>-0.008 1.08717e-12 -0.0553 -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.698132</lower>
-          <upper>0.523599</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='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>-1.22173</lower>
-          <upper>1.0472</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='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>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head.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.959931</lower>
-          <upper>0.959931</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_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>
-    </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' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/dh/generated/gazebo_models/iCubParis02/icub_simulation.urdf b/dh/generated/gazebo_models/iCubParis02/icub_simulation.urdf
deleted file mode 100644
index ff5c402..0000000
--- a/dh/generated/gazebo_models/iCubParis02/icub_simulation.urdf
+++ /dev/null
@@ -1,1230 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.01" iyz="2.0817e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.008 0 0.0553" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="1.35525e-20" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.15e-07" ixz="-4.08e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.016069 -0.000613931 -0.0467154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-8.74769e-06" ixz="0.000625247" iyy="0.01" iyz="-1.03356e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.008 -5.54343e-12 -0.0553" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.008 -5.54343e-12 -0.0553" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.337e-06" ixz="2.39e-07" iyy="0.01" iyz="1.9381e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.278e-05" ixz="4.823e-06" iyy="0.01" iyz="-1.8188e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.059e-06" ixz="1.451e-06" iyy="0.01" iyz="1.545e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.01" iyz="-6.2e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.01" iyz="7.86e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="9e-09" ixz="0" iyy="0.01" iyz="-1.7e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.66e-07" ixz="3.65e-07" iyy="0.01" iyz="-2.0524e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="0.01" iyz="-7.42844e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.01" iyz="3.63726e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="0.01" iyz="3.38813e-21" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.008 0 0.0553" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="1.35525e-20" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.02e-07" ixz="-3.99e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.016069 0.000613931 -0.0467154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="8.74769e-06" ixz="0.000625247" iyy="0.01" iyz="1.03358e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.008 1.08717e-12 -0.0553" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.008 1.08717e-12 -0.0553" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.66e-06" ixz="1.4e-06" iyy="0.01" iyz="1.82e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.01" iyz="1.76e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="0.01" iyz="8.47033e-22" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.1e-08" ixz="-1e-09" iyy="0.01" iyz="4.22e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.53e-07" ixz="2.03e-07" iyy="0.01" iyz="2.07e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="0.01" iyz="-6.5128e-06" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="-2.71051e-20" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.01" iyz="-3.849e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 -5.66898e-35 -0.0143" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0.008 0 0.0553" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 0 -0.0143" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 -5.66898e-35 -0.0143" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0.008 0 0.0553" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 0 -0.0143" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 -1.38778e-17 -0.0143" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-</robot>
diff --git a/dh/generated/gazebo_models/iCubParis02/model.config b/dh/generated/gazebo_models/iCubParis02/model.config
deleted file mode 100644
index 190379b..0000000
--- a/dh/generated/gazebo_models/iCubParis02/model.config
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0"?>
-<model>
-<name>iCubParis02</name><version>1.0</version><sdf version='1.4'>icub.sdf</sdf>
-</model>
diff --git a/dh/generated/gazebo_models/icubGazeboSim/icub.sdf b/dh/generated/gazebo_models/icubGazeboSim/icub.sdf
deleted file mode 100644
index 86412be..0000000
--- a/dh/generated/gazebo_models/icubGazeboSim/icub.sdf
+++ /dev/null
@@ -1,1970 +0,0 @@
-<sdf version='1.5'>
-  <model name='iCub'>
-    <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>-2.07694</lower>
-          <upper>0.296706</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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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>-2.07694</lower>
-          <upper>0.296706</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>
-              <kd>1</kd>
-              <max_vel>100</max_vel>
-              <min_depth>0.001</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.698132</lower>
-          <upper>0.523599</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='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>-1.22173</lower>
-          <upper>1.0472</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='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>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head.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.959931</lower>
-          <upper>0.959931</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_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>
-    </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' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/dh/generated/gazebo_models/icubGazeboSim/icub_simulation.urdf b/dh/generated/gazebo_models/icubGazeboSim/icub_simulation.urdf
deleted file mode 100644
index b67746f..0000000
--- a/dh/generated/gazebo_models/icubGazeboSim/icub_simulation.urdf
+++ /dev/null
@@ -1,1230 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.01" iyz="2.0817e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0 0 0.037" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.15e-07" ixz="-4.08e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.01" iyz="-6.62361e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="4.45647e-12 -5.54343e-12 -0.037" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="4.45647e-12 -5.54343e-12 -0.037" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.337e-06" ixz="2.39e-07" iyy="0.01" iyz="1.9381e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.278e-05" ixz="4.823e-06" iyy="0.01" iyz="-1.8188e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.059e-06" ixz="1.451e-06" iyy="0.01" iyz="1.545e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.01" iyz="-6.2e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.01" iyz="7.86e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="9e-09" ixz="0" iyy="0.01" iyz="-1.7e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.66e-07" ixz="3.65e-07" iyy="0.01" iyz="-2.0524e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="0.01" iyz="-7.42844e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.01" iyz="3.63726e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="0.01" iyz="3.38813e-21" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0 0 0.037" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.02e-07" ixz="-3.99e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.51893e-06" ixz="0.000567784" iyy="0.01" iyz="6.62535e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="4.45647e-12 1.08717e-12 -0.037" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="4.45647e-12 1.08717e-12 -0.037" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.66e-06" ixz="1.4e-06" iyy="0.01" iyz="1.82e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.01" iyz="1.76e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="0.01" iyz="8.47033e-22" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.1e-08" ixz="-1e-09" iyy="0.01" iyz="4.22e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.53e-07" ixz="2.03e-07" iyy="0.01" iyz="2.07e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="0.01" iyz="-6.5128e-06" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="-2.71051e-20" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.01" iyz="-3.849e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -2.65687e-65 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.037" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -2.65687e-65 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.037" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -9.86076e-35 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-</robot>
diff --git a/dh/generated/gazebo_models/icubGazeboSim/model.config b/dh/generated/gazebo_models/icubGazeboSim/model.config
deleted file mode 100644
index 6240f28..0000000
--- a/dh/generated/gazebo_models/icubGazeboSim/model.config
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0"?>
-<model>
-<name>icubGazeboSim</name><version>1.0</version><sdf version='1.4'>icub.sdf</sdf>
-</model>
diff --git a/dh/generated/gazebo_models/icubGazeboSimNoFT/icub.sdf b/dh/generated/gazebo_models/icubGazeboSimNoFT/icub.sdf
deleted file mode 100644
index 65c5d70..0000000
--- a/dh/generated/gazebo_models/icubGazeboSimNoFT/icub.sdf
+++ /dev/null
@@ -1,1672 +0,0 @@
-<sdf version='1.5'>
-  <model name='iCub'>
-    <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.0362088 0 -0 0</pose>
-        <mass>0.626</mass>
-        <inertia>
-          <ixx>0.0201092</ixx>
-          <ixy>-7.4e-08</ixy>
-          <ixz>1.0835e-05</ixz>
-          <iyy>0.0201092</iyy>
-          <iyz>-6.2e-08</iyz>
-          <izz>0.02</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>-2.07694</lower>
-          <upper>0.296706</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_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_2</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.0165911 -0.000423192 0.0174128 0 -0 0</pose>
-        <mass>0.861</mass>
-        <inertia>
-          <ixx>0.020149</ixx>
-          <ixy>-1.79423e-06</ixy>
-          <ixz>0.000693896</ixz>
-          <iyy>0.0202557</iyy>
-          <iyz>-3.87909e-06</iyz>
-          <izz>0.0201069</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_ankle_2_collision'>
-        <pose>0.03 0 0.026 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-      </collision>
-      <visual name='l_ankle_2_visual'>
-        <pose>4.45647e-12 -5.54343e-12 0 -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_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='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.0362088 0 -0 0</pose>
-        <mass>0.626</mass>
-        <inertia>
-          <ixx>0.0201092</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.0201092</iyy>
-          <iyz>0</iyz>
-          <izz>0.02</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>-2.07694</lower>
-          <upper>0.296706</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_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_2</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.0165911 0.000423192 0.0174128 0 -0 0</pose>
-        <mass>0.861</mass>
-        <inertia>
-          <ixx>0.020149</ixx>
-          <ixy>1.79423e-06</ixy>
-          <ixz>0.000693895</ixz>
-          <iyy>0.0202557</iyy>
-          <iyz>3.87927e-06</iyz>
-          <izz>0.0201069</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_ankle_2_collision'>
-        <pose>0.03 0 0.026 0 -0 0</pose>
-        <geometry>
-          <box>
-            <size>0.15 0.07 0.03</size>
-          </box>
-        </geometry>
-      </collision>
-      <visual name='r_ankle_2_visual'>
-        <pose>4.45647e-12 1.08717e-12 0 -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_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='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.01321 -6.4e-05 0.08938 0 -0 0</pose>
-        <mass>0.884</mass>
-        <inertia>
-          <ixx>0.0202921</ixx>
-          <ixy>-9.03e-07</ixy>
-          <ixz>-6.334e-05</ixz>
-          <iyy>0.0203031</iyy>
-          <iyz>4.031e-06</iyz>
-          <izz>0.020011</izz>
-        </inertia>
-      </inertial>
-      <collision name='l_shoulder_3_collision'>
-        <pose>-0.0054334 0.000143922 0 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_shoulder_3_visual'>
-        <pose>-0.0054334 0.000143922 0 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_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_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_shoulder_3</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.698132</lower>
-          <upper>0.523599</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='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>-1.22173</lower>
-          <upper>1.0472</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='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>
-        <geometry>
-          <mesh>
-            <scale>1 1 1</scale>
-            <uri>model://icub/meshes/visual/icub_head.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.959931</lower>
-          <upper>0.959931</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_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.01321 6.4e-05 0.08938 0 -0 0</pose>
-        <mass>0.884</mass>
-        <inertia>
-          <ixx>0.0203006</ixx>
-          <ixy>-8.51e-07</ixy>
-          <ixz>-6.39001e-05</ixz>
-          <iyy>0.0203116</iyy>
-          <iyz>-3.61e-06</iyz>
-          <izz>0.0200111</izz>
-        </inertia>
-      </inertial>
-      <collision name='r_shoulder_3_collision'>
-        <pose>-0.0054334 -0.000143922 0 -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_shoulder_3_visual'>
-        <pose>-0.0054334 -0.000143922 0 -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_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_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_shoulder_3</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>
-    </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' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_right_arm' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm.ini</yarpConfigurationFile>
-      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-    </plugin>
-    <plugin name='controlboard_left_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_left_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <plugin name='controlboard_right_leg' filename='libgazebo_yarp_controlboard.so'>
-      <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_leg.ini</yarpConfigurationFile>
-    </plugin>
-    <pose>0 0 0.70 0 0 0</pose>
-  </model>
-</sdf>
diff --git a/dh/generated/gazebo_models/icubGazeboSimNoFT/icub_simulation.urdf b/dh/generated/gazebo_models/icubGazeboSimNoFT/icub_simulation.urdf
deleted file mode 100644
index b67746f..0000000
--- a/dh/generated/gazebo_models/icubGazeboSimNoFT/icub_simulation.urdf
+++ /dev/null
@@ -1,1230 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.01" iyz="2.0817e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0 0 0.037" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.15e-07" ixz="-4.08e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.01" iyz="-6.62361e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="4.45647e-12 -5.54343e-12 -0.037" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="4.45647e-12 -5.54343e-12 -0.037" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.337e-06" ixz="2.39e-07" iyy="0.01" iyz="1.9381e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.278e-05" ixz="4.823e-06" iyy="0.01" iyz="-1.8188e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.059e-06" ixz="1.451e-06" iyy="0.01" iyz="1.545e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.01" iyz="-6.2e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.01" iyz="7.86e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="9e-09" ixz="0" iyy="0.01" iyz="-1.7e-08" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.66e-07" ixz="3.65e-07" iyy="0.01" iyz="-2.0524e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="0.01" iyz="-7.42844e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.01" iyz="3.63726e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="0.01" iyz="3.38813e-21" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0 0 0.037" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-5.02e-07" ixz="-3.99e-07" iyy="0.01" iyz="-3.71e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="4.51893e-06" ixz="0.000567784" iyy="0.01" iyz="6.62535e-07" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="4.45647e-12 1.08717e-12 -0.037" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="4.45647e-12 1.08717e-12 -0.037" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.66e-06" ixz="1.4e-06" iyy="0.01" iyz="1.82e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.01" iyz="1.76e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="0.01" iyz="8.47033e-22" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="2.1e-08" ixz="-1e-09" iyy="0.01" iyz="4.22e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.53e-07" ixz="2.03e-07" iyy="0.01" iyz="2.07e-05" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="0.01" iyz="-6.5128e-06" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.01" iyz="3.57e-06" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="-2.71051e-20" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.01" iyz="0" izz="0.01" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.01" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.01" iyz="-3.849e-07" izz="0.01" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -2.65687e-65 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.037" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -2.65687e-65 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.037" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -9.86076e-35 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-</robot>
diff --git a/dh/generated/gazebo_models/icubGazeboSimNoFT/model.config b/dh/generated/gazebo_models/icubGazeboSimNoFT/model.config
deleted file mode 100644
index 8fb2843..0000000
--- a/dh/generated/gazebo_models/icubGazeboSimNoFT/model.config
+++ /dev/null
@@ -1,4 +0,0 @@
-<?xml version="1.0"?>
-<model>
-<name>icubGazeboSimNoFT</name><version>1.0</version><sdf version='1.4'>icub.sdf</sdf>
-</model>
diff --git a/dh/generated/urdf/iCubDarmstadt01/icub.urdf b/dh/generated/urdf/iCubDarmstadt01/icub.urdf
deleted file mode 100644
index f20f51a..0000000
--- a/dh/generated/urdf/iCubDarmstadt01/icub.urdf
+++ /dev/null
@@ -1,1290 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.643" />
-            <origin xyz="2.65892e-05 -0.00082253 0.0102574" rpy="0 -0 0" />
-            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="2.68085e-05" iyy="0.0040935" iyz="-0.000353796" izz="0.0036688" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.8389e-05" ixy="-5.15e-07" ixz="-4.08e-07" iyy="9.244e-06" iyz="-3.71e-07" izz="2.9968e-05" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.00167275" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.00320869" iyz="-6.62361e-07" izz="0.00208378" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000765393" ixy="4.337e-06" ixz="2.39e-07" iyy="0.000164578" iyz="1.9381e-05" izz="0.00069806" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000157143" ixy="1.278e-05" ixz="4.823e-06" iyy="0.000247995" iyz="-1.8188e-05" izz="0.000380535" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.695" />
-            <origin xyz="8.0431e-05 -0.00691643 -0.0023047" rpy="0 -0 0" />
-            <inertia ixx="0.00447053" ixy="-2.2209e-05" ixz="1.39712e-05" iyy="0.00325085" iyz="4.77629e-05" izz="0.00490487" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.982" />
-            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
-            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.0041739" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="2.032" />
-            <origin xyz="-0.0761044 -4.50091e-05 0.0062408" rpy="0 -0 0" />
-            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="-0.0039454" iyy="0.070277" iyz="0.00071995" izz="0.0659645" />
-        </inertial>
-        <visual>
-            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="5.4421e-05" ixy="9e-09" ixz="0" iyy="9.331e-06" iyz="-1.7e-08" izz="5.4862e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.0001372" ixy="4.66e-07" ixz="3.65e-07" iyy="8.2927e-05" iyz="-2.0524e-05" izz="9.9274e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="4.84118e-05" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="5.79067e-05" iyz="-7.42844e-07" izz="6.7103e-05" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="1.522" />
-            <origin xyz="0.0021968 -0.174502 0.0013437" rpy="0 -0 0" />
-            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="5.37405e-05" iyy="0.01004" iyz="0.001203" izz="0.0458255" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.643" />
-            <origin xyz="-2.65892e-05 -0.00082253 -0.0102574" rpy="0 -0 0" />
-            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="-2.68085e-05" iyy="0.0040935" iyz="0.000353796" izz="0.0036688" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="4.70198e-38" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.84e-05" ixy="-5.02e-07" ixz="-3.99e-07" iyy="9.24e-06" iyz="-3.71e-07" izz="2.99e-05" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.00167275" ixy="4.51893e-06" ixz="0.000567784" iyy="0.00320869" iyz="6.62535e-07" izz="0.00208378" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000766" ixy="5.66e-06" ixz="1.4e-06" iyy="0.000164" iyz="1.82e-05" izz="0.000699" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000154" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.00025" iyz="1.76e-05" izz="0.000378" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.695" />
-            <origin xyz="8.0431e-05 -0.00691643 0.0023047" rpy="0 -0 0" />
-            <inertia ixx="0.00447053" ixy="-2.2209e-05" ixz="-1.39712e-05" iyy="0.00325085" iyz="-4.77629e-05" izz="0.00490487" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.982" />
-            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
-            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.0041739" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="2.032" />
-            <origin xyz="-0.0761044 -4.50091e-05 -0.0062408" rpy="0 -0 0" />
-            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="0.0039454" iyy="0.070277" iyz="-0.00071995" izz="0.0659645" />
-        </inertial>
-        <visual>
-            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.000123" ixy="2.1e-08" ixz="-1e-09" iyy="2.44e-05" iyz="4.22e-06" izz="0.000113" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.000137" ixy="-4.53e-07" ixz="2.03e-07" iyy="8.3e-05" iyz="2.07e-05" izz="9.93e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="3.44122e-05" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="4.33544e-05" iyz="-6.5128e-06" izz="6.49393e-05" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="1.522" />
-            <origin xyz="0.0021968 -0.174502 -0.0013437" rpy="0 -0 0" />
-            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="-5.37405e-05" iyy="0.01004" iyz="-0.001203" izz="0.0458255" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.2005 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="-0.0009175 -0.234545 -1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 -1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.2005 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="-0.0009175 -0.234545 1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-    <gazebo reference="l_arm_ft_sensor">
-        <sensor name="left_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_arm_ft_sensor">
-        <sensor name="right_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_leg_ft_sensor">
-        <sensor name="left_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_leg_ft_sensor">
-        <sensor name="right_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_foot_ft_sensor">
-        <sensor name="left_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_foot_ft_sensor">
-        <sensor name="right_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-</robot>
diff --git a/dh/generated/urdf/iCubGenova01/icub.urdf b/dh/generated/urdf/iCubGenova01/icub.urdf
deleted file mode 100644
index f20f51a..0000000
--- a/dh/generated/urdf/iCubGenova01/icub.urdf
+++ /dev/null
@@ -1,1290 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.643" />
-            <origin xyz="2.65892e-05 -0.00082253 0.0102574" rpy="0 -0 0" />
-            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="2.68085e-05" iyy="0.0040935" iyz="-0.000353796" izz="0.0036688" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.8389e-05" ixy="-5.15e-07" ixz="-4.08e-07" iyy="9.244e-06" iyz="-3.71e-07" izz="2.9968e-05" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.00167275" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.00320869" iyz="-6.62361e-07" izz="0.00208378" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000765393" ixy="4.337e-06" ixz="2.39e-07" iyy="0.000164578" iyz="1.9381e-05" izz="0.00069806" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000157143" ixy="1.278e-05" ixz="4.823e-06" iyy="0.000247995" iyz="-1.8188e-05" izz="0.000380535" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.695" />
-            <origin xyz="8.0431e-05 -0.00691643 -0.0023047" rpy="0 -0 0" />
-            <inertia ixx="0.00447053" ixy="-2.2209e-05" ixz="1.39712e-05" iyy="0.00325085" iyz="4.77629e-05" izz="0.00490487" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.982" />
-            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
-            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.0041739" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="2.032" />
-            <origin xyz="-0.0761044 -4.50091e-05 0.0062408" rpy="0 -0 0" />
-            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="-0.0039454" iyy="0.070277" iyz="0.00071995" izz="0.0659645" />
-        </inertial>
-        <visual>
-            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="5.4421e-05" ixy="9e-09" ixz="0" iyy="9.331e-06" iyz="-1.7e-08" izz="5.4862e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.0001372" ixy="4.66e-07" ixz="3.65e-07" iyy="8.2927e-05" iyz="-2.0524e-05" izz="9.9274e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="4.84118e-05" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="5.79067e-05" iyz="-7.42844e-07" izz="6.7103e-05" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="1.522" />
-            <origin xyz="0.0021968 -0.174502 0.0013437" rpy="0 -0 0" />
-            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="5.37405e-05" iyy="0.01004" iyz="0.001203" izz="0.0458255" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.643" />
-            <origin xyz="-2.65892e-05 -0.00082253 -0.0102574" rpy="0 -0 0" />
-            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="-2.68085e-05" iyy="0.0040935" iyz="0.000353796" izz="0.0036688" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="4.70198e-38" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.84e-05" ixy="-5.02e-07" ixz="-3.99e-07" iyy="9.24e-06" iyz="-3.71e-07" izz="2.99e-05" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.00167275" ixy="4.51893e-06" ixz="0.000567784" iyy="0.00320869" iyz="6.62535e-07" izz="0.00208378" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000766" ixy="5.66e-06" ixz="1.4e-06" iyy="0.000164" iyz="1.82e-05" izz="0.000699" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000154" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.00025" iyz="1.76e-05" izz="0.000378" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.695" />
-            <origin xyz="8.0431e-05 -0.00691643 0.0023047" rpy="0 -0 0" />
-            <inertia ixx="0.00447053" ixy="-2.2209e-05" ixz="-1.39712e-05" iyy="0.00325085" iyz="-4.77629e-05" izz="0.00490487" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.982" />
-            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
-            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.0041739" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="2.032" />
-            <origin xyz="-0.0761044 -4.50091e-05 -0.0062408" rpy="0 -0 0" />
-            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="0.0039454" iyy="0.070277" iyz="-0.00071995" izz="0.0659645" />
-        </inertial>
-        <visual>
-            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.000123" ixy="2.1e-08" ixz="-1e-09" iyy="2.44e-05" iyz="4.22e-06" izz="0.000113" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.000137" ixy="-4.53e-07" ixz="2.03e-07" iyy="8.3e-05" iyz="2.07e-05" izz="9.93e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="3.44122e-05" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="4.33544e-05" iyz="-6.5128e-06" izz="6.49393e-05" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="1.522" />
-            <origin xyz="0.0021968 -0.174502 -0.0013437" rpy="0 -0 0" />
-            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="-5.37405e-05" iyy="0.01004" iyz="-0.001203" izz="0.0458255" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.2005 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="-0.0009175 -0.234545 -1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 -1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.2005 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="-0.0009175 -0.234545 1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-8.67362e-19 1.38778e-17 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-    <gazebo reference="l_arm_ft_sensor">
-        <sensor name="left_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_arm_ft_sensor">
-        <sensor name="right_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_leg_ft_sensor">
-        <sensor name="left_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_leg_ft_sensor">
-        <sensor name="right_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_foot_ft_sensor">
-        <sensor name="left_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_foot_ft_sensor">
-        <sensor name="right_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-</robot>
diff --git a/dh/generated/urdf/iCubGenova03/icub.urdf b/dh/generated/urdf/iCubGenova03/icub.urdf
deleted file mode 100644
index cf11300..0000000
--- a/dh/generated/urdf/iCubGenova03/icub.urdf
+++ /dev/null
@@ -1,1290 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.00063323" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.00068776" iyz="2.0817e-05" izz="0.000313897" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.8389e-05" ixy="-5.15e-07" ixz="-4.08e-07" iyy="9.244e-06" iyz="-3.71e-07" izz="2.9968e-05" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.007069 -0.000613931 -0.0334154" rpy="0 -0 0" />
-            <inertia ixx="0.00193893" ixy="-1.3505e-05" ixz="0.0001823" iyy="0.0030191" iyz="-3.30533e-06" izz="0.00162801" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.017 -5.54343e-12 -0.042" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.017 -5.54343e-12 -0.042" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000765393" ixy="4.337e-06" ixz="2.39e-07" iyy="0.000164578" iyz="1.9381e-05" izz="0.00069806" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000157143" ixy="1.278e-05" ixz="4.823e-06" iyy="0.000247995" iyz="-1.8188e-05" izz="0.000380535" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.000471076" ixy="2.059e-06" ixz="1.451e-06" iyy="0.000346478" iyz="1.545e-06" izz="0.000510315" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.000561583" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.000738049" iyz="-6.2e-08" izz="0.000294119" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.00099895" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.00445054" iyz="7.86e-07" izz="0.00420766" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="5.4421e-05" ixy="9e-09" ixz="0" iyy="9.331e-06" iyz="-1.7e-08" izz="5.4862e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.0001372" ixy="4.66e-07" ixz="3.65e-07" iyy="8.2927e-05" iyz="-2.0524e-05" izz="9.9274e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="4.84118e-05" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="5.79067e-05" iyz="-7.42844e-07" izz="6.7103e-05" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.00759107" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.00142302" iyz="3.63726e-05" izz="0.00755385" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="2.71051e-20" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="5.42101e-20" iyz="3.38813e-21" izz="1.35525e-20" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.84e-05" ixy="-5.02e-07" ixz="-3.99e-07" iyy="9.24e-06" iyz="-3.71e-07" izz="2.99e-05" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.007069 0.000613931 -0.0334154" rpy="0 -0 0" />
-            <inertia ixx="0.00193893" ixy="1.3505e-05" ixz="0.0001823" iyy="0.0030191" iyz="3.30551e-06" izz="0.00162801" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.017 1.08717e-12 -0.042" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.017 1.08717e-12 -0.042" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000766" ixy="5.66e-06" ixz="1.4e-06" iyy="0.000164" iyz="1.82e-05" izz="0.000699" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000154" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.00025" iyz="1.76e-05" izz="0.000378" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="-5.42101e-20" ixy="0" ixz="0" iyy="-5.42101e-20" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="-6.2511e-19" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="-1.73472e-18" iyz="8.47033e-22" izz="-1.73472e-18" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.000123" ixy="2.1e-08" ixz="-1e-09" iyy="2.44e-05" iyz="4.22e-06" izz="0.000113" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.000137" ixy="-4.53e-07" ixz="2.03e-07" iyy="8.3e-05" iyz="2.07e-05" izz="9.93e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="3.44122e-05" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="4.33544e-05" iyz="-6.5128e-06" izz="6.49393e-05" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="4.07592e-18" iyz="-2.71051e-20" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -7.60894e-35 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 0 -0.001" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -7.60894e-35 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 0 -0.001" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -1.38778e-17 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-    <gazebo reference="l_arm_ft_sensor">
-        <sensor name="left_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_arm_ft_sensor">
-        <sensor name="right_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_leg_ft_sensor">
-        <sensor name="left_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_leg_ft_sensor">
-        <sensor name="right_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_foot_ft_sensor">
-        <sensor name="left_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_foot_ft_sensor">
-        <sensor name="right_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-</robot>
diff --git a/dh/generated/urdf/iCubParis01/icub.urdf b/dh/generated/urdf/iCubParis01/icub.urdf
deleted file mode 100644
index bb08b27..0000000
--- a/dh/generated/urdf/iCubParis01/icub.urdf
+++ /dev/null
@@ -1,1290 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0225 -0.1005 6.15385e-18" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="-4.81482e-35" iyy="-3.46945e-18" iyz="3.85186e-34" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0055 -5.05151e-15 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0055 -5.05151e-15 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.00063323" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.00068776" iyz="2.0817e-05" izz="0.000313897" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.8389e-05" ixy="-5.15e-07" ixz="-4.08e-07" iyy="9.244e-06" iyz="-3.71e-07" izz="2.9968e-05" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.007069 -0.000613931 -0.0334154" rpy="0 -0 0" />
-            <inertia ixx="0.00193893" ixy="-1.3505e-05" ixz="0.0001823" iyy="0.0030191" iyz="-3.30533e-06" izz="0.00162801" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.017 -5.54343e-12 -0.042" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.017 -5.54343e-12 -0.042" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000765393" ixy="4.337e-06" ixz="2.39e-07" iyy="0.000164578" iyz="1.9381e-05" izz="0.00069806" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000157143" ixy="1.278e-05" ixz="4.823e-06" iyy="0.000247995" iyz="-1.8188e-05" izz="0.000380535" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.000471076" ixy="2.059e-06" ixz="1.451e-06" iyy="0.000346478" iyz="1.545e-06" izz="0.000510315" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.000561583" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.000738049" iyz="-6.2e-08" izz="0.000294119" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.00099895" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.00445054" iyz="7.86e-07" izz="0.00420766" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="5.4421e-05" ixy="9e-09" ixz="0" iyy="9.331e-06" iyz="-1.7e-08" izz="5.4862e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.0001372" ixy="4.66e-07" ixz="3.65e-07" iyy="8.2927e-05" iyz="-2.0524e-05" izz="9.9274e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="4.84118e-05" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="5.79067e-05" iyz="-7.42844e-07" izz="6.7103e-05" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.00759107" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.00142302" iyz="3.63726e-05" izz="0.00755385" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0.270176" />
-            <origin xyz="0.00246408 -2.13821e-50 -0.000235713" rpy="0 -0 0" />
-            <inertia ixx="0.000100463" ixy="-1.77658e-07" ixz="4.49143e-07" iyy="4.5426e-05" iyz="-1.26829e-07" izz="2.32934e-21" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0.272306" />
-            <origin xyz="0 0.00337529 0.00545442" rpy="0 -0 0" />
-            <inertia ixx="0.000142823" ixy="-5.92615e-09" ixz="-2.20067e-09" iyy="8.28849e-05" iyz="-9.13211e-06" izz="8.76203e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 -5.55112e-17" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 -5.55112e-17" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="2.71051e-20" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="5.42101e-20" iyz="3.38813e-21" izz="1.35525e-20" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.84e-05" ixy="-5.02e-07" ixz="-3.99e-07" iyy="9.24e-06" iyz="-3.71e-07" izz="2.99e-05" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.007069 0.000613931 -0.0334154" rpy="0 -0 0" />
-            <inertia ixx="0.00193893" ixy="1.3505e-05" ixz="0.0001823" iyy="0.0030191" iyz="3.30551e-06" izz="0.00162801" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.017 1.08717e-12 -0.042" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.017 1.08717e-12 -0.042" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000766" ixy="5.66e-06" ixz="1.4e-06" iyy="0.000164" iyz="1.82e-05" izz="0.000699" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000154" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.00025" iyz="1.76e-05" izz="0.000378" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="-5.42101e-20" ixy="0" ixz="0" iyy="-5.42101e-20" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="-6.2511e-19" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="-1.73472e-18" iyz="8.47033e-22" izz="-1.73472e-18" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.000123" ixy="2.1e-08" ixz="-1e-09" iyy="2.44e-05" iyz="4.22e-06" izz="0.000113" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.000137" ixy="-4.53e-07" ixz="2.03e-07" iyy="8.3e-05" iyz="2.07e-05" izz="9.93e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="3.44122e-05" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="4.33544e-05" iyz="-6.5128e-06" izz="6.49393e-05" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="4.07592e-18" iyz="-2.71051e-20" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -7.60894e-35 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="0 -0.001 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0225 -0.1005 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 0 -0.001" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -7.60894e-35 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.033 -2.13821e-50 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0.017 0 0.042" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 0 -0.001" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0353 -1.38778e-17 -0.001" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-    <gazebo reference="l_arm_ft_sensor">
-        <sensor name="left_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_arm_ft_sensor">
-        <sensor name="right_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_leg_ft_sensor">
-        <sensor name="left_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_leg_ft_sensor">
-        <sensor name="right_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_foot_ft_sensor">
-        <sensor name="left_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_foot_ft_sensor">
-        <sensor name="right_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-</robot>
diff --git a/dh/generated/urdf/iCubParis02/icub.urdf b/dh/generated/urdf/iCubParis02/icub.urdf
deleted file mode 100644
index 36b51a9..0000000
--- a/dh/generated/urdf/iCubParis02/icub.urdf
+++ /dev/null
@@ -1,1290 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.00063323" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.00068776" iyz="2.0817e-05" izz="0.000313897" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.008 0 0.0553" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="1.35525e-20" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.8389e-05" ixy="-5.15e-07" ixz="-4.08e-07" iyy="9.244e-06" iyz="-3.71e-07" izz="2.9968e-05" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.016069 -0.000613931 -0.0467154" rpy="0 -0 0" />
-            <inertia ixx="0.00285654" ixy="-8.74769e-06" ixz="0.000625247" iyy="0.004116" iyz="-1.03356e-05" izz="0.00180731" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.008 -5.54343e-12 -0.0553" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.008 -5.54343e-12 -0.0553" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000765393" ixy="4.337e-06" ixz="2.39e-07" iyy="0.000164578" iyz="1.9381e-05" izz="0.00069806" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000157143" ixy="1.278e-05" ixz="4.823e-06" iyy="0.000247995" iyz="-1.8188e-05" izz="0.000380535" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.000471076" ixy="2.059e-06" ixz="1.451e-06" iyy="0.000346478" iyz="1.545e-06" izz="0.000510315" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.000561583" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.000738049" iyz="-6.2e-08" izz="0.000294119" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.00099895" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.00445054" iyz="7.86e-07" izz="0.00420766" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="5.4421e-05" ixy="9e-09" ixz="0" iyy="9.331e-06" iyz="-1.7e-08" izz="5.4862e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.0001372" ixy="4.66e-07" ixz="3.65e-07" iyy="8.2927e-05" iyz="-2.0524e-05" izz="9.9274e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="4.84118e-05" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="5.79067e-05" iyz="-7.42844e-07" izz="6.7103e-05" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.00759107" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.00142302" iyz="3.63726e-05" izz="0.00755385" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="2.71051e-20" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="5.42101e-20" iyz="3.38813e-21" izz="1.35525e-20" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0.008 0 0.0553" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="1.35525e-20" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.84e-05" ixy="-5.02e-07" ixz="-3.99e-07" iyy="9.24e-06" iyz="-3.71e-07" izz="2.99e-05" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.016069 0.000613931 -0.0467154" rpy="0 -0 0" />
-            <inertia ixx="0.00285653" ixy="8.74769e-06" ixz="0.000625247" iyy="0.004116" iyz="1.03358e-05" izz="0.00180731" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.008 1.08717e-12 -0.0553" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.008 1.08717e-12 -0.0553" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000766" ixy="5.66e-06" ixz="1.4e-06" iyy="0.000164" iyz="1.82e-05" izz="0.000699" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000154" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.00025" iyz="1.76e-05" izz="0.000378" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="-5.42101e-20" ixy="0" ixz="0" iyy="-5.42101e-20" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="-6.2511e-19" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="-1.73472e-18" iyz="8.47033e-22" izz="-1.73472e-18" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.000123" ixy="2.1e-08" ixz="-1e-09" iyy="2.44e-05" iyz="4.22e-06" izz="0.000113" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.000137" ixy="-4.53e-07" ixz="2.03e-07" iyy="8.3e-05" iyz="2.07e-05" izz="9.93e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="3.44122e-05" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="4.33544e-05" iyz="-6.5128e-06" izz="6.49393e-05" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="4.07592e-18" iyz="-2.71051e-20" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 -5.66898e-35 -0.0143" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0.008 0 0.0553" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 0 -0.0143" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 -5.66898e-35 -0.0143" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0.008 0 0.0553" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 0 -0.0143" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-0.0263 -1.38778e-17 -0.0143" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-    <gazebo reference="l_arm_ft_sensor">
-        <sensor name="left_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_arm_ft_sensor">
-        <sensor name="right_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_leg_ft_sensor">
-        <sensor name="left_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_leg_ft_sensor">
-        <sensor name="right_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_foot_ft_sensor">
-        <sensor name="left_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_foot_ft_sensor">
-        <sensor name="right_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-</robot>
diff --git a/dh/generated/urdf/icubGazeboSim/icub.urdf b/dh/generated/urdf/icubGazeboSim/icub.urdf
deleted file mode 100644
index 0f3fa59..0000000
--- a/dh/generated/urdf/icubGazeboSim/icub.urdf
+++ /dev/null
@@ -1,1290 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.00063323" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.00068776" iyz="2.0817e-05" izz="0.000313897" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0 0 0.037" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.8389e-05" ixy="-5.15e-07" ixz="-4.08e-07" iyy="9.244e-06" iyz="-3.71e-07" izz="2.9968e-05" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.00167275" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.00320869" iyz="-6.62361e-07" izz="0.00208378" />
-        </inertial>
-        <visual>
-            <origin xyz="4.45647e-12 -5.54343e-12 -0.037" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="4.45647e-12 -5.54343e-12 -0.037" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000765393" ixy="4.337e-06" ixz="2.39e-07" iyy="0.000164578" iyz="1.9381e-05" izz="0.00069806" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000157143" ixy="1.278e-05" ixz="4.823e-06" iyy="0.000247995" iyz="-1.8188e-05" izz="0.000380535" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.000471076" ixy="2.059e-06" ixz="1.451e-06" iyy="0.000346478" iyz="1.545e-06" izz="0.000510315" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.000561583" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.000738049" iyz="-6.2e-08" izz="0.000294119" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.00099895" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.00445054" iyz="7.86e-07" izz="0.00420766" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="5.4421e-05" ixy="9e-09" ixz="0" iyy="9.331e-06" iyz="-1.7e-08" izz="5.4862e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.0001372" ixy="4.66e-07" ixz="3.65e-07" iyy="8.2927e-05" iyz="-2.0524e-05" izz="9.9274e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="4.84118e-05" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="5.79067e-05" iyz="-7.42844e-07" izz="6.7103e-05" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.00759107" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.00142302" iyz="3.63726e-05" izz="0.00755385" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="2.71051e-20" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="5.42101e-20" iyz="3.38813e-21" izz="1.35525e-20" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0 0 0.037" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.84e-05" ixy="-5.02e-07" ixz="-3.99e-07" iyy="9.24e-06" iyz="-3.71e-07" izz="2.99e-05" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.00167275" ixy="4.51893e-06" ixz="0.000567784" iyy="0.00320869" iyz="6.62535e-07" izz="0.00208378" />
-        </inertial>
-        <visual>
-            <origin xyz="4.45647e-12 1.08717e-12 -0.037" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="4.45647e-12 1.08717e-12 -0.037" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000766" ixy="5.66e-06" ixz="1.4e-06" iyy="0.000164" iyz="1.82e-05" izz="0.000699" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000154" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.00025" iyz="1.76e-05" izz="0.000378" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="-5.42101e-20" ixy="0" ixz="0" iyy="-5.42101e-20" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="-6.2511e-19" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="-1.73472e-18" iyz="8.47033e-22" izz="-1.73472e-18" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.000123" ixy="2.1e-08" ixz="-1e-09" iyy="2.44e-05" iyz="4.22e-06" izz="0.000113" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.000137" ixy="-4.53e-07" ixz="2.03e-07" iyy="8.3e-05" iyz="2.07e-05" izz="9.93e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="3.44122e-05" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="4.33544e-05" iyz="-6.5128e-06" izz="6.49393e-05" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="4.07592e-18" iyz="-2.71051e-20" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -2.65687e-65 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.037" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -2.65687e-65 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.037" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -9.86076e-35 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-    <gazebo reference="l_arm_ft_sensor">
-        <sensor name="left_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_arm_ft_sensor">
-        <sensor name="right_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_leg_ft_sensor">
-        <sensor name="left_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_leg_ft_sensor">
-        <sensor name="right_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_foot_ft_sensor">
-        <sensor name="left_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_foot_ft_sensor">
-        <sensor name="right_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-</robot>
diff --git a/dh/generated/urdf/icubGazeboSimNoFT/icub.urdf b/dh/generated/urdf/icubGazeboSimNoFT/icub.urdf
deleted file mode 100644
index 0f3fa59..0000000
--- a/dh/generated/urdf/icubGazeboSimNoFT/icub.urdf
+++ /dev/null
@@ -1,1290 +0,0 @@
-<robot name="iCub">
-    <link name="base_link" />
-    <link name="chest">
-        <inertial>
-            <mass value="4.81" />
-            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
-            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="chest_skin_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="codyco_balancing_world">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="head">
-        <inertial>
-            <mass value="3" />
-            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="imu_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 0.0172" rpy="0 -0 0" />
-            <inertia ixx="0.00063323" ixy="-7.081e-06" ixz="4.1421e-05" iyy="0.00068776" iyz="2.0817e-05" izz="0.000313897" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -5.54343e-12 4.45647e-12" rpy="1.5708 1.53106e-11 1.53105e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0 0 0.037" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.8389e-05" ixy="-5.15e-07" ixz="-4.08e-07" iyy="9.244e-06" iyz="-3.71e-07" izz="2.9968e-05" />
-        </inertial>
-    </link>
-    <link name="l_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.00167275" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.00320869" iyz="-6.62361e-07" izz="0.00208378" />
-        </inertial>
-        <visual>
-            <origin xyz="4.45647e-12 -5.54343e-12 -0.037" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="4.45647e-12 -5.54343e-12 -0.037" rpy="2.35619 1.5708 2.35619" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000765393" ixy="4.337e-06" ixz="2.39e-07" iyy="0.000164578" iyz="1.9381e-05" izz="0.00069806" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000157143" ixy="1.278e-05" ixz="4.823e-06" iyy="0.000247995" iyz="-1.8188e-05" izz="0.000380535" />
-        </inertial>
-        <visual>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0.000471076" ixy="2.059e-06" ixz="1.451e-06" iyy="0.000346478" iyz="1.545e-06" izz="0.000510315" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="0.000561583" ixy="-7.4e-08" ixz="1.0835e-05" iyy="0.000738049" iyz="-6.2e-08" izz="0.000294119" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 0.00211" rpy="0 -0 0" />
-            <inertia ixx="0.00099895" ixy="-0.000185699" ixz="-6.3147e-05" iyy="0.00445054" iyz="7.86e-07" izz="0.00420766" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
-            <inertia ixx="5.4421e-05" ixy="9e-09" ixz="0" iyy="9.331e-06" iyz="-1.7e-08" izz="5.4862e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.0001372" ixy="4.66e-07" ixz="3.65e-07" iyy="8.2927e-05" iyz="-2.0524e-05" izz="9.9274e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_shoulder_3">
-        <inertial>
-            <mass value="0.156157" />
-            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
-            <inertia ixx="4.84118e-05" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="5.79067e-05" iyz="-7.42844e-07" izz="6.7103e-05" />
-        </inertial>
-    </link>
-    <link name="l_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_arm">
-        <inertial>
-            <mass value="0.727843" />
-            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="l_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 -0.00039" rpy="0 -0 0" />
-            <inertia ixx="0.00759107" ixy="-6.726e-05" ixz="2.267e-06" iyy="0.00142302" iyz="3.63726e-05" izz="0.00755385" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="l_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="neck_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="neck_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_1">
-        <inertial>
-            <mass value="0.746" />
-            <origin xyz="-0.0054 0.00163 -0.0172" rpy="0 -0 0" />
-            <inertia ixx="2.71051e-20" ixy="-1.69407e-21" ixz="1.35525e-20" iyy="5.42101e-20" iyz="3.38813e-21" izz="1.35525e-20" />
-        </inertial>
-        <visual>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.11022e-16 -1.08717e-12 -4.45647e-12" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_ankle_2">
-        <inertial>
-            <mass value="0.2675" />
-            <origin xyz="0 0 0.037" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_elbow_1">
-        <inertial>
-            <mass value="0.074" />
-            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
-            <inertia ixx="2.84e-05" ixy="-5.02e-07" ixz="-3.99e-07" iyy="9.24e-06" iyz="-3.71e-07" izz="2.99e-05" />
-        </inertial>
-    </link>
-    <link name="r_foot">
-        <inertial>
-            <mass value="0.5935" />
-            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
-            <inertia ixx="0.00167275" ixy="4.51893e-06" ixz="0.000567784" iyy="0.00320869" iyz="6.62535e-07" izz="0.00208378" />
-        </inertial>
-        <visual>
-            <origin xyz="4.45647e-12 1.08717e-12 -0.037" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="4.45647e-12 1.08717e-12 -0.037" rpy="0.321762 1.5708 -2.81983" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_foot_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_forearm">
-        <inertial>
-            <mass value="0.525" />
-            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
-            <inertia ixx="0.000766" ixy="5.66e-06" ixz="1.4e-06" iyy="0.000164" iyz="1.82e-05" izz="0.000699" />
-        </inertial>
-        <visual>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_forearm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_gripper">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hand">
-        <inertial>
-            <mass value="0.213" />
-            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
-            <inertia ixx="0.000154" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.00025" iyz="1.76e-05" izz="0.000378" />
-        </inertial>
-        <visual>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hand_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_hip_1">
-        <inertial>
-            <mass value="0.754" />
-            <origin xyz="0 -0.0782 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_2">
-        <inertial>
-            <mass value="0.526" />
-            <origin xyz="0 0 0.03045" rpy="0 -0 0" />
-            <inertia ixx="-5.42101e-20" ixy="0" ixz="0" iyy="-5.42101e-20" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_hip_3">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_lower_leg">
-        <inertial>
-            <mass value="1.264" />
-            <origin xyz="-0.1071 0.00182 -0.00211" rpy="0 -0 0" />
-            <inertia ixx="-6.2511e-19" ixy="5.42101e-20" ixz="-5.42101e-20" iyy="-1.73472e-18" iyz="8.47033e-22" izz="-1.73472e-18" />
-        </inertial>
-        <visual>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="5.55112e-17 2.28232e-12 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_1">
-        <inertial>
-            <mass value="0.189" />
-            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
-            <inertia ixx="0.000123" ixy="2.1e-08" ixz="-1e-09" iyy="2.44e-05" iyz="4.22e-06" izz="0.000113" />
-        </inertial>
-        <visual>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_2">
-        <inertial>
-            <mass value="0.179" />
-            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
-            <inertia ixx="0.000137" ixy="-4.53e-07" ixz="2.03e-07" iyy="8.3e-05" iyz="2.07e-05" izz="9.93e-05" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_shoulder_3">
-        <inertial>
-            <mass value="0.155" />
-            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
-            <inertia ixx="3.44122e-05" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="4.33544e-05" iyz="-6.5128e-06" izz="6.49393e-05" />
-        </inertial>
-    </link>
-    <link name="r_sole">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_arm">
-        <inertial>
-            <mass value="0.729" />
-            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
-            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
-        </inertial>
-        <visual>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_upper_arm_dh_frame">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="r_upper_leg">
-        <inertial>
-            <mass value="2.175" />
-            <origin xyz="0.00144 -0.15943 0.00039" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="4.07592e-18" iyz="-2.71051e-20" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="r_wrist_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="root_link">
-        <inertial>
-            <mass value="4.72" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
-        </inertial>
-    </link>
-    <link name="torso_1">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-            <material name="material.metal">
-                <texture />
-                <color rgba="0 0 0 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
-            <geometry>
-                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
-            </geometry>
-        </collision>
-    </link>
-    <link name="torso_2">
-        <inertial>
-            <mass value="0" />
-            <origin xyz="0 0 0" rpy="0 -0 0" />
-            <inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
-        </inertial>
-    </link>
-    <joint name="torso_yaw" type="revolute">
-        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="torso_2" />
-        <child link="chest" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
-    </joint>
-    <joint name="chest_skin_frame_fixed_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="chest_skin_frame" />
-    </joint>
-    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -2.65687e-65 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="codyco_balancing_world" />
-    </joint>
-    <joint name="neck_yaw" type="revolute">
-        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_2" />
-        <child link="head" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
-    </joint>
-    <joint name="imu_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="head" />
-        <child link="imu_frame" />
-    </joint>
-    <joint name="l_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="-1.5708 0 0" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="l_lower_leg" />
-        <child link="l_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="l_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
-        <axis xyz="1 0 0" />
-        <parent link="l_ankle_1" />
-        <child link="l_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="l_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_upper_arm" />
-        <child link="l_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="l_foot_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.037" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_ankle_2" />
-        <child link="l_foot" />
-    </joint>
-    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_foot_dh_frame" />
-    </joint>
-    <joint name="l_wrist_prosup" type="revolute">
-        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_elbow_1" />
-        <child link="l_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_forearm" />
-        <child link="l_forearm_dh_frame" />
-    </joint>
-    <joint name="l_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_gripper" />
-    </joint>
-    <joint name="l_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 1" />
-        <parent link="l_wrist_1" />
-        <child link="l_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hand" />
-        <child link="l_hand_dh_frame" />
-    </joint>
-    <joint name="l_hip_pitch" type="revolute">
-        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="l_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="l_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
-        <axis xyz="1 0 -6.12323e-17" />
-        <parent link="l_hip_1" />
-        <child link="l_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="l_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_hip_2" />
-        <child link="l_hip_3" />
-    </joint>
-    <joint name="l_knee" type="revolute">
-        <origin xyz="0 -0.2236 -1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="l_upper_leg" />
-        <child link="l_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="l_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
-        <axis xyz="0 -1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="l_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="l_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_shoulder_1" />
-        <child link="l_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="l_shoulder_yaw" type="revolute">
-        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
-        <axis xyz="0 6.12323e-17 1" />
-        <parent link="l_shoulder_2" />
-        <child link="l_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="l_sole_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -2.65687e-65 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_foot" />
-        <child link="l_sole" />
-    </joint>
-    <joint name="l_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_shoulder_3" />
-        <child link="l_upper_arm" />
-    </joint>
-    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="l_upper_arm" />
-        <child link="l_upper_arm_dh_frame" />
-    </joint>
-    <joint name="l_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_hip_3" />
-        <child link="l_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="l_wrist_pitch" type="revolute">
-        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="l_forearm" />
-        <child link="l_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="neck_pitch" type="revolute">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="chest" />
-        <child link="neck_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
-    </joint>
-    <joint name="neck_roll" type="revolute">
-        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="neck_1" />
-        <child link="neck_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
-    </joint>
-    <joint name="r_ankle_pitch" type="revolute">
-        <origin xyz="-0.213 0 0" rpy="1.5708 -0 0" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_lower_leg" />
-        <child link="r_ankle_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
-    </joint>
-    <joint name="r_ankle_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
-        <axis xyz="-1 1.22465e-16 0" />
-        <parent link="r_ankle_1" />
-        <child link="r_ankle_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
-    </joint>
-    <joint name="r_elbow" type="revolute">
-        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_upper_arm" />
-        <child link="r_elbow_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
-    </joint>
-    <joint name="r_foot_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.037" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_ankle_2" />
-        <child link="r_foot" />
-    </joint>
-    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_foot_dh_frame" />
-    </joint>
-    <joint name="r_wrist_prosup" type="revolute">
-        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_elbow_1" />
-        <child link="r_forearm" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
-    </joint>
-    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_forearm" />
-        <child link="r_forearm_dh_frame" />
-    </joint>
-    <joint name="r_gripper_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_gripper" />
-    </joint>
-    <joint name="r_wrist_yaw" type="revolute">
-        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
-        <axis xyz="0 0 1" />
-        <parent link="r_wrist_1" />
-        <child link="r_hand" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
-    </joint>
-    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hand" />
-        <child link="r_hand_dh_frame" />
-    </joint>
-    <joint name="r_hip_pitch" type="revolute">
-        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="r_hip_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
-    </joint>
-    <joint name="r_hip_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
-        <axis xyz="-1 0 6.12323e-17" />
-        <parent link="r_hip_1" />
-        <child link="r_hip_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
-    </joint>
-    <joint name="r_leg_ft_sensor" type="fixed">
-        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_hip_2" />
-        <child link="r_hip_3" />
-    </joint>
-    <joint name="r_knee" type="revolute">
-        <origin xyz="0 -0.2236 1.36916e-17" rpy="3.14159 -1.2326e-32 1.5708" />
-        <axis xyz="0 1.22465e-16 -1" />
-        <parent link="r_upper_leg" />
-        <child link="r_lower_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
-    </joint>
-    <joint name="r_shoulder_pitch" type="revolute">
-        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
-        <axis xyz="0 1 5.55112e-17" />
-        <parent link="chest" />
-        <child link="r_shoulder_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
-    </joint>
-    <joint name="r_shoulder_roll" type="revolute">
-        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_shoulder_1" />
-        <child link="r_shoulder_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
-    </joint>
-    <joint name="r_shoulder_yaw" type="revolute">
-        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
-        <axis xyz="0 6.12323e-17 -1" />
-        <parent link="r_shoulder_2" />
-        <child link="r_shoulder_3" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
-    </joint>
-    <joint name="r_sole_fixed_joint" type="fixed">
-        <origin xyz="-1.24246e-32 -9.86076e-35 0.004" rpy="3.14159 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_foot" />
-        <child link="r_sole" />
-    </joint>
-    <joint name="r_arm_ft_sensor" type="fixed">
-        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="r_shoulder_3" />
-        <child link="r_upper_arm" />
-    </joint>
-    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
-        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
-        <axis xyz="0 0 0" />
-        <parent link="r_upper_arm" />
-        <child link="r_upper_arm_dh_frame" />
-    </joint>
-    <joint name="r_hip_yaw" type="revolute">
-        <origin xyz="0 0 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
-        <axis xyz="0 -1 6.12323e-17" />
-        <parent link="r_hip_3" />
-        <child link="r_upper_leg" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
-    </joint>
-    <joint name="r_wrist_pitch" type="revolute">
-        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="r_forearm" />
-        <child link="r_wrist_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
-    </joint>
-    <joint name="base_fixed_joint" type="fixed">
-        <origin xyz="0 0 0" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="base_link" />
-        <child link="root_link" />
-    </joint>
-    <joint name="torso_joint" type="fixed">
-        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
-        <axis xyz="0 0 0" />
-        <parent link="chest" />
-        <child link="torso" />
-    </joint>
-    <joint name="torso_pitch" type="revolute">
-        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="root_link" />
-        <child link="torso_1" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
-    </joint>
-    <joint name="torso_roll" type="revolute">
-        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
-        <axis xyz="0 1 6.12323e-17" />
-        <parent link="torso_1" />
-        <child link="torso_2" />
-        <dynamics damping="1" friction="0" />
-        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
-    </joint>
-    <gazebo reference="l_arm_ft_sensor">
-        <sensor name="left_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_arm_ft_sensor">
-        <sensor name="right_arm_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_leg_ft_sensor">
-        <sensor name="left_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_leg_ft_sensor">
-        <sensor name="right_leg_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="l_foot_ft_sensor">
-        <sensor name="left_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-    <gazebo reference="r_foot_ft_sensor">
-        <sensor name="right_foot_ft" type="force_torque">
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-            <update_rate>100</update_rate>
-            <always_on>1</always_on>
-        </sensor>
-    </gazebo>
-</robot>
diff --git a/dh/generator/CMakeLists.txt b/dh/generator/CMakeLists.txt
deleted file mode 100644
index cfc4b74..0000000
--- a/dh/generator/CMakeLists.txt
+++ /dev/null
@@ -1,44 +0,0 @@
-# Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia - iCub Facility
-# CopyPolicy: Released under the terms of the GNU GPL v2.0.
-
-find_package(YCM REQUIRED)
-find_package(YARP 3.0 REQUIRED COMPONENTS OS dev sig math gsl)
-find_package(ICUB REQUIRED)
-
-find_package(urdfdom REQUIRED)
-find_package(orocos_kdl REQUIRED)
-find_package(Eigen3 REQUIRED)
-find_package(TinyXML REQUIRED)
-
-find_package(SDFormat REQUIRED)
-find_package(iDynTree 0.11 REQUIRED)
-
-set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ICUB_LINK_FLAGS}")
-# Note: SDFormat_CXX_FLAGS contains explictly the --std=c++11 flag 
-# if used with iDynTree >= 0.11, that uses C++14 in its header, this 
-# flag will create compilation errors. As the compilation units will 
-# be compiled as C++14 as they are linking to iDynTree, we can safely 
-# remove comment out this line
-# See https://github.com/robotology/icub-model-generator/issues/105
-# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${SDFormat_CXX_FLAGS}")
-
-# add YARP include directories
-include_directories(${urdfdom_INCLUDE_DIR}
-                    ${orocos_kdl_INCLUDE_DIRS}
-                    ${EIGEN3_INCLUDE_DIR} ${YARP_INCLUDE_DIR} ${iDynTree_INCLUDE_DIRS}
-                    ${iDyn_INCLUDE_DIRS}
-                    ${YARP_INCLUDE_DIRS}
-                    ${skinDynLib_INCLUDE_DIRS} ${SDFormat_INCLUDE_DIRS}
-                    ${urdfdom_INCLUDE_DIRS})
-
-add_library(urdf_from_dh_utils STATIC urdf_utils.h
-                                      urdf_utils.cpp
-                                      urdf_sdf_from_dh_utils.h
-                                      urdf_sdf_from_dh_utils.cpp)
-
-target_link_libraries(urdf_from_dh_utils PUBLIC ${SDFormat_LIBRARIES} ${iDynTree_LIBRARIES} ${YARP_LIBRARIES} ${ICUB_LIBRARIES} ${kdl_codyco_LIBRARIES} ${orocos_kdl_LIBRARIES} ${urdfdom_LIBRARIES} ${iDynTree_LIBRARIES} ${TinyXML_LIBRARIES})
-add_executable(icub_urdf_sdf_from_dh_generator icub_urdf_sdf_from_dh_generator.cpp)
-target_link_libraries(icub_urdf_sdf_from_dh_generator urdf_from_dh_utils)
-
-
-install(TARGETS icub_urdf_sdf_from_dh_generator DESTINATION bin)
diff --git a/dh/generator/CMakeLists2.txt b/dh/generator/CMakeLists2.txt
deleted file mode 100644
index 179abd9..0000000
--- a/dh/generator/CMakeLists2.txt
+++ /dev/null
@@ -1,48 +0,0 @@
-# Copyright (C) 2010 RobotCub Consortium
-# CopyPolicy: Released under the terms of the GNU GPL v2.0.
-
-
-SET(PROJECTNAME icub_urdf)
-
-PROJECT(${PROJECTNAME})
-
-# YARP needs CMake 2.6 or greater
-cmake_minimum_required(VERSION 2.6)
-
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
-
-# find YARP
-find_package(YARP REQUIRED)
-find_package(ICUB REQUIRED)
-find_package(Eigen3 REQUIRED)
-find_package(urdfdom REQUIRED)
-find_package(orocos_kdl REQUIRED)
-find_package(kdl_format_io REQUIRED)
-find_package(TinyXML REQUIRED)
-
-# add YARP include directories
-include_directories(${YARP_INCLUDE_DIRS} ${ICUB_INCLUDE_DIRS} ${urdfdom_INCLUDE_DIR} ${kdl_urdf_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
-
-
-#FILE(GLOB folder_source normal_excited_regressor_test.cpp ../lib/inertia_common.cpp)
-#FILE(GLOB folder_header ../lib/inertia_common.h)
-
-#SOURCE_GROUP("Source Files" FILES ${folder_source})
-#SOURCE_GROUP("Header Files" FILES ${folder_header})
-
-INCLUDE_DIRECTORIES(${iDynTree_INCLUDE_DIRS}
-                    ${iDyn_INCLUDE_DIRS}
-                    ${YARP_INCLUDE_DIRS}
-                    ${skinDynLib_INCLUDE_DIRS})
-
-#ADD_EXECUTABLE(${PROJECTNAME} ${folder_source} ${folder_header})
-
-#TARGET_LINK_LIBRARIES(${PROJECTNAME} iDyn ${YARP_LIBRARIES} skinDynLib)
-
-#INSTALL(TARGETS ${PROJECTNAME} DESTINATION bin)
-
-add_executable(icub_urdf_from_iDyn icub_urdf_from_iDyn.cpp)
-
-target_link_libraries(icub_urdf_from_iDyn ${YARP_LIBRARIES} ${ICUB_LIBRARIES} iDynTree ${kdl_codyco_LIBRARIES} ${orocos_kdl_LIBRARIES} ${urdfdom_LIBRARIES} ${kdl_format_io_LIBRARIES} ${TinyXML_LIBRARIES})
-
-INSTALL(TARGETS icub_urdf_from_iDyn DESTINATION bin)
diff --git a/dh/generator/README b/dh/generator/README
deleted file mode 100644
index 6397d88..0000000
--- a/dh/generator/README
+++ /dev/null
@@ -1,14 +0,0 @@
-Collection of utilities for manipulating URDF files.
-
-urdf_get_meshes
-===============
-Utility for getting the meshes from a URDF file to another URDF file.
-This utility works on two hypothesis: that the base frame of the two urdf files are coincident, and 
-the link for which the mesh are imported have the same name in the original file).
-
-Usage:
-
-   urdf_get_meshes urdf_meshes.xml urdf_input.xml urdf_output.xml
-
-
-In which the urdf_output.xml is the same model of urdf_input.xml, but with the meshes of urdf_meshes.xml
diff --git a/dh/generator/cmake/FindEigen3.cmake b/dh/generator/cmake/FindEigen3.cmake
deleted file mode 100644
index 9c546a0..0000000
--- a/dh/generator/cmake/FindEigen3.cmake
+++ /dev/null
@@ -1,81 +0,0 @@
-# - Try to find Eigen3 lib
-#
-# This module supports requiring a minimum version, e.g. you can do
-#   find_package(Eigen3 3.1.2)
-# to require version 3.1.2 or newer of Eigen3.
-#
-# Once done this will define
-#
-#  EIGEN3_FOUND - system has eigen lib with correct version
-#  EIGEN3_INCLUDE_DIR - the eigen include directory
-#  EIGEN3_VERSION - eigen version
-
-# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
-# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
-# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
-# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
-
-if(NOT Eigen3_FIND_VERSION)
-  if(NOT Eigen3_FIND_VERSION_MAJOR)
-    set(Eigen3_FIND_VERSION_MAJOR 2)
-  endif(NOT Eigen3_FIND_VERSION_MAJOR)
-  if(NOT Eigen3_FIND_VERSION_MINOR)
-    set(Eigen3_FIND_VERSION_MINOR 91)
-  endif(NOT Eigen3_FIND_VERSION_MINOR)
-  if(NOT Eigen3_FIND_VERSION_PATCH)
-    set(Eigen3_FIND_VERSION_PATCH 0)
-  endif(NOT Eigen3_FIND_VERSION_PATCH)
-
-  set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
-endif(NOT Eigen3_FIND_VERSION)
-
-macro(_eigen3_check_version)
-  file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
-
-  string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
-  set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
-  string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
-  set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
-
-  set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
-  if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK FALSE)
-  else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-    set(EIGEN3_VERSION_OK TRUE)
-  endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
-
-  if(NOT EIGEN3_VERSION_OK)
-
-    message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
-                   "but at least version ${Eigen3_FIND_VERSION} is required")
-  endif(NOT EIGEN3_VERSION_OK)
-endmacro(_eigen3_check_version)
-
-if (EIGEN3_INCLUDE_DIR)
-
-  # in cache already
-  _eigen3_check_version()
-  set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
-
-else (EIGEN3_INCLUDE_DIR)
-
-  find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
-      PATHS
-      ${CMAKE_INSTALL_PREFIX}/include
-      ${KDE4_INCLUDE_DIR}
-      PATH_SUFFIXES eigen3 eigen
-    )
-
-  if(EIGEN3_INCLUDE_DIR)
-    _eigen3_check_version()
-  endif(EIGEN3_INCLUDE_DIR)
-
-  include(FindPackageHandleStandardArgs)
-  find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
-
-  mark_as_advanced(EIGEN3_INCLUDE_DIR)
-
-endif(EIGEN3_INCLUDE_DIR)
-
diff --git a/dh/generator/cmake/FindTinyXML.cmake b/dh/generator/cmake/FindTinyXML.cmake
deleted file mode 100644
index f21c4cb..0000000
--- a/dh/generator/cmake/FindTinyXML.cmake
+++ /dev/null
@@ -1,40 +0,0 @@
-# Try to find the TinyXML library
-# TinyXML_FOUND - system has TinyXML
-# TinyXML_INCLUDE_DIRS - TinyXML include directory
-# TinyXML_LIBRARY_DIRS - TinyXML library directory
-# TinyXML_LIBRARIES - TinyXML libraries
-
-# Copyright (C) 2012  iCub Facility, Istituto Italiano di Tecnologia
-# Author: Daniele E. Domenichelli <daniele.domenichelli@iit.it>
-#
-# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
-
-
-if(NOT WIN32)
-    find_package(PkgConfig)
-    if(PKG_CONFIG_FOUND)
-        if(TinyXML_FIND_VERSION)
-            if(TinyXML_FIND_VERSION_EXACT)
-                pkg_check_modules(PC_TINYXML QUIET tinyxml=${TinyXML_FIND_VERSION})
-            else(TinyXML_FIND_VERSION_EXACT)
-                pkg_check_modules(PC_TINYXML QUIET tinyxml>=${TinyXML_FIND_VERSION})
-            endif(TinyXML_FIND_VERSION_EXACT)
-        else(TinyXML_FIND_VERSION)
-            pkg_check_modules(PC_TINYXML QUIET tinyxml)
-        endif(TinyXML_FIND_VERSION)
-    endif(PKG_CONFIG_FOUND)
-endif(NOT WIN32)
-
-set(TinyXML_INCLUDE_DIRS ${PC_TINYXML_INCLUDE_DIRS} CACHE PATH "TinyXML include directory" FORCE)
-set(TinyXML_LIBRARY_DIRS ${PC_TINYXML_LIBRARY_DIRS} CACHE PATH "TinyXML library directory" FORCE)
-set(TinyXML_LIBRARIES ${PC_TINYXML_LIBRARIES} CACHE STRING "TinyXML libraries" FORCE)
-
-include(FindPackageHandleStandardArgs)
-find_package_handle_standard_args(TinyXML
-                                  DEFAULT_MSG
-                                  TinyXML_LIBRARIES
-)
-
-set(TinyXML_FOUND ${TINYXML_FOUND})
-
-mark_as_advanced(TinyXML_INCLUDE_DIRS TinyXML_LIBRARY_DIRS TinyXML_LIBRARIES)
diff --git a/dh/generator/deprecated_icub_urdf_sdf_generator.cpp b/dh/generator/deprecated_icub_urdf_sdf_generator.cpp
deleted file mode 100644
index 90840ca..0000000
--- a/dh/generator/deprecated_icub_urdf_sdf_generator.cpp
+++ /dev/null
@@ -1,112 +0,0 @@
-/**
- * Copyright  (C)  2014 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#include "urdf_sdf_from_dh_utils.h"
-
-void printTree(urdf::LinkSharedPtr link,int level = 0)
-{
-  level+=2;
-  int count = 0;
-  for (std::vector< urdf::LinkSharedPtr >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
-  {
-    if (*child)
-    {
-      for(int j=0;j<level;j++) std::cout << "  "; //indent
-      std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << " with joint: " << (*child)->parent_joint->name << std::endl;
-      // first grandchild
-      printTree(*child,level);
-    }
-    else
-    {
-      for(int j=0;j<level;j++) std::cout << " "; //indent
-      std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
-    }
-  }
-
-}
-
-void printJoints(boost::shared_ptr<urdf::ModelInterface> urdf)
-{
-    std::cout << "printing " << urdf->joints_.size() << " size" << std::endl;
-    for(std::map<std::string,boost::shared_ptr<urdf::Joint> >::const_iterator it = urdf->joints_.begin(); it != urdf->joints_.end(); it++ ) {
-        std::cout << it->first << " : " << it->second->name << std::endl;
-    }
-}
-
-int main(int argc, char* argv[])
-{
-    bool status = true;
-
-    yarp::os::Property opt;
-
-    opt.fromCommand(argc,argv);
-
-    if( opt.check("help") || !opt.check("output_directory") || !opt.check("data_directory") ) {
-        std::cerr << "Utility for generating URDF and SDF models for iCub robots" << std::endl;
-        std::cerr << "Creating one URDF with data from CAD, and a SDF directory with parameters modified to work in Gazebo (with a URDF that matches the modified simulated model)" << std::endl;
-        std::cerr << "Usage: \t icub_urdf_sdf_generator --data_directory ./data/ --output_directory ./icub-models/" << std::endl;
-        std::cerr << "Additional options: --min_mass and --min_inertia are used to select the minimum mass and minimum inertia to a give to a link in Gazebo model (default: mass 0.1, inertia: 0.01)" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    std::string output_directory = opt.find("output_directory").asString();
-    std::string data_directory = opt.find("data_directory").asString();
-
-    if( *(output_directory.rbegin()) != '/' ) {
-        output_directory = output_directory + "/";
-    }
-
-    if( *(data_directory.rbegin()) != '/' ) {
-        data_directory = data_directory + "/";
-    }
-
-    double mass_epsilon = 0.1;
-    double inertia_epsilon = 0.01;
-
-    if( opt.check("min_mass" )  ) mass_epsilon = opt.find("min_mass").asDouble();
-    if( opt.check("min_inertia" )  ) inertia_epsilon = opt.find("min_inertia").asDouble();
-
-    //Generate model database
-    std::vector<std::string> robot_names;
-
-    //Generating model for black iCub
-    //                          robot_name     directory    head   legs   feet    Paris02  gazeboSim
-    bool simple_meshes = false;
-    if( !generate_iCub_model("iCubGenova01",output_directory, 2    , 2    , 2   , false, false, simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
-    robot_names.push_back("iCubGenova01");
-
-    //Generating model for red iCub
-    if( !generate_iCub_model("iCubGenova03",output_directory, 2    , 1    , 2   , false, false, simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
-    robot_names.push_back("iCubGenova03");
-
-    if( !generate_iCub_model("iCubParis01",output_directory, 1    , 1    , 2   , false, false, simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
-    robot_names.push_back("iCubParis01");
-
-    //Generating model for red iCub
-    if( !generate_iCub_model("iCubParis02",output_directory, 2    , 1    , 2   , true, false,  simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
-    robot_names.push_back("iCubParis02");
-
-    //Generating model for Darmastad iCub
-    if( !generate_iCub_model("iCubDarmstadt01",output_directory, 2    , 2    , 2  , false, false,  simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
-    robot_names.push_back("iCubDarmstad01");
-
-    //Generating model for icubGazeboSim
-    if( !generate_iCub_model("icubGazeboSim",output_directory, 2    , 1    , 2   , false, true, simple_meshes , data_directory,mass_epsilon,inertia_epsilon) ) return EXIT_FAILURE;
-    robot_names.push_back("icubGazeboSim");
-
-    //Generating model for icubGazeboSim
-    if( !generate_iCub_model("icubGazeboSimNoFT",output_directory, 2    , 1    , 2   , false, true, simple_meshes , data_directory,mass_epsilon,inertia_epsilon,true) ) return EXIT_FAILURE;
-    robot_names.push_back("icubGazeboSimNoFT");
-
-
-    std::cerr << "iCub model files successfully created" << std::endl;
-
-    std::cerr << "Generating gazebo database" << std::endl;
-    generate_gazebo_database(robot_names,output_directory);
-
-    return EXIT_SUCCESS;
-}
-
diff --git a/dh/generator/icub_dh_to_urdf_sdf_utils.cpp b/dh/generator/icub_dh_to_urdf_sdf_utils.cpp
deleted file mode 100644
index 08f8b83..0000000
--- a/dh/generator/icub_dh_to_urdf_sdf_utils.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-/**
- * Copyright  (C)  2013 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#include <iCub/iDynTree/idyn2kdl_icub.h>
-
-#include <iostream>
-#include <cstdlib>
-
-#include <iCub/iDyn/iDyn.h>
-#include <iCub/iDyn/iDynBody.h>
-
-#include <kdl/tree.hpp>
-
-#include <iDynTree/ModelIO/impl/urdf_export.hpp>
-
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <tinyxml.h>
-
-#include <boost/function.hpp>
-
-#include <yarp/os/Property.h>
-
-#define NAME "icub_urdf_from_iDyn"
-
-using namespace urdf;
-
-void printTree(boost::shared_ptr<const Link> link,int level = 0)
-{
-  level+=2;
-  int count = 0;
-  for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
-  {
-    if (*child)
-    {
-      for(int j=0;j<level;j++) std::cout << "  "; //indent
-      std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << " FrameToTip " << urdf_export_helpers::values2str((*child)->parent_joint->parent_to_joint_origin_transform.position) << std::endl;
-      // first grandchild
-      printTree(*child,level);
-    }
-    else
-    {
-      for(int j=0;j<level;j++) std::cout << " "; //indent
-      std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
-    }
-  }
-
-}
-
-
-int main(int argc, char* argv[])
-{
-    bool status = true;
-
-    yarp::os::Property opt;
-
-    opt.fromCommand(argc,argv);
-
-    if( opt.check("help") || !opt.check("output") ) {
-        std::cerr << "Usage: \t icub_urdf_from_iDyn --headV2 --legsV2 --output output.xml" << std::endl;
-        std::cerr << "Default values: head and legs v2" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    std::string output_file = opt.find("output").asString().c_str();
-
-    iCub::iDyn::version_tag icub_type;
-
-    icub_type.head_version = 2;
-    icub_type.legs_version = 2;
-
-    if( opt.check("headV2") ) icub_type.head_version = 2;
-    if( opt.check("legsV2") ) icub_type.legs_version = 2;
-    if( opt.check("headV1") ) icub_type.head_version = 1;
-    if( opt.check("legsV1") ) icub_type.legs_version = 1;
-
-
-    iCub::iDyn::iCubWholeBody icub_idyn(icub_type);
-
-    KDL::Tree icub_kdl;
-
-    KDL::JntArray dummy1,dummy2;
-
-    if( ! toKDL(icub_idyn,icub_type,icub_kdl,dummy1,dummy2,iCub::iDynTree::SKINDYNLIB_SERIALIZATION,false,true) ) {
-        std::cerr << "Fatal error in iDyn - KDL conversion" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    boost::shared_ptr<urdf::ModelInterface> icub_ptr(new urdf::ModelInterface);
-
-    //std::cout << "iCub KDL::Tree: " << std::endl;
-    //std::cout << icub_kdl << std::endl;
-
-
-    if( ! iDynTree::treeToUrdfModel(icub_kdl,"test_icub",*icub_ptr) ) {
-        std::cerr << "Fatal error in KDL - URDF conversion" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    boost::shared_ptr<const Link> root_link=icub_ptr->getRoot();
-    printTree(root_link);
-
-    TiXmlDocument*  xml_doc =  exportURDF(icub_ptr);
-    if( ! xml_doc->SaveFile(output_file) ) {
-        std::cerr << "Fatal error in URDF xml saving" << std::endl;
-    }
-
-    std::cerr << "URDF file successfully created" << std::endl;
-
-    return EXIT_SUCCESS;
-}
diff --git a/dh/generator/icub_urdf_from_iDyn.cpp b/dh/generator/icub_urdf_from_iDyn.cpp
deleted file mode 100644
index c6511c7..0000000
--- a/dh/generator/icub_urdf_from_iDyn.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-/**
- * Copyright  (C)  2013 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#include <iCub/iDynTree/idyn2kdl_icub.h>
-
-#include <iostream>
-#include <cstdlib>
-
-#include <iCub/iDyn/iDyn.h>
-#include <iCub/iDyn/iDynBody.h>
-
-#include <kdl/tree.hpp>
-
-#include <iDynTree/ModelIO/impl/urdf_export.hpp>
-
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <tinyxml.h>
-
-#include <boost/function.hpp>
-
-#include <yarp/os/Property.h>
-
-#define NAME "icub_urdf_from_iDyn"
-
-using namespace urdf;
-
-void printTree(boost::shared_ptr<const Link> link,int level = 0)
-{
-  level+=2;
-  int count = 0;
-  for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
-  {
-    if (*child)
-    {
-      for(int j=0;j<level;j++) std::cout << "  "; //indent
-      std::cout << "child(" << (count++)+1 << "):  " << (*child)->name  << " FrameToTip " << urdf_export_helpers::values2str((*child)->parent_joint->parent_to_joint_origin_transform.position) << std::endl;
-      // first grandchild
-      printTree(*child,level);
-    }
-    else
-    {
-      for(int j=0;j<level;j++) std::cout << " "; //indent
-      std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
-    }
-  }
-
-}
-
-
-int main(int argc, char* argv[])
-{
-    bool status = true;
-
-    yarp::os::Property opt;
-
-    opt.fromCommand(argc,argv);
-
-    if( opt.check("help") || !opt.check("output") ) {
-        std::cerr << "Usage: \t icub_urdf_from_iDyn --headV2 --legsV2 --output output.xml" << std::endl;
-        std::cerr << "Default values: head and legs v2" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    std::string output_file = opt.find("output").asString().c_str();
-
-    iCub::iDyn::version_tag icub_type;
-
-    icub_type.head_version = 2;
-    icub_type.legs_version = 2;
-
-    if( opt.check("headV2") ) icub_type.head_version = 2;
-    if( opt.check("legsV2") ) icub_type.legs_version = 2;
-    if( opt.check("headV1") ) icub_type.head_version = 1;
-    if( opt.check("legsV1") ) icub_type.legs_version = 1;
-
-
-    iCub::iDyn::iCubWholeBody icub_idyn(icub_type);
-
-    KDL::Tree icub_kdl;
-
-    KDL::JntArray dummy1,dummy2;
-
-    if( ! toKDL(icub_idyn,icub_type,icub_kdl,dummy1,dummy2,iCub::iDynTree::SKINDYNLIB_SERIALIZATION,false,true) ) {
-        std::cerr << "Fatal error in iDyn - KDL conversion" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    boost::shared_ptr<urdf::ModelInterface> icub_ptr(new urdf::ModelInterface);
-
-    //std::cout << "iCub KDL::Tree: " << std::endl;
-    //std::cout << icub_kdl << std::endl;
-
-    if( ! iDynTree::treeToUrdfModel(icub_kdl,"test_icub",*icub_ptr) ) {
-        std::cerr << "Fatal error in KDL - URDF conversion" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    boost::shared_ptr<const Link> root_link=icub_ptr->getRoot();
-    printTree(root_link);
-
-    TiXmlDocument*  xml_doc =  exportURDF(icub_ptr);
-    if( ! xml_doc->SaveFile(output_file) ) {
-        std::cerr << "Fatal error in URDF xml saving" << std::endl;
-    }
-
-    std::cerr << "URDF file successfully created" << std::endl;
-
-    return EXIT_SUCCESS;
-}
diff --git a/dh/generator/icub_urdf_sdf_from_dh_generator.cpp b/dh/generator/icub_urdf_sdf_from_dh_generator.cpp
deleted file mode 100644
index 52821e7..0000000
--- a/dh/generator/icub_urdf_sdf_from_dh_generator.cpp
+++ /dev/null
@@ -1,135 +0,0 @@
-/**
- * Copyright  (C)  2014 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#include "urdf_sdf_from_dh_utils.h"
-
-int main(int argc, char* argv[])
-{
-    bool status = true;
-
-    yarp::os::Property opt;
-
-    opt.fromCommand(argc,argv);
-
-    if( opt.check("help") ||
-        !opt.check("data_directory") ||
-        !opt.check("YARP_ROBOT_NAME") ||
-        !opt.check("outputfile") )
-    {
-        std::cerr << "Utility for generating URDF and SDF models for iCub robots from DH parameters hardcoded in iKin/iDyn/iDynTree libraries." << std::endl;
-        std::cerr << "Creating one URDF with data from CAD, and a SDF directory with parameters modified to work in Gazebo (with a URDF that matches the modified simulated model)" << std::endl;
-        std::cerr << "Usage: \t icub_urdf_sdf_from_dh_generator --data_directory ./data/ --output_directory ./icub-models/" << std::endl;
-        std::cerr << "Additional options: --min_mass and --min_inertia are used to select the minimum mass and minimum inertia to a give to a link in Gazebo model (default: mass 0.1, inertia: 0.01)" << std::endl;
-        std::cerr << "\t\t --headV1/headV2 : specify the version of the head part" << std::endl;
-        std::cerr << "\t\t --armsV1/armsV2 : specify the version of the arm parts" << std::endl;
-        std::cerr << "\t\t --legsV1/legsV2 : specify the version of the leg parts" << std::endl;
-        std::cerr << "\t\t --feetV1/feetV2 : specify the version of the leg parts" << std::endl;
-        std::cerr << "\t\t --YARP_ROBOT_NAME : specify the YARP_ROBOT_NAME of the created robot." << std::endl;
-        std::cerr << "\t\t --iCubParis02 : specify if the generated model is iCubParis02" << std::endl;
-        std::cerr << "\t\t --outputfile  : specify the path of the generated file" << std::endl;
-
-        return EXIT_FAILURE;
-    }
-
-    std::string output_file = opt.find("outputfile").asString();
-    std::string data_directory = opt.find("data_directory").asString();
-
-    if( *(data_directory.rbegin()) != '/' ) {
-        data_directory = data_directory + "/";
-    }
-
-    double mass_epsilon = 0.1;
-    double inertia_epsilon = 0.01;
-
-    if( opt.check("min_mass" )  ) mass_epsilon = opt.find("min_mass").asDouble();
-    if( opt.check("min_inertia" )  ) inertia_epsilon = opt.find("min_inertia").asDouble();
-
-    // Parse part info
-    int version_head = -1;
-    if( opt.check("headV2") )
-    {
-        version_head = 2;
-    }
-    if( opt.check("headV1") )
-    {
-        version_head = 1;
-    }
-    if( version_head < 0 )
-    {
-        std::cerr << "icub_urdf_sdf_from_dh_generator : missing head version"  << std::endl;
-        return EXIT_FAILURE;
-    }
-
-
-    int version_arms = -1;
-    if( opt.check("armsV2") )
-    {
-        version_arms = 2;
-    }
-    if( opt.check("armsV1") )
-    {
-        version_arms = 1;
-    }
-    if( version_arms < 0 )
-    {
-        std::cerr << "icub_urdf_sdf_from_dh_generator : missing arms version"  << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    int version_legs = -1;
-    if( opt.check("legsV2") )
-    {
-        version_legs = 2;
-    }
-    if( opt.check("legsV1") )
-    {
-        version_legs = 1;
-    }
-    if( version_legs < 0 )
-    {
-        std::cerr << "icub_urdf_sdf_from_dh_generator : missing legs version"  << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    int version_feet = -1;
-    if( opt.check("feetV2") )
-    {
-        version_feet = 2;
-    }
-    if( opt.check("feetV1") )
-    {
-        version_feet = 1;
-    }
-    if( version_feet < 0 )
-    {
-        std::cerr << "icub_urdf_sdf_from_dh_generator : missing feet version" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    // iCubParis02 is a special robot, and needs a special options
-    bool isiCubParis02 = opt.check("iCubParis02");
-
-
-    //Generating model for black iCub
-    //                          robot_name     directory    head   legs   feet    Paris02  gazeboSim
-    bool simple_meshes = false;
-    urdf::ModelInterfaceSharedPtr  model;
-    bool ok = generate_iCub_urdf_model(opt.find("YARP_ROBOT_NAME").asString(),
-                                       version_head , version_legs , version_feet,
-                                       isiCubParis02, false, simple_meshes ,
-                                       data_directory,mass_epsilon,inertia_epsilon,false,model,output_file);
-
-    if( !ok )
-    {
-        std::cerr << "Failure in generating URDF model for " << opt.find("YARP_ROBOT_NAME").asString() << std::endl;
-        return EXIT_FAILURE;
-    }
-    else
-    {
-        return EXIT_SUCCESS;
-    }
-}
-
diff --git a/dh/generator/urdf_gazebo_cleanup.cpp b/dh/generator/urdf_gazebo_cleanup.cpp
deleted file mode 100644
index 90b3151..0000000
--- a/dh/generator/urdf_gazebo_cleanup.cpp
+++ /dev/null
@@ -1,152 +0,0 @@
-/**
- * Copyright  (C)  2013 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <kdl/treefksolverpos_recursive.hpp>
-#include <kdl/frames_io.hpp>
-
-#include "urdf_utils.h"
-
-#include <iDynTree/ModelIO/impl/urdf_import.hpp>
-
-#include <iostream>
-#include <sstream>
-#include <fstream>
-
-#include <tinyxml.h>
-
-#include <yarp/os/Value.h>
-#include <yarp/os/Property.h>
-
-using namespace iDynTree;
-using namespace KDL;
-using namespace std;
-
-using namespace urdf;
-using namespace boost;
-
-
-
-
-int main(int argc, char* argv[])
-{
-
-    bool status = true;
-        bool ret = true;
-
-    bool verbose = true;
-
-
-    yarp::os::Property opt;
-    opt.fromCommand(argc,argv);
-
-    double mass_epsilon = 0.1;
-    double inertia_epsilon = 0.01;
-
-    if( opt.check("min_mass" )  ) mass_epsilon = opt.find("min_mass").asDouble();
-    if( opt.check("min_inertia" )  ) inertia_epsilon = opt.find("min_inertia").asDouble();
-
-        std::string rule4_prefix =  "model://icub/";
-
-    if( opt.check("help") || !opt.check("input") || !opt.check("output") )
-    {
-        std::cerr << "Usage: \t urdf_gazebo_cleanup --input urdf_input.xml --output urdf_output.xml" << std::endl;
-        std::cerr << "This tool is used to make a urdf file suitable for conversion in SDF format used by Gazebo Simulator" << std::endl;
-        std::cerr << "The following modification are made: " << std::endl;
-        std::cerr << "  * (Rule 0) If a link without inertia is in the root, remove it (to handle base_link as commonly found in humanoid models for REP 120)" << std::endl;
-        std::cerr << "  * (Rule 1) Links with zero mass, with no childrens and connected to their parent with a fixed joint are removed" << std::endl;
-        std::cerr << "  * (Rule 2) The fixed joint connecting two non-leaf links (typically used in iCub for modeling FT sensors) are transformed in rotational joint with no range (to address this issue https://bitbucket.org/osrf/gazebo/issue/618/add-option-to-disable-auto-fixed-joint)" << std::endl;
-        std::cerr << "  * (Rule 3) For the remaining  (also if the previous steps are not actually exectuted)  linkswith zero mass or zero diagonal inertia elements a small mass ( " << mass_epsilon << " ) and diagonal inertia elements " << inertia_epsilon << "  is added, to avoid stability problems in simulation" << std::endl;
-        std::cerr << "  * (Rule 4) add a model:// uri prefix to all meshes uris" << std::endl;
-        std::cerr << "Additional options: --no_rule0 avoid applyng rule0, --no_rule1 avoid applyng rule1 and so on" << std::endl;
-        std::cerr << "Additional options: --min_mass and --min_inertia are used to select the minimum mass to a give to an object in Rule 3" << std::endl;
-        std::cerr << "Additional options: --rule4_prefix is used to specify the prefix used in rule 4 (default: " << rule4_prefix << " ) " << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    if( opt.check("rule4_prefix") ) rule4_prefix = opt.find("rule4_prefix").asString().c_str();
-
-
-
-    std::string file_name_urdf_input(opt.find("input").asString().c_str());
-    std::string file_name_urdf_output(opt.find("output").asString().c_str());
-
-
-    boost::shared_ptr<urdf::ModelInterface> urdf_input;
-
-    std::ifstream t_im(file_name_urdf_input.c_str());
-    std::stringstream buffer_im;
-    buffer_im << t_im.rdbuf();
-    urdf_input = parseURDF(buffer_im.str());
-    if( !ret ) {
-        std::cerr << "urdf_gazebo_cleanup: Fatal error in URDF xml parsing" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    if( !opt.check("no_rule0") ) {
-        urdf_gazebo_cleanup_remove_massless_root(urdf_input);
-
-        std::cout << "urdf_gazebo_cleanup: Rule 0 applied successfully" << std::endl;
-    }
-
-
-
-    if( !opt.check("no_rule1") ) {
-        if( urdf_gazebo_cleanup_remove_frames(urdf_input) ) {
-            std::cout << "urdf_gazebo_cleanup: Rule 1 applied successfully" << std::endl;
-        } else {
-            std::cout << "urdf_gazebo_cleanup: Rule 1 failed" << std::endl;
-            return EXIT_FAILURE;
-        }
-    }
-
-
-    if( !opt.check("no_rule2") ) {
-        if( urdf_gazebo_cleanup_transform_FT_sensors(urdf_input) ) {
-            std::cout << "urdf_gazebo_cleanup: Rule 2 applied successfully" << std::endl;
-        } else {
-            std::cout << "urdf_gazebo_cleanup: Rule 2 failed" << std::endl;
-            return EXIT_FAILURE;
-        }
-    }
-
-
-    if( !opt.check("no_rule3") ) {
-        if( urdf_gazebo_cleanup_regularize_masses(urdf_input,mass_epsilon,inertia_epsilon) ) {
-            std::cout << "urdf_gazebo_cleanup: Rule 3 applied successfully" << std::endl;
-        } else {
-            std::cout << "urdf_gazebo_cleanup: Rule 3 failed" << std::endl;
-            return EXIT_FAILURE;
-        }
-    }
-
-    if( !opt.check("no_rule4") ) {
-        if( urdf_gazebo_cleanup_add_model_uri(urdf_input,rule4_prefix) ) {
-            std::cout << "urdf_gazebo_cleanup: Rule 4 applied successfully" << std::endl;
-        } else {
-            std::cout << "urdf_gazebo_cleanup: Rule 4 failed" << std::endl;
-            return EXIT_FAILURE;
-        }
-    }
-
-
-
-
-    TiXmlDocument* xml_doc;
-
-    xml_doc = exportURDF(urdf_input);
-
-    if( ! xml_doc->SaveFile(file_name_urdf_output) ) {
-        std::cerr << "urdf_gazebo_cleanup: Fatal error in URDF xml saving" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    std::cerr << "urdf_gazebo_cleanup: URDF file successfully created" << std::endl;
-
-    return EXIT_SUCCESS;
-}
diff --git a/dh/generator/urdf_get_limits.cpp b/dh/generator/urdf_get_limits.cpp
deleted file mode 100644
index d66e03e..0000000
--- a/dh/generator/urdf_get_limits.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/**
- * Copyright  (C)  2013 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <kdl/treefksolverpos_recursive.hpp>
-#include <kdl/frames_io.hpp>
-
-#include <iDynTree/ModelIO/impl/urdf_import.hpp>
-
-#include <iostream>
-#include <sstream>
-#include <fstream>
-
-#include <tinyxml.h>
-
-#include "urdf_utils.h"
-
-using namespace iDynTree;
-using namespace KDL;
-using namespace std;
-
-using namespace urdf;
-using namespace boost;
-
-
-int main(int argc, char* argv[])
-{
-    bool status = true;
-        bool ret = true;
-
-    bool verbose = true;
-
-    if( argc != 4 ) {
-        std::cerr << "Usage: \t urdf_get_limits urdf_limits.xml urdf_input.xml urdf_output.xml" << std::endl;
-        std::cerr << "In which the urdf_output.xml is the same model of urdf_input.xml, but with the limits of urdf_limits.xml" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    std::string file_name_urdf_limits(argv[1]);
-    std::string file_name_urdf_input(argv[2]);
-    std::string file_name_urdf_output(argv[3]);
-
-
-    boost::shared_ptr<urdf::ModelInterface> urdf_limits;
-    boost::shared_ptr<urdf::ModelInterface> urdf_input;
-
-    std::ifstream t_um(file_name_urdf_limits.c_str());
-    std::stringstream buffer_um;
-    buffer_um << t_um.rdbuf();
-    urdf_limits = parseURDF(buffer_um.str());
-
-    std::ifstream t_im(file_name_urdf_input.c_str());
-    std::stringstream buffer_im;
-    buffer_im << t_im.rdbuf();
-    urdf_input = parseURDF(buffer_im.str());
-    if( !ret ) {
-        std::cerr << "Fatal error in URDF xml parsing" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    urdf_import_limits(urdf_input,urdf_limits);
-
-    TiXmlDocument* xml_doc;
-
-    xml_doc = exportURDF(urdf_input);
-
-    if( ! xml_doc->SaveFile(file_name_urdf_output) ) {
-        std::cerr << "Fatal error in URDF xml saving" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    std::cerr << "URDF file successfully created" << std::endl;
-
-    return EXIT_SUCCESS;
-}
diff --git a/dh/generator/urdf_get_meshes.cpp b/dh/generator/urdf_get_meshes.cpp
deleted file mode 100644
index 5560535..0000000
--- a/dh/generator/urdf_get_meshes.cpp
+++ /dev/null
@@ -1,81 +0,0 @@
-/**
- * Copyright  (C)  2013 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
- 
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include "urdf_utils.h"
-
-#include <iDynTree/ModelIO/impl/urdf_import.hpp>
-
-#include <iostream>
-#include <sstream>
-#include <fstream>
-
-#include <tinyxml.h>
-
-using namespace iDynTree;
-using namespace KDL;
-using namespace std;
-
-using namespace urdf;
-using namespace boost;
-
-
-
-int main(int argc, char* argv[])
-{
-    bool status = true;
-        bool ret = true;
-        
-    bool verbose = true;
-    
-    if( argc != 4 ) {
-        std::cerr << "Usage: \t urdf_get_meshes urdf_meshes.xml urdf_input.xml urdf_output.xml" << std::endl;
-        std::cerr << "In which the urdf_output.xml is the same model of urdf_input.xml, but with the meshes of urdf_meshes.xml" << std::endl;
-        return EXIT_FAILURE;
-    }
-    
-    std::string file_name_urdf_meshes(argv[1]);
-    std::string file_name_urdf_input(argv[2]);
-    std::string file_name_urdf_output(argv[3]);
-
-
-    boost::shared_ptr<urdf::ModelInterface> urdf_meshes;
-    boost::shared_ptr<urdf::ModelInterface> urdf_input;
-
-    std::ifstream t_um(file_name_urdf_meshes.c_str());
-    std::stringstream buffer_um;
-    buffer_um << t_um.rdbuf();
-    urdf_meshes = parseURDF(buffer_um.str());
-    
-    std::ifstream t_im(file_name_urdf_input.c_str());
-    std::stringstream buffer_im;
-    buffer_im << t_im.rdbuf();
-    urdf_input = parseURDF(buffer_im.str());
-    if( !ret ) {
-        std::cerr << "Fatal error in URDF xml parsing" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    if( !urdf_import_meshes(urdf_input,urdf_meshes) ) {
-        std::cerr << "Fatal error in URDF mesh importing" << std::endl;
-        return false;
-    }
-    
-    TiXmlDocument* xml_doc;
-    
-    xml_doc = exportURDF(urdf_input);
-    
-    if( ! xml_doc->SaveFile(file_name_urdf_output) ) {
-        std::cerr << "Fatal error in URDF xml saving" << std::endl;
-        return EXIT_FAILURE;
-    }
-    
-    std::cerr << "URDF file successfully created" << std::endl;
-    
-    return EXIT_SUCCESS;
-}
diff --git a/dh/generator/urdf_get_total_mass.cpp b/dh/generator/urdf_get_total_mass.cpp
deleted file mode 100644
index 3704a4a..0000000
--- a/dh/generator/urdf_get_total_mass.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-/**
- * Copyright  (C)  2013 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
- 
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <kdl/treefksolverpos_recursive.hpp>
-#include <kdl/frames_io.hpp>
-
-#include <iDynTree/ModelIO/impl/urdf_import.hpp>
-
-#include <iostream>
-#include <sstream>
-#include <fstream>
-
-#include <tinyxml.h>
-
-#include "urdf_utils.h"
-
-using namespace iDynTree;
-using namespace KDL;
-using namespace std;
-
-using namespace urdf;
-using namespace boost;
-
-
-int main(int argc, char* argv[])
-{
-    bool status = true;
-        bool ret = true;
-        
-    bool verbose = true;
-    
-    if( argc != 2 ) {
-        std::cerr << "Usage: \t urdf_get_total mass urdf_model.xml" << std::endl;
-        std::cerr << "Prints the total mass of the urdf model" << std::endl;
-        return EXIT_FAILURE;
-    }
-    
-    std::string file_name_urdf(argv[1]);
-
-    boost::shared_ptr<urdf::ModelInterface> urdf_input;
-
-    
-    std::ifstream t_im(file_name_urdf.c_str());
-    std::stringstream buffer_im;
-    buffer_im << t_im.rdbuf();
-    urdf_input = parseURDF(buffer_im.str());
-    if( !urdf_input ) {
-        std::cerr << "Fatal error in URDF xml parsing" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    double mass = getTotalMass(*urdf_input);
-    
-    std::cout << "URDF file has a total mass of " << mass << std::endl;
-    
-    return EXIT_SUCCESS;
-}
diff --git a/dh/generator/urdf_print_homogeneous_transformations.cpp b/dh/generator/urdf_print_homogeneous_transformations.cpp
deleted file mode 100644
index 0a47d35..0000000
--- a/dh/generator/urdf_print_homogeneous_transformations.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/**
- * Copyright  (C)  2013 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
- 
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <kdl/treefksolverpos_recursive.hpp>
-#include <kdl/frames_io.hpp>
-
-#include <iDynTree/ModelIO/impl/urdf_import.hpp>
-
-#include <iostream>
-#include <sstream>
-#include <fstream>
-
-#include <tinyxml.h>
-
-#include "urdf_utils.h"
-
-using namespace iDynTree;
-using namespace KDL;
-using namespace std;
-
-using namespace urdf;
-using namespace boost;
-
-
-int main(int argc, char* argv[])
-{
-    bool status = true;
-        bool ret = true;
-        
-    bool verbose = true;
-    
-    if( argc != 2 ) {
-        std::cerr << "Usage: \t urdf_print_homegenous_transformation urdf_model.xml" << std::endl;
-        std::cerr << "Prints the homogeneous transformation between the different reference frames of an the urdf model" << std::endl;
-        return EXIT_FAILURE;
-    }
-    
-    std::string file_name_urdf(argv[1]);
-
-    boost::shared_ptr<urdf::ModelInterface> urdf_input;
-
-    
-    std::ifstream t_im(file_name_urdf.c_str());
-    std::stringstream buffer_im;
-    buffer_im << t_im.rdbuf();
-    urdf_input = parseURDF(buffer_im.str());
-    if( !urdf_input ) {
-        std::cerr << "Fatal error in URDF xml parsing" << std::endl;
-        return EXIT_FAILURE;
-    }
-
-    ret = urdf_print_hom_transformations(urdf_input);
-    
-    if(!ret ) {
-        std::cerr << "Error in printing homogeneous matrices" << std::endl;
-		return EXIT_FAILURE;
-	}
-
-    return EXIT_SUCCESS;
-}
diff --git a/dh/generator/urdf_sdf_from_dh_utils.cpp b/dh/generator/urdf_sdf_from_dh_utils.cpp
deleted file mode 100644
index 48a3e90..0000000
--- a/dh/generator/urdf_sdf_from_dh_utils.cpp
+++ /dev/null
@@ -1,899 +0,0 @@
-#include "urdf_sdf_from_dh_utils.h"
-
-
-
-bool addGazeboYarpPluginsControlboardToSDF(sdf::ElementPtr model, std::string part_name, std::string robot_name)
-{
-    /*
-     <plugin name="controlboard_right_arm" filename="libgazebo_yarp_controlboard.so">
-        <yarpConfigurationFile>model://icub/conf/gazebo_icub_right_arm.ini</yarpConfigurationFile>
-        <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
-     </plugin>
-     */
-    sdf::ElementPtr plugin = model->AddElement("plugin");
-    plugin->GetAttribute("name")->SetFromString("controlboard_"+part_name);
-    plugin->GetAttribute("filename")->SetFromString("libgazebo_yarp_controlboard.so");
-
-    sdf::ElementPtr yarpConfigurationFile_description(new sdf::Element);
-    yarpConfigurationFile_description->SetName("yarpConfigurationFile");
-    plugin->AddElementDescription(yarpConfigurationFile_description);
-    sdf::ElementPtr yarpConfigurationFile = plugin->AddElement("yarpConfigurationFile");
-    yarpConfigurationFile->AddValue("string","model://icub/conf/gazebo_icub_"+part_name+".ini",false);
-
-    if( part_name == "right_arm" || part_name == "left_arm" ) {
-        sdf::ElementPtr initialConfiguration_description(new sdf::Element);
-        initialConfiguration_description->SetName("initialConfiguration");
-        plugin->AddElementDescription(initialConfiguration_description);
-        sdf::ElementPtr initialConfiguration = plugin->AddElement("initialConfiguration");
-        initialConfiguration->AddValue("string","-0.52 0.52 0 0.785 0 0 0.698",false);
-    }
-
-    return true;
-}
-
-/**
- *
- * @param part: head,torso,right_arm,left_arm,right_leg,left_leg
- */
-bool addGazeboYarpPluginsControlboardToSDF(sdf::SDFPtr icub_sdf, std::string part_name, std::string robot_name)
-{
-    /// \todo add check on parts
-    return addGazeboYarpPluginsControlboardToSDF(icub_sdf->Root()->GetElement("model"),part_name,robot_name);
-}
-
-/**
- *
- * @param sensor: can be only imu_sensor
- */
-bool addGazeboYarpPluginsIMUToSDF(sdf::SDFPtr icub_sdf, std::string sensor_name, std::string robot_name)
-{
-    assert( icub_sdf->Root()->HasElement("model"));
-    /*
-     <sensor name="imu_sensor" type="imu">
-        <always_on>1</always_on>
-        <update_rate>100</update_rate>
-        <visualize>1</visualize>
-        <imu/>
-        <plugin filename="libgazebo_yarp_imu.so" name="iCub_yarp_gazebo_plugin_IMU">
-          <yarpConfigurationFile>model://icub/conf/gazebo_icub_inertial.ini</yarpConfigurationFile>
-        </plugin>
-       <pose>0.0185 -0.1108 0.0066 1.5708 -0 0</pose>
-      </sensor>
-     */
-    if( sensor_name == "imu_sensor" ) {
-        //imu sensor is child of link head
-        sdf::ElementPtr model_elem = icub_sdf->Root()->GetElement("model");
-        for (sdf::ElementPtr link = model_elem->GetElement("link"); link; link = link->GetNextElement("link")) {
-            if( link->GetAttribute("name")->GetAsString() == "head" ) {
-                sdf::ElementPtr sensor = link->AddElement("sensor");
-                sensor->GetAttribute("name")->SetFromString(sensor_name);
-                sensor->GetAttribute("type")->SetFromString("imu");
-                sdf::ElementPtr always_on = sensor->AddElement("always_on");
-                always_on->AddValue("int","1",false);
-                sdf::ElementPtr update_rate = sensor->AddElement("update_rate");
-                update_rate->AddValue("int","100",false);
-                sdf::ElementPtr visualize = sensor->AddElement("visualize");
-                visualize->AddValue("int","1",false);
-                sdf::ElementPtr imu = sensor->AddElement("imu");
-                sdf::ElementPtr plugin = sensor->AddElement("plugin");
-                plugin->GetAttribute("filename")->SetFromString("libgazebo_yarp_imu.so");
-                plugin->GetAttribute("name")->SetFromString("iCub_yarp_gazebo_plugin_IMU");
-                //plugin->AddAttribute("filename","string","libgazebo_yarp_imu.so",false);
-                //plugin->AddAttribute("name","string","iCub_yarp_gazebo_plugin_IMU",false);
-                sdf::ElementPtr yarpConfigurationFile_description(new sdf::Element);
-                yarpConfigurationFile_description->SetName("yarpConfigurationFile");
-                plugin->AddElementDescription(yarpConfigurationFile_description);
-                sdf::ElementPtr yarpConfigurationFile = plugin->AddElement("yarpConfigurationFile");
-                yarpConfigurationFile->AddValue("string","model://icub/conf/gazebo_icub_inertial.ini",false);
-                sdf::ElementPtr pose = sensor->AddElement("pose");
-                pose->AddValue("string","0.0185 -0.1108 0.0066 1.5708 -0 0",false);
-            }
-        }
-        return true;
-    } else {
-        return false;
-    }
-}
-
-/**
- *
- * @param sensor_name: left_arm,right_arm,left_leg,right_leg,left_foot,right_foot
- */
-std::string getFTJointName(const std::string sensor_name)
-{
-    if( sensor_name == "left_arm" ) {
-        return "l_arm_ft_sensor";
-    } else if ( sensor_name == "right_arm" ) {
-        return "r_arm_ft_sensor";
-    } else if ( sensor_name == "left_leg" ) {
-        return "l_leg_ft_sensor";
-    } else if ( sensor_name == "right_leg" ) {
-        return "r_leg_ft_sensor";
-    } else if ( sensor_name == "left_foot" ) {
-        return "l_foot_ft_sensor";
-    } else if ( sensor_name == "right_foot" ) {
-        return "r_foot_ft_sensor";
-    } else {
-        return ERROR_FT_SENSOR_JOINT;
-    }
-}
-
-/**
- * Add the force/torque sensor description to the URDF (using gazebo extentions)
- * @param sensor_name: left_arm,right_arm,left_leg,right_leg,left_foot,right_foot
- */
-bool addGazeboYarpPluginsFTToURDF(TiXmlDocument* urdf_doc, std::string sensor_name)
-{
-    // FT sensors extracted from iDyn are defined with respect to child link reference frame <frame>child</frame>
-    // and they are measuring the wrench that the child link applies on the parent link <measure_direction>child_to_parent</measure_direction>
-
-    //Get the URDF joint that models the FT sensor
-    std::string ft_sensor_joint = getFTJointName(sensor_name);
-
-    if( ft_sensor_joint == ERROR_FT_SENSOR_JOINT ) {
-        return false;
-    }
-
-    /*
-       <gazebo reference="l_foot_ft_sensor">
-        <sensor name='left_foot_ft' type='force_torque'>
-        <force_torque>
-           <frame>child</frame>
-           <measure_direction>child_to_parent</measure_direction>
-        </force_torque>
-        <always_on>1</always_on>
-       <update_rate>100</update_rate>
-       </sensor>
-       </gazebo>
-     */
-
-    TiXmlElement * robot_element = urdf_doc->FirstChildElement( "robot" );
-
-    TiXmlElement * gazebo_element = new TiXmlElement( "gazebo" );
-    robot_element->LinkEndChild(gazebo_element);
-
-    gazebo_element->SetAttribute("reference",ft_sensor_joint.c_str());
-
-    TiXmlElement * sensor_element = new TiXmlElement( "sensor" );
-    gazebo_element->LinkEndChild(sensor_element);
-
-    std::string sdf_sensor_name = sensor_name+"_ft";
-    sensor_element->SetAttribute("name",sdf_sensor_name.c_str());
-    sensor_element->SetAttribute("type","force_torque");
-
-    TiXmlElement * force_torque_element = new TiXmlElement( "force_torque" );
-    sensor_element->LinkEndChild(force_torque_element);
-
-    TiXmlElement * frame_element = new TiXmlElement( "frame" );
-    force_torque_element->LinkEndChild(frame_element);
-
-    TiXmlText * frame_text = new TiXmlText( "child" );
-    frame_element->LinkEndChild(frame_text);
-
-    TiXmlElement * measure_direction_element = new TiXmlElement( "measure_direction" );
-    force_torque_element->LinkEndChild(measure_direction_element);
-
-    TiXmlText * measure_direction_text = new TiXmlText( "child_to_parent" );
-    measure_direction_element->LinkEndChild(measure_direction_text);
-
-    TiXmlElement * update_rate_element = new TiXmlElement( "update_rate" );
-    sensor_element->LinkEndChild(update_rate_element);
-
-    TiXmlText * update_rate_text = new TiXmlText( "100" );
-    update_rate_element->LinkEndChild(update_rate_text);
-
-    TiXmlElement * always_on_element = new TiXmlElement( "always_on" );
-    sensor_element->LinkEndChild(always_on_element);
-
-    TiXmlText * always_on_text = new TiXmlText( "1" );
-    always_on_element->LinkEndChild(always_on_text);
-
-    return true;
-}
-
-bool add_iDynTreeFTToURDF(TiXmlDocument* urdf_doc, std::string sensor_name)
-{
-    // FT sensors extracted from iDyn are defined with respect to child link reference frame <frame>child</frame>
-    // and they are measuring the wrench that the child link applies on the parent link <measure_direction>child_to_parent</measure_direction>
-
-    //Get the URDF joint that models the FT sensor
-    std::string ft_sensor_joint = getFTJointName(sensor_name);
-
-    if( ft_sensor_joint == ERROR_FT_SENSOR_JOINT ) {
-        return false;
-    }
-
-    /*
-        <sensor name="l_leg_ft_sensor" type="force_torque">
-            <parent joint="l_leg_ft_sensor"/>
-            <force_torque>
-                <frame>child</frame>
-                <measure_direction>child_to_parent</measure_direction>
-            </force_torque>
-        </sensor>
-     */
-
-    TiXmlElement * robot_element = urdf_doc->FirstChildElement( "robot" );
-
-    TiXmlElement * sensor_element = new TiXmlElement( "sensor" );
-    robot_element->LinkEndChild(sensor_element);
-
-    sensor_element->SetAttribute("type","force_torque");
-    sensor_element->SetAttribute("name",ft_sensor_joint.c_str());
-
-    sensor_element->SetAttribute("type","force_torque");
-
-    TiXmlElement * parent_element = new TiXmlElement( "parent" );
-    sensor_element->LinkEndChild(parent_element);
-
-    parent_element->SetAttribute("joint",ft_sensor_joint);
-
-    TiXmlElement * force_torque_element = new TiXmlElement( "force_torque" );
-    sensor_element->LinkEndChild(force_torque_element);
-
-    TiXmlElement * frame_element = new TiXmlElement( "frame" );
-    force_torque_element->LinkEndChild(frame_element);
-
-    TiXmlText * frame_text = new TiXmlText( "child" );
-    frame_element->LinkEndChild(frame_text);
-
-    TiXmlElement * measure_direction_element = new TiXmlElement( "measure_direction" );
-    force_torque_element->LinkEndChild(measure_direction_element);
-
-    TiXmlText * measure_direction_text = new TiXmlText( "child_to_parent" );
-    measure_direction_element->LinkEndChild(measure_direction_text);
-
-
-    return true;
-}
-
-void AddElementAndSetValue(sdf::ElementPtr parent, std::string childType, std::string value_type, std::string value)
-{
-    sdf::ElementPtr child = parent->AddElement(childType);
-    child->AddValue(value_type,value,false);
-}
-
-bool addGazeboSurfaceFrictionInformationToCollisionSDF(sdf::ElementPtr collision_sdf)
-{
-        /*
-        From iCub hand modified urdf,
-        commit https://github.com/robotology/icub-gazebo/commit/7cd9ee3059fef2344f9029bffa3a8b2b8895c059
-
-        <collision name='l_foot_collision'>
-                <!-- <pose>3.95315e-11 4.452e-07 -5.14839e-06 8.98039e-06 -1.36732e-05 -1.01971e-06</pose> -->
-           <pose>-3e-02 0 3.5e-2 0 0 0</pose>
-                <geometry>
-<!--                     <mesh>
-                        <scale>1 1 1</scale>
-                        <uri>model://icub/meshes/collision/icub_simple_collision_l_foot.dae</uri>
-                    </mesh> -->
-                   <box>
-                        <size>0.03 0.07 0.15</size>
-                    </box>
-                </geometry>
-                <!-- <max_contacts> 10 </max_contacts> -->
-               <max_contacts> 4 </max_contacts>
-                <surface>
-                    <contact>
-                        <ode>
-                            <soft_erp>  0.200  </soft_erp>
-                            <soft_cfm>  0.000  </soft_cfm>
-                            <kp>        100000.000  </kp>
-                            <kd>        1.0     </kd>
-                            <max_vel>   100.00  </max_vel>
-                            <min_depth> 0.0010  </min_depth>
-                        </ode>
-                    </contact>
-                    <friction>
-                        <ode>
-                            <mu>  1  </mu>
-                            <mu2> 1  </mu2>
-                            <fdir1>0.000000 0.000000 0.000000</fdir1>
-                            <slip1> 0.00000 </slip1>
-                            <slip2> 0.00000 </slip2>
-                        </ode>
-                    </friction>
-                    <bounce>
-                        <restitution_coefficient>0.000000</restitution_coefficient>
-                        <threshold>100000.000000</threshold>
-                    </bounce>
-                </surface>
-            </collision>
-
-     */
-
-    // handle max_contacts
-    sdf::ElementPtr max_contacts = collision_sdf->AddElement("max_contacts");
-    max_contacts->AddValue("int","4",false);
-
-    // handle surface
-    sdf::ElementPtr surface = collision_sdf->AddElement("surface");
-
-    // contact group
-    sdf::ElementPtr contact = surface->AddElement("contact");
-    sdf::ElementPtr ode = contact->AddElement("ode");
-    AddElementAndSetValue(ode,"soft_erp","double","0.2");
-    AddElementAndSetValue(ode,"soft_cfm","double","0.0");
-    AddElementAndSetValue(ode,"kp","double","100000.000");
-    AddElementAndSetValue(ode,"kd","double","1.0");
-    AddElementAndSetValue(ode,"max_vel","double","100.00");
-    AddElementAndSetValue(ode,"min_depth","double","0.0010");
-
-    // friction group
-    sdf::ElementPtr friction = surface->AddElement("friction");
-    sdf::ElementPtr friction_ode = friction->AddElement("ode");
-    AddElementAndSetValue(friction_ode,"mu","double","1.000000");
-    AddElementAndSetValue(friction_ode,"mu2","double","1.000000");
-    AddElementAndSetValue(friction_ode,"fdir1","vector3","0.0 0.0 0.0");
-    AddElementAndSetValue(friction_ode,"slip1","double","0.000000");
-    AddElementAndSetValue(friction_ode,"slip2","double","0.000000");
-
-    // bounce group
-    sdf::ElementPtr bounce = surface->AddElement("bounce");
-    AddElementAndSetValue(bounce,"restitution_coefficient","double","0.000000");
-    AddElementAndSetValue(bounce,"threshold","double","100000.000000");
-
-    return true;
-}
-
-
-/**
- * For a given link add surface friction informations to the SDF
- *
- */
-bool addGazeboSurfaceFrictionInformationToSDF(sdf::SDFPtr icub_sdf, std::string link_name)
-{
-
-    sdf::ElementPtr model_elem = icub_sdf->Root()->GetElement("model");
-
-    for (sdf::ElementPtr link = model_elem->GetElement("link"); link; link = link->GetNextElement("link"))
-    {
-        if( link->GetAttribute("name")->GetAsString() == link_name )
-        {
-            for (sdf::ElementPtr collision = link->GetElement("collision"); collision; collision = link->GetNextElement("collision"))
-            {
-                addGazeboSurfaceFrictionInformationToCollisionSDF(collision);
-            }
-        }
-    }
-
-    return true;
-}
-
-
-
-bool addGazeboYarpPluginsFTToSDF(sdf::ElementPtr joint_element, std::string sensor_name, std::string robot_name)
-{
-    /*
-     <sensor name="left_leg_ft" type="force_torque">
-        <always_on>true</always_on>
-        <visualize>true</visualize>
-        <update_rate>100</update_rate>
-        <pose>0 0 0 0 0 0</pose>
-        <plugin filename="libgazebo_yarp_forcetorque.so" name="left_leg_ft_plugin">
-          <yarpConfigurationFile>model://icub/conf/FT/gazebo_icub_left_leg_ft.ini</yarpConfigurationFile>
-        </plugin>
-      </sensor>
-     */
-    sdf::ElementPtr sensor = joint_element->AddElement("sensor");
-                    sensor->GetAttribute("name")->SetFromString(sensor_name+"_ft");
-                    sensor->GetAttribute("type")->SetFromString("force_torque");
-                    sdf::ElementPtr always_on = sensor->AddElement("always_on");
-                    always_on->AddValue("int","1",false);
-                    sdf::ElementPtr update_rate = sensor->AddElement("update_rate");
-                    update_rate->AddValue("int","100",false);
-                    sdf::ElementPtr pose = sensor->AddElement("pose");
-                    pose->AddValue("string","0 0 0 0 0 0",false);
-                    sdf::ElementPtr plugin = sensor->AddElement("plugin");
-                    plugin->GetAttribute("filename")->SetFromString("libgazebo_yarp_forcetorque.so");
-                    plugin->GetAttribute("name")->SetFromString(sensor_name+"_ft_plugin");
-                    sdf::ElementPtr yarpConfigurationFile_description(new sdf::Element);
-                    yarpConfigurationFile_description->SetName("yarpConfigurationFile");
-                    plugin->AddElementDescription(yarpConfigurationFile_description);
-                    sdf::ElementPtr yarpConfigurationFile = plugin->AddElement("yarpConfigurationFile");
-                    yarpConfigurationFile->AddValue("string","model://icub/conf/FT/gazebo_icub_"+sensor_name+"_ft.ini",false);
-
-    return true;
-}
-
-
-/**
- * Add the force/torque sensor description (with the relative gazebo_yarp_plugin)
- * @param sensor_name: left_arm,right_arm,left_leg,right_leg,left_foot,right_foot
- */
-bool addGazeboYarpPluginsFTToSDF(sdf::SDFPtr icub_sdf, std::string sensor_name, std::string robot_name)
-{
-    bool ret;
-
-    sdf::ElementPtr model_elem = icub_sdf->Root()->GetElement("model");
-
-    //Get the gazebo joint that models the FT sensor
-    std::string ft_sensor_joint = getFTJointName(sensor_name);
-
-    if( ft_sensor_joint == ERROR_FT_SENSOR_JOINT ) {
-        return false;
-    }
-
-    for (sdf::ElementPtr joint = model_elem->GetElement("joint"); joint; joint = joint->GetNextElement("joint")) {
-        if( joint->GetAttribute("name")->GetAsString() == ft_sensor_joint ) {
-            ret = addGazeboYarpPluginsFTToSDF(joint,sensor_name,robot_name);
-            if( !ret ) { return false; }
-        }
-    }
-
-    return true;
-}
-
-
-bool addGazeboODEContactsProperty(sdf::ElementPtr collision_elem)
-{
-    /*
-     * <surface>
-         <contact>
-            <ode>
-                <kp>1e+06</kp>
-                <kd>100</kd>
-                <max_vel>1</max_vel>
-                <min_depth>0</min_depth>
-            </ode>
-        </contact>
-        <friction>
-            <ode>
-                <mu>1</mu>
-                <mu2>1</mu2>
-                <fdir1>1 0 0</fdir1>
-            </ode>
-        </friction>
-        </surface>
-    */
-    sdf::ElementPtr surface = collision_elem->AddElement("surface");
-
-    sdf::ElementPtr contact = surface->AddElement("contact");
-    sdf::ElementPtr ode_contact = contact->AddElement("ode");
-
-    sdf::ElementPtr kp = ode_contact->AddElement("kp");
-    kp->AddValue("string","1e06",false);
-
-    sdf::ElementPtr kd = ode_contact->AddElement("kd");
-    kd->AddValue("string","100",false);
-
-    sdf::ElementPtr max_vel = ode_contact->AddElement("max_vel");
-    max_vel->AddValue("string","1",false);
-
-    sdf::ElementPtr min_depth = ode_contact->AddElement("min_depth");
-    min_depth->AddValue("string","0",false);
-
-    sdf::ElementPtr friction = surface->AddElement("friction");
-    sdf::ElementPtr ode_friction = friction->AddElement("ode");
-
-    sdf::ElementPtr mu = ode_friction->AddElement("mu");
-    mu->AddValue("string","1",false);
-
-    sdf::ElementPtr mu2 = ode_friction->AddElement("mu2");
-    mu2->AddValue("string","1",false);
-
-    sdf::ElementPtr fdir1 = ode_friction->AddElement("fdir1");
-    fdir1->AddValue("string","1 0 0",false);
-
-    return true;
-}
-
-bool addGazeboODEContactsProperties(sdf::SDFPtr icub_sdf, std::string link_name, std::string collision_name)
-{
-    sdf::ElementPtr model_elem = icub_sdf->Root()->GetElement("model");
-
-    for (sdf::ElementPtr link = model_elem->GetElement("link"); link; link = link->GetNextElement("link")) {
-        if( link->GetAttribute("name")->GetAsString() == link_name ) {
-            for (sdf::ElementPtr collision = link->GetElement("collision"); collision; collision = collision->GetNextElement("collision")) {
-                if( collision->GetAttribute("name")->GetAsString() == collision_name ) {
-                    return addGazeboODEContactsProperty(collision);
-                }
-            }
-        }
-    }
-
-    return false;
-}
-
-bool substituteCollisionWithBoxInCollisionSDF(sdf::ElementPtr collision,
-                                     std::string new_box_collision_pose,
-                                     std::string box_size)
-{
-    sdf::ElementPtr geometry = collision->GetElement("geometry");
-
-    // Remove existing mesh
-    sdf::ElementPtr mesh = geometry->GetElement("mesh");
-    geometry->RemoveChild(mesh);
-
-    // Overwrite existing pose tag
-    sdf::ElementPtr pose = collision->GetElement("pose");
-    pose->Set(new_box_collision_pose);
-
-    // Add a new box
-    sdf::ElementPtr box = geometry->AddElement("box");
-    sdf::ElementPtr size = box->GetElement("size");
-    size->Set(box_size);
-
-    return true;
-}
-
-bool substituteCollisionWithBoxInSDF(sdf::SDFPtr icub_sdf,
-                                     std::string link_name,
-                                     std::string collision_name,
-                                     std::string new_box_collision_pose,
-                                     std::string box_size)
-{
-    sdf::ElementPtr model_elem = icub_sdf->Root()->GetElement("model");
-
-    for (sdf::ElementPtr link = model_elem->GetElement("link"); link; link = link->GetNextElement("link"))
-    {
-        if( link->GetAttribute("name")->GetAsString() == link_name )
-        {
-            for (sdf::ElementPtr collision = link->GetElement("collision"); collision; collision = link->GetNextElement("collision"))
-            {
-                if( collision->GetAttribute("name")->GetAsString() == collision_name )
-                {
-                    substituteCollisionWithBoxInCollisionSDF(collision,new_box_collision_pose,box_size);
-                }
-            }
-        }
-    }
-
-    return true;
-}
-
-bool generate_model_config_file(std::string robot_name, std::string gazebo_robot_model_directory)
-{
-
-    std::ofstream model_config_file;
-    std::string model_config_file_name = gazebo_robot_model_directory+"model.config";
-    model_config_file.open(model_config_file_name.c_str());
-    model_config_file << "<?xml version=\"1.0\"?>\n<model>\n<name>";
-    model_config_file << robot_name;
-    model_config_file << "</name><version>1.0</version><sdf version='1.4'>icub.sdf</sdf>\n</model>\n";
-    model_config_file.close();
-
-    return true;
-}
-
-std::string get_gazebo_model_directory(std::string root_directory)
-{
-    return root_directory+"gazebo_models/";
-}
-
-bool generate_iCub_urdf_model(std::string iCub_name,
-                              int head_version,
-                               int legs_version,
-                               int feet_version,
-                               bool is_iCubParis02,
-                               bool is_icubGazeboSim,
-                               bool simple_meshes,
-                               std::string data_directory,
-                               double mass_epsilon,
-                               double inertia_epsilon,
-                               bool noFTsimulation,
-                               urdf::ModelInterfaceSharedPtr  urdf_file,
-                               std::string outputfilename
-                             )
-{
-
-    std::cout << "Generating iCub model for " << iCub_name << " (head: " << head_version << " , legs: " << legs_version << " , feet: " << feet_version << " )" << std::endl;
-    if( feet_version == 2 ) { std::cout << "Generating FT sensor in the feet" << std::endl; }
-
-    std::string paris_directory = data_directory+"urdf_paris/";
-
-    std::string paris_subdirectory;
-    if( simple_meshes ) {
-        paris_subdirectory = "icub_simple";
-    } else {
-        paris_subdirectory = "icub";
-    }
-
-    //////////////////////////////////
-    //Generating urdf from iDyn
-    //////////////////////////////////
-    iCub::iDyn::version_tag icub_type;
-
-    icub_type.head_version = head_version;
-    icub_type.legs_version = legs_version;
-
-
-    iCub::iDyn::iCubWholeBody icub_idyn(icub_type);
-
-    KDL::Tree icub_kdl;
-
-    KDL::JntArray dummy1,dummy2;
-    bool addRootWeight = true;
-    bool debugFlag         = false;
-    bool ft_feet = false;
-
-    if( feet_version == 2 )
-    {
-        ft_feet = true;
-    }
-
-    if( ! toKDL(icub_idyn,icub_type,icub_kdl,dummy1,dummy2,iCub::iDynTree::SKINDYNLIB_SERIALIZATION,ft_feet,addRootWeight,debugFlag,is_iCubParis02,is_icubGazeboSim) ) {
-        std::cerr << "Fatal error in iDyn - KDL conversion" << std::endl;
-        return false;
-    }
-
-    urdf::ModelInterfaceSharedPtr  urdf_idyn(new urdf::ModelInterface);
-
-    //std::cout << "iCub KDL::Tree: " << std::endl;
-    //std::cout << icub_kdl << std::endl;
-
-    if( ! iDynTree::treeToUrdfModel(icub_kdl,"iCub",*urdf_idyn) ) {
-        std::cerr << "Fatal error in KDL - URDF conversion" << std::endl;
-        return false;
-    }
-
-
-    //////////////////////////////////////////////////////////////////
-    //// Getting meshes and limits from urdf paris files
-    //////////////////////////////////////////////////////////////////
-    urdf::ModelInterfaceSharedPtr  urdf_paris;
-
-    std::string filename_urdf_paris = paris_directory+paris_subdirectory+"/icub.xml";
-
-
-    std::ifstream t_um(filename_urdf_paris.c_str());
-    std::stringstream buffer_um;
-    buffer_um << t_um.rdbuf();
-    urdf_paris = urdf::parseURDF(buffer_um.str());
-
-    std::cout << "Loading file " << filename_urdf_paris << std::endl;
-
-
-    bool ret_ok = urdf_import_limits(urdf_idyn,urdf_paris);
-
-    if( !ret_ok ) {
-        std::cerr << "Fatal error in importing limits from " << filename_urdf_paris  << std::endl;
-        return false;
-    }
-
-    ret_ok = urdf_import_meshes(urdf_idyn,urdf_paris);
-
-    if( !ret_ok ) {
-        std::cerr << "Fatal error in importing meshes from " << filename_urdf_paris  << std::endl;
-        return false;
-    }
-
-    //Add damping
-    double damping_ie_viscous = 1.0;
-    double friction_ie_coulomb = 0.0;
-    ret_ok = urdf_set_friction_parameters(urdf_idyn,damping_ie_viscous,friction_ie_coulomb);
-
-    if( !ret_ok ) {
-        std::cerr << "Fatal error in setting friction parameters"  << std::endl;
-        return false;
-    }
-
-    /////////////////////////////////////////////////////////////////////////
-    ///// Exporting "normal" urdf file
-    /////////////////////////////////////////////////////////////////////////
-    TiXmlDocument* xml_doc;
-
-    xml_doc = exportURDF(urdf_idyn);
-
-    // Add sensors
-    bool ret=true;
-    ret = ret && addGazeboYarpPluginsFTToURDF(xml_doc,"left_arm");
-    ret = ret && addGazeboYarpPluginsFTToURDF(xml_doc,"right_arm");
-    ret = ret && addGazeboYarpPluginsFTToURDF(xml_doc,"left_leg");
-    ret = ret && addGazeboYarpPluginsFTToURDF(xml_doc,"right_leg");
-    if( ft_feet ) {
-        std::cerr << "Adding feet FT sensors" << std::endl;
-        ret = ret && addGazeboYarpPluginsFTToURDF(xml_doc,"left_foot");
-        ret = ret && addGazeboYarpPluginsFTToURDF(xml_doc,"right_foot");
-    }
-
-    ret = ret && add_iDynTreeFTToURDF(xml_doc,"left_arm");
-    ret = ret && add_iDynTreeFTToURDF(xml_doc,"right_arm");
-    ret = ret && add_iDynTreeFTToURDF(xml_doc,"left_leg");
-    ret = ret && add_iDynTreeFTToURDF(xml_doc,"right_leg");
-    if( ft_feet ) {
-        ret = ret && add_iDynTreeFTToURDF(xml_doc,"left_foot");
-        ret = ret && add_iDynTreeFTToURDF(xml_doc,"right_foot");
-    }
-
-    if( !ret ) { std::cerr << "Problem in adding ft sensors to the URDF file" << std::endl; return false; }
-
-    if( ! xml_doc->SaveFile(outputfilename) ) {
-        std::cerr << "Fatal error in URDF xml saving filename " << outputfilename  << std::endl;
-        return false;
-    }
-
-    urdf_file = urdf_idyn;
-
-    return true;
-}
-
-bool generate_iCub_sdf_model(std::string iCub_name,
-                              int head_version,
-                               int legs_version,
-                               int feet_version,
-                               bool is_iCubParis02,
-                               bool is_icubGazeboSim,
-                               bool simple_meshes,
-                               std::string data_directory,
-                               double mass_epsilon,
-                               double inertia_epsilon,
-                               bool noFTsimulation,
-                               urdf::ModelInterfaceSharedPtr  urdf_idyn,
-                               std::string outputfilename
-                            )
-{
-    std::string gazebo_mesh_model_name = "icub";
-    std::string gazebo_uri_prefix = "model://icub/";
-    std::string gazebo_sdf_filename = outputfilename;
-
-    //////////////////////////////////////////////////////////////////////////
-    ///// Creating gazebo file
-    /////////////////////////////////////////////////////////////////////////
-    if( ! urdf_gazebo_cleanup_regularize_masses(urdf_idyn,mass_epsilon,inertia_epsilon) ) { std::cerr << "Error in regularizing masses root " << std::endl; return false; }
-
-    //////////////////////////////////////////////////////////////////////////
-    ////// Creating URDF file coherent with gazebo workarounds for use in iDynTree
-    ///////////////////////////////////////////////////////////////////////
-
-    /*
-    TiXmlDocument * xml_doc = exportURDF(urdf_idyn);
-
-    if( ! xml_doc->SaveFile(outputfilename) ) {
-        std::cerr << "Fatal error in URDF xml saving" << std::endl;
-        return false;
-    }*/
-
-    if( ! urdf_gazebo_cleanup_remove_massless_root(urdf_idyn) ) { std::cerr << "Error in removing massless root " << std::endl; return false; }
-    //printTree(urdf_idyn->getRoot());
-    //printJoints(urdf_idyn);
-    if( ! urdf_gazebo_cleanup_remove_frames(urdf_idyn) ) { std::cerr << "Error in removing frames " << std::endl; return false; }
-
-    if( ! noFTsimulation )
-    {
-        if( ! urdf_gazebo_cleanup_transform_FT_sensors(urdf_idyn) ) { std::cerr << "Error in transforming FT junctions " << std::endl; return false; }
-    }
-
-    if( ! urdf_gazebo_cleanup_regularize_masses(urdf_idyn,mass_epsilon,inertia_epsilon) ) { std::cerr << "Error in regularizing masses root " << std::endl; return false; }
-    if( ! urdf_gazebo_cleanup_add_model_uri(urdf_idyn,gazebo_uri_prefix) ) { std::cerr << "Error in adding model URIs " << std::endl; return false; }
-
-    TiXmlDocument * xml_doc = xml_doc = exportURDF(urdf_idyn);
-
-    /*
-    if( ! xml_doc->SaveFile(filename_urdf_gazebo_conversion) ) {
-        std::cerr << "Fatal error in URDF xml saving" << std::endl;
-        return false;
-    }*/
-
-    std::stringstream ss;
-    ss << *xml_doc;
-    std::string urdf_for_gazebo_conversion_string = ss.str();
-
-    //Ugly workaround, I know, probably can be avoided by directly using sdfformat library
-    //std::string gazebo_conversion_command = "gzsdf print " + filename_urdf_gazebo_conversion + " > " + gazebo_sdf_filename;
-    //std::cout << "Running command: " << gazebo_conversion_command << std::endl;
-
-    //system(gazebo_conversion_command.c_str());
-    //if( !system(gazebo_conversion_command.c_str()) ) { std::cerr << "Error in urdf - sdf conversion" << std::endl; return false; }
-
-    //\todo: adding plugins to generated sdf
-    sdf::SDFPtr icub_sdf(new sdf::SDF());
-    sdf::init(icub_sdf);
-
-    if( ! sdf::readString(urdf_for_gazebo_conversion_string,icub_sdf) ) { std::cerr << "Problem in reading SDF file" << std::endl; return false; }
-
-    if( ! icub_sdf->Root()->HasElement("model") ) { std::cerr << "Problem in parsing SDF dom" << std::endl; return false; }
-
-    // Substitute foot collisions with a box
-    if( ! noFTsimulation )
-    {
-        substituteCollisionWithBoxInSDF(icub_sdf,"l_foot","l_foot_collision","0.03 0 -0.01 0 0 0","0.15 0.07 0.03");
-        substituteCollisionWithBoxInSDF(icub_sdf,"r_foot","r_foot_collision","0.03 0 -0.01 0 0 0","0.15 0.07 0.03");
-    }
-    else
-    {
-        substituteCollisionWithBoxInSDF(icub_sdf,"l_ankle_2","l_ankle_2_collision","0.03 0 0.026 0 -0 0","0.15 0.07 0.03");
-        substituteCollisionWithBoxInSDF(icub_sdf,"r_ankle_2","r_ankle_2_collision","0.03 0 0.026 0 -0 0","0.15 0.07 0.03");
-    }
-
-    //Adding IMU sensor (in all robots)
-    /// \todo add difference between head V1 and head V2
-    if( ! addGazeboYarpPluginsIMUToSDF(icub_sdf,"imu_sensor",iCub_name) ) { std::cerr << "Problem in adding imu sensor" << std::endl; return false; }
-
-    //Adding FT sensors (in all robots)
-    bool ret=true;
-    ret = ret && addGazeboYarpPluginsFTToSDF(icub_sdf,"left_arm",iCub_name);
-    ret = ret && addGazeboYarpPluginsFTToSDF(icub_sdf,"right_arm",iCub_name);
-    ret = ret && addGazeboYarpPluginsFTToSDF(icub_sdf,"left_leg",iCub_name);
-    ret = ret && addGazeboYarpPluginsFTToSDF(icub_sdf,"right_leg",iCub_name);
-    if( feet_version == 2 ) {
-        std::cerr << "Adding feet FT sensors" << std::endl;
-        ret = ret && addGazeboYarpPluginsFTToSDF(icub_sdf,"left_foot",iCub_name);
-        ret = ret && addGazeboYarpPluginsFTToSDF(icub_sdf,"right_foot",iCub_name);
-    }
-    if( !ret ) { std::cerr << "Problem in adding ft sensors" << std::endl; return false; }
-
-    //Adding controlboards (in all robots, in the future add differences)
-    ret = ret && addGazeboYarpPluginsControlboardToSDF(icub_sdf,"torso",iCub_name);
-    if( !ret ) { std::cerr << "Problem in adding torso controlboard" << std::endl; return false; }
-    ret = ret && addGazeboYarpPluginsControlboardToSDF(icub_sdf,"head",iCub_name);
-    if( !ret ) { std::cerr << "Problem in adding head controlboard" << std::endl; return false; }
-    ret = ret && addGazeboYarpPluginsControlboardToSDF(icub_sdf,"left_arm",iCub_name);
-    if( !ret ) { std::cerr << "Problem in adding left_arm controlboards" << std::endl; return false; }
-    ret = ret && addGazeboYarpPluginsControlboardToSDF(icub_sdf,"right_arm",iCub_name);
-    if( !ret ) { std::cerr << "Problem in adding right_arm controlboards" << std::endl; return false; }
-    ret = ret && addGazeboYarpPluginsControlboardToSDF(icub_sdf,"left_leg",iCub_name);
-    if( !ret ) { std::cerr << "Problem in adding left_leg controlboards" << std::endl; return false; }
-    ret = ret && addGazeboYarpPluginsControlboardToSDF(icub_sdf,"right_leg",iCub_name);
-    if( !ret ) { std::cerr << "Problem in adding right_leg controlboards" << std::endl; return false; }
-
-    //Adding parameters for properly handling of the contacts
-    ret = ret && addGazeboSurfaceFrictionInformationToSDF(icub_sdf,"l_foot");
-    if( !ret ) { std::cerr << "Problem in adding contact properties" << std::endl; return false; }
-    ret = ret && addGazeboSurfaceFrictionInformationToSDF(icub_sdf,"r_foot");
-    if( !ret ) { std::cerr << "Problem in adding contact properties" << std::endl; return false; }
-
-    //Adding pose to avoid intersection with ground
-    //<pose>0 0 0.70 0 0 0 </pose>
-    sdf::ElementPtr pose = icub_sdf->Root()->GetElement("model")->AddElement("pose");
-    pose->AddValue("string","0 0 0.70 0 0 0",false);
-
-    icub_sdf->Write(gazebo_sdf_filename);
-
-    return true;
-}
-
-
-/**
- * Generate iCub URDF and SDF models, starting from UPMC meshes and kinematics/dynamics information from iDyn
- *
- * @param head_version can be 1 or 2
- * @param legs_version can be 1 or 2
- * @param feet_version can be 1 or 2
- * @param simple_meshes if true uses the simple visualization meshes, if false uses the heavy detailed one
- */
-/*
-bool generate_iCub_model(std::string iCub_name,
-                         int head_version,
-                         int legs_version,
-                         int feet_version,
-                         bool is_iCubParis02,
-                         bool is_icubGazeboSim,
-                         bool simple_meshes,
-                         std::string data_directory,
-                         double mass_epsilon,
-                         double inertia_epsilon,
-                         bool noFTsimulation)
-{
-    bool ok = true;
-    urdf::ModelInterfaceSharedPtr  urdf_file;
-    ok = ok && generate_iCub_urdf_model(iCub_name,head_version,legs_version,feet_version,is_iCubParis02,is_icubGazeboSim,simple_meshes,data_directory,mass_epsilon,inertia_epsilon,noFTsimulation,urdf_file,root_directory+"/icub.urdf");
-    ok = ok && generate_iCub_sdf_model(iCub_name,head_version,legs_version,feet_version,is_iCubParis02,is_icubGazeboSim,simple_meshes,data_directory,mass_epsilon,inertia_epsilon,noFTsimulation,urdf_file);
-    return true;
-}*/
-
-
-bool generate_gazebo_database(const std::vector<std::string> & robot_names, const std::string root_directory)
-{
-    std::string gazebo_model_directory = get_gazebo_model_directory(root_directory);
-
-    std::ofstream database_file;
-    std::string database_file_name = gazebo_model_directory+"database.config";
-
-    std::cerr << "generating gazebo database at : " << std::endl;;
-
-
-    database_file.open(database_file_name.c_str());
-    database_file << "<?xml version='1.0'?>\n<database>\n";
-    database_file << "<name>icub-model-generator Database</name>\n";
-    database_file << "<models>";
-    for(int i=0; i < robot_names.size(); i++ )
-    {
-        database_file << "<uri>file://"+robot_names[i]+"</uri>\n";
-    }
-
-    database_file << "</models>\n</database>\n" << std::endl;
-    database_file.close();
-
-    return true;
-}
diff --git a/dh/generator/urdf_sdf_from_dh_utils.h b/dh/generator/urdf_sdf_from_dh_utils.h
deleted file mode 100644
index 16d9d0b..0000000
--- a/dh/generator/urdf_sdf_from_dh_utils.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/**
- * Copyright  (C) 2013-2014 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#ifndef URDF_SDF_FROM_DH_UTILS
-#define URDF_SDF_FROM_DH_UTILS
-
-
-#include <iCub/iDynTree/idyn2kdl_icub.h>
-
-#include <iostream>
-#include <fstream>
-#include <cstdlib>
-
-#include <iCub/iDyn/iDyn.h>
-#include <iCub/iDyn/iDynBody.h>
-
-#include <kdl/tree.hpp>
-
-#include <iDynTree/ModelIO/impl/urdf_export.hpp>
-
-#include <urdf_model/model.h>
-#include <urdf_parser/urdf_parser.h>
-
-#include <tinyxml.h>
-#include <yarp/os/Property.h>
-#include <yarp/os/Os.h>
-
-
-#include <sdf/parser.hh>
-#include <sdf/parser_urdf.hh>
-
-#include "urdf_utils.h"
-
-#define NAME "icub_urdf_sdf_generator"
-
-#define ERROR_FT_SENSOR_JOINT "__error_ft_sensor_joint__"
-
-
-bool addGazeboYarpPluginsControlboardToSDF(sdf::ElementPtr model, std::string part_name, std::string robot_name);
-
-/**
- *
- * @param part: head,torso,right_arm,left_arm,right_leg,left_leg
- */
-bool addGazeboYarpPluginsControlboardToSDF(sdf::SDFPtr icub_sdf, std::string part_name, std::string robot_name);
-
-/**
- *
- * @param sensor: can be only imu_sensor
- */
-bool addGazeboYarpPluginsIMUToSDF(sdf::SDFPtr icub_sdf, std::string sensor_name, std::string robot_name);
-
-/**
- *
- * @param sensor_name: left_arm,right_arm,left_leg,right_leg,left_foot,right_foot
- */
-std::string getFTJointName(const std::string sensor_name);
-
-/**
- * Add the force/torque sensor description to the URDF (using gazebo extentions)
- * @param sensor_name: left_arm,right_arm,left_leg,right_leg,left_foot,right_foot
- */
-bool addGazeboYarpPluginsFTToURDF(TiXmlDocument* urdf_doc, std::string sensor_name);
-
-/**
- * Add the force/torque sensor description to the URDF (using iDynTree extentions, documented
- * in https://github.com/robotology/idyntree/blob/master/doc/model_loading.md).
- *
- * @param sensor_name: left_arm,right_arm,left_leg,right_leg,left_foot,right_foot
- */
-bool add_iDynTreeFTToURDF(TiXmlDocument* urdf_doc, std::string sensor_name);
-
-void AddElementAndSetValue(sdf::ElementPtr parent, std::string childType, std::string value_type, std::string value);
-
-bool addGazeboSurfaceFrictionInformationToCollisionSDF(sdf::ElementPtr collision_sdf);
-
-/**
- * For a given link add surface friction informations to the SDF
- *
- */
-bool addGazeboSurfaceFrictionInformationToSDF(sdf::SDFPtr icub_sdf, std::string link_name);
-
-bool addGazeboYarpPluginsFTToSDF(sdf::ElementPtr joint_element, std::string sensor_name, std::string robot_name);
-
-/**
- * Add the force/torque sensor description (with the relative gazebo_yarp_plugin)
- * @param sensor_name: left_arm,right_arm,left_leg,right_leg,left_foot,right_foot
- */
-bool addGazeboYarpPluginsFTToSDF(sdf::SDFPtr icub_sdf, std::string sensor_name, std::string robot_name);
-
-bool addGazeboODEContactsProperty(sdf::ElementPtr collision_elem);
-bool addGazeboODEContactsProperties(sdf::SDFPtr icub_sdf, std::string link_name, std::string collision_name);
-
-bool substituteCollisionWithBoxInCollisionSDF(sdf::ElementPtr collision,
-                                     std::string new_box_collision_pose,
-                                     std::string box_size);
-
-bool substituteCollisionWithBoxInSDF(sdf::SDFPtr icub_sdf,
-                                     std::string link_name,
-                                     std::string collision_name,
-                                     std::string new_box_collision_pose,
-                                     std::string box_size);
-
-bool generate_model_config_file(std::string robot_name, std::string gazebo_robot_model_directory);
-
-std::string get_gazebo_model_directory(std::string root_directory);
-
-bool generate_iCub_urdf_model(std::string iCub_name,
-                              int head_version,
-                               int legs_version,
-                               int feet_version,
-                               bool is_iCubParis02,
-                               bool is_icubGazeboSim,
-                               bool simple_meshes,
-                               std::string data_directory,
-                               double mass_epsilon,
-                               double inertia_epsilon,
-                               bool noFTsimulation,
-                               urdf::ModelInterfaceSharedPtr  urdf_file,
-                               std::string outputfile );
-
-bool generate_iCub_sdf_model(std::string iCub_name,
-                              int head_version,
-                               int legs_version,
-                               int feet_version,
-                               bool is_iCubParis02,
-                               bool is_icubGazeboSim,
-                               bool simple_meshes,
-                               std::string data_directory,
-                               double mass_epsilon,
-                               double inertia_epsilon,
-                               bool noFTsimulation,
-                               urdf::ModelInterfaceSharedPtr  urdf_file);
-
-/**
- * Generate iCub URDF and SDF models, starting from UPMC meshes and kinematics/dynamics information from iDyn
- *
- * @param head_version can be 1 or 2
- * @param legs_version can be 1 or 2
- * @param feet_version can be 1 or 2
- * @param simple_meshes if true uses the simple visualization meshes, if false uses the heavy detailed one
- */
-/*
-bool generate_iCub_model(std::string iCub_name,
-                         std::string root_directory,
-                         int head_version,
-                         int legs_version,
-                         int feet_version,
-                         bool is_iCubParis02,
-                         bool is_icubGazeboSim,
-                         bool simple_meshes,
-                         std::string data_directory,
-                         double mass_epsilon,
-                         double inertia_epsilon,
-                         bool noFTsimulation = false
-                        );*/
-
-bool generate_gazebo_database(const std::vector<std::string> & robot_names, const std::string root_directory);
-
-#endif
diff --git a/dh/generator/urdf_utils.cpp b/dh/generator/urdf_utils.cpp
deleted file mode 100644
index 7991040..0000000
--- a/dh/generator/urdf_utils.cpp
+++ /dev/null
@@ -1,492 +0,0 @@
-/**
- * Copyright  (C) 2013-2014 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#include "urdf_utils.h"
-
-#include <urdf_model/model.h>
-
-#include <kdl/tree.hpp>
-
-#include <kdl/treefksolverpos_recursive.hpp>
-#include <kdl/frames_io.hpp>
-
-#include <iDynTree/ModelIO/impl/urdf_import.hpp>
-
-///< \todo TODO add support for deleting a long chain of null link connected by fixed base
-bool deleteLink(urdf::ModelInterfaceSharedPtr  urdf_input, std::string link_to_delete, bool verbose=false)
-{
-    if( link_to_delete != urdf_input->getRoot()->name ) {
-        //deleting normal link
-
-        //delete links (remove from global and from parent (also for joint)
-        urdf::LinkSharedPtr link_sptr = urdf_input->links_[link_to_delete];
-        if( link_sptr->child_links.size() != 0 ) { std::cerr << "deleteLink error: tryng to delete a link with a child" << std::endl; return false;}
-        urdf::JointSharedPtr joint_sptr = link_sptr->parent_joint;
-        urdf::LinkSharedPtr parent_link = link_sptr->getParent();
-
-        //remove links from cparent_link->child of parent
-        for(int i=0; i < parent_link->child_links.size(); i++ ) {
-            if( parent_link->child_links[i]->name == link_sptr->name ) {
-                parent_link->child_links.erase( parent_link->child_links.begin()+i);
-                parent_link->child_joints.erase( parent_link->child_joints.begin()+i);
-            }
-        }
-
-        if( verbose ) {
-            std::cout << "deleteLink: removing link  " << link_sptr->name << std::endl;
-            std::cout << "deleteLink: removing joint " << joint_sptr->name << std::endl;
-        }
-        int deleted = 0;
-        deleted = urdf_input->links_.erase(link_sptr->name);
-        if( deleted != 1 ) { if( verbose ) { std::cout << "deleteLink error joint: Link not found" << std::endl; } return false; }
-        deleted = urdf_input->joints_.erase(link_sptr->name);
-        if( deleted != 1 ) { if( verbose ) { std::cout << "deleteLink error joint: Link not found" << std::endl; } return false; }
-
-    } else {
-        //deleting root link
-
-        urdf::LinkSharedPtr link_sptr = urdf_input->links_[link_to_delete];
-
-        if( link_sptr->child_links.size() != 1 ) { std::cerr << "deleteLink error: tryng to delete a root link with more than a child" << std::endl; return false; }
-
-        urdf::JointSharedPtr joint_sptr = link_sptr->child_joints[0];
-
-        urdf::LinkSharedPtr new_root = link_sptr->child_links[0];
-
-        urdf_input->root_link_ = new_root;
-
-        urdf_input->links_.erase(link_sptr->name);
-        urdf_input->joints_.erase(new_root->name);
-
-    }
-
-    return true;
-
-}
-
-bool deleteLinks(urdf::ModelInterfaceSharedPtr  urdf_input, std::vector<std::string> linksToDelete, bool verbose=false)
-{
-    for(int i=0; i < linksToDelete.size(); i++ ) {
-        if( !deleteLink(urdf_input,linksToDelete[i],verbose) ) return false;
-    }
-    return true;
-}
-
-// construct vector
-urdf::Vector3 toUrdf(const KDL::Vector & v)
-{
-    return urdf::Vector3(v.x(), v.y(), v.z());
-}
-
-// construct rotation
-urdf::Rotation toUrdf(const KDL::Rotation & r)
-{
-    double x,y,z,w;
-    r.GetQuaternion(x,y,z,w);
-    return urdf::Rotation(x,y,z,w);
-}
-
-// construct pose
-urdf::Pose toUrdf(const KDL::Frame & p)
-{
-    urdf::Pose ret;
-    ret.rotation = toUrdf(p.M);
-    ret.position = toUrdf(p.p);
-    return ret;
-}
-
-// construct vector
-KDL::Vector toKdl(urdf::Vector3 v)
-{
-  return KDL::Vector(v.x, v.y, v.z);
-}
-
-// construct rotation
-KDL::Rotation toKdl(urdf::Rotation r)
-{
-  return KDL::Rotation::Quaternion(r.x, r.y, r.z, r.w);
-}
-
-// construct pose
-KDL::Frame toKdl(urdf::Pose p)
-{
-  return KDL::Frame(toKdl(p.rotation), toKdl(p.position));
-}
-
-double getTotalMass(urdf::ModelInterface & model)
-{
-    std::vector<urdf::LinkSharedPtr > input_links;
-
-    model.getLinks(input_links);
-
-    double total_mass = 0.0;
-
-    std::cout << "Found " << input_links.size() << " links in input URDF " << std::endl;
-    for(int i=0; i < input_links.size(); i++ )
-    {
-        if( input_links[i]->inertial ) {
-            total_mass = total_mass + input_links[i]->inertial->mass;
-        }
-    }
-
-    return total_mass;
-}
-
-bool urdf_import_meshes(urdf::ModelInterfaceSharedPtr urdf_input,
-                        urdf::ModelInterfaceSharedPtr urdf_meshes,
-                        bool verbose)
-{
-    bool ret = true;
-    KDL::Tree kdl_meshes, kdl_input;
-    ret = ret && iDynTree::treeFromUrdfModel(*urdf_meshes,kdl_meshes,false);
-    ret = ret && iDynTree::treeFromUrdfModel(*urdf_input,kdl_input,false);
-    if( !ret ) {
-        std::cerr << "Fatal error in URDF KDL conversion saving" << std::endl;
-        return false;
-    }
-
-    if(kdl_meshes.getNrOfJoints() != kdl_input.getNrOfJoints())
-    {
-        std::cerr << "Fatal error, the two models have a different number of DOFs" << std::endl;
-        return false;
-    }
-
-    KDL::JntArray q(kdl_meshes.getNrOfJoints());
-    SetToZero(q);
-
-    KDL::TreeFkSolverPos_recursive meshes_solver(kdl_meshes);
-    KDL::TreeFkSolverPos_recursive input_solver(kdl_input);
-
-    //Iterate over the links of urdf_input, and copy the mesh
-    std::vector<urdf::LinkSharedPtr > input_links;
-
-    urdf_input->getLinks(input_links);
-
-    if( verbose ) std::cout << "Found " << input_links.size() << " links in input URDF " << std::endl;
-    for(int i=0; i < input_links.size(); i++ )
-    {
-        KDL::Frame base_link_meshes, base_link_input;
-        KDL::Frame link_visual_old, link_collision_old;
-        KDL::Frame link_visual_new, link_collision_new;
-        std::string input_name = input_links[i]->name;
-
-        if( verbose ) std::cout << "Processing link " << input_name << std::endl;
-
-        urdf::LinkConstSharedPtr mesh_link_ptr = urdf_meshes->getLink(input_name);
-
-
-        if( mesh_link_ptr ) {
-            if( verbose ) std::cout << "Tryng to copy meshes of link " << input_name << std::endl;
-            //if there is a link with the same name, copy the mesh
-            //supporting only single mesh
-            if( mesh_link_ptr->collision || mesh_link_ptr->visual ) {
-                int retur;
-                retur = meshes_solver.JntToCart(q,base_link_meshes,input_name);
-                if( retur < 0 ) { return false; }
-                retur = input_solver.JntToCart(q,base_link_input,input_name);
-                if( retur < 0 ) { return false; }
-            }
-            if( mesh_link_ptr->collision ) {
-                link_collision_old = toKdl(mesh_link_ptr->collision->origin);
-                link_collision_new = base_link_input.Inverse()*base_link_meshes*link_collision_old;
-
-                if( verbose ) std::cout << "Old collision origin " << link_collision_old << std::endl;
-                if( verbose ) std::cout << "New collision origin " << link_collision_new << std::endl;
-
-
-                input_links[i]->collision.reset(new urdf::Collision);
-
-                input_links[i]->collision->geometry.reset(new urdf::Geometry);
-
-                input_links[i]->collision->origin = toUrdf(link_collision_new);
-
-                switch( mesh_link_ptr->collision->geometry->type ) {
-                    case urdf::Geometry::SPHERE:
-                        input_links[i]->collision->geometry.reset(new urdf::Sphere);
-                        *(input_links[i]->collision->geometry) = *(mesh_link_ptr->collision->geometry);
-                    break;
-                    case urdf::Geometry::BOX:
-                        input_links[i]->collision->geometry.reset(new urdf::Box);
-                        *(input_links[i]->collision->geometry) = *(mesh_link_ptr->collision->geometry);
-                    break;
-                    case urdf::Geometry::CYLINDER:
-                        input_links[i]->collision->geometry.reset(new urdf::Cylinder);
-                        *(input_links[i]->collision->geometry) = *(mesh_link_ptr->collision->geometry);
-                    break;
-                    case urdf::Geometry::MESH:
-                        input_links[i]->collision->geometry.reset(new urdf::Mesh);
-                        if( verbose ) std::cout << "Copyng collision mesh with filename " << (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->filename << std::endl;
-                        (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->collision->geometry))->filename = (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->filename;
-                        (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->collision->geometry))->scale = (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->collision->geometry))->scale;
-                    break;
-                }
-
-                input_links[i]->collision_array.push_back(input_links[i]->collision);
-
-            }
-            if( mesh_link_ptr->visual ) {
-                link_visual_old = toKdl(mesh_link_ptr->visual->origin);
-                link_visual_new = base_link_input.Inverse()*base_link_meshes*link_visual_old;
-
-                input_links[i]->visual.reset(new urdf::Visual);
-
-                input_links[i]->visual->origin = toUrdf(link_visual_new);
-
-                input_links[i]->visual->material_name = mesh_link_ptr->visual->material_name;
-
-                input_links[i]->visual->material.reset(new urdf::Material);
-
-                *(input_links[i]->visual->material) = *(mesh_link_ptr->visual->material);
-
-                input_links[i]->visual->geometry.reset(new urdf::Geometry);
-
-                switch( mesh_link_ptr->visual->geometry->type ) {
-                    case urdf::Geometry::SPHERE:
-                        input_links[i]->visual->geometry.reset(new urdf::Sphere);
-                        *(input_links[i]->visual->geometry) = *(mesh_link_ptr->visual->geometry);
-                    break;
-                    case urdf::Geometry::BOX:
-                        input_links[i]->visual->geometry.reset(new urdf::Box);
-                        *(input_links[i]->visual->geometry) = *(mesh_link_ptr->visual->geometry);
-                    break;
-                    case urdf::Geometry::CYLINDER:
-                        input_links[i]->visual->geometry.reset(new urdf::Cylinder);
-                        *(input_links[i]->visual->geometry) = *(mesh_link_ptr->visual->geometry);
-                    break;
-                    case urdf::Geometry::MESH:
-                        input_links[i]->visual->geometry.reset(new urdf::Mesh);
-                        if( verbose ) std::cout << "Copyng visual mesh with filename " << (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->filename << std::endl;
-                        (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->visual->geometry))->filename = (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->filename;
-                        (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->visual->geometry))->scale = (urdf::static_pointer_cast<urdf::Mesh>(mesh_link_ptr->visual->geometry))->scale;
-                    break;
-                }
-
-                input_links[i]->visual_array.push_back(input_links[i]->visual);
-
-
-            }
-        } else {
-            std::cout << "No link found with name " << input_name << std::endl;
-        }
-    }
-    return true;
-}
-
-bool urdf_import_limits(urdf::ModelInterfaceSharedPtr urdf_input, urdf::ModelInterfaceSharedPtr urdf_limits)
-{
-    //Iterate over the joints of urdf_input, and copy the limits
-    for( std::map<std::string, urdf::JointSharedPtr >::iterator it = urdf_input->joints_.begin();
-         it != urdf_input->joints_.end(); it++ )
-    {
-        std::string joint_name = it->second->name;
-        std::string link_name = it->first;
-        std::cout << "urdf_import_limits: Processing joint " << joint_name << " relative to link " << link_name << std::endl;
-
-        urdf::JointConstSharedPtr limits_joints_ptr = urdf_limits->getJoint(joint_name);
-
-
-        if( limits_joints_ptr ) {
-            std::cout << "urdf_import_limits: Tryng to copy limits of joint " << joint_name << std::endl;
-
-            it->second->limits.reset(new urdf::JointLimits);
-            *(it->second->limits) = *(limits_joints_ptr->limits);
-
-            //Copy also the type to switch from continuous to revolute
-            it->second->type = limits_joints_ptr->type;
-
-        } else {
-            std::cout << "urdf_import_limits: No joint found with link name " << link_name << std::endl;
-        }
-    }
-    return true;
-}
-
-bool urdf_set_friction_parameters(urdf::ModelInterfaceSharedPtr urdf_input, double damping, double friction)
-{
-    //Iterate over the joints of urdf_input, and copy the limits
-    for( std::map<std::string, urdf::JointSharedPtr >::iterator it = urdf_input->joints_.begin();
-         it != urdf_input->joints_.end(); it++ )
-    {
-        if( it->second->type == urdf::Joint::REVOLUTE ||
-            it->second->type == urdf::Joint::CONTINUOUS )
-        {
-            it->second->dynamics.reset(new urdf::JointDynamics);
-            it->second->dynamics->damping = damping;
-            it->second->dynamics->friction = friction;
-        }
-    }
-
-    return true;
-}
-
-bool urdf_print_hom_transformations(urdf::ModelInterfaceSharedPtr urdf_input)
-{
-    //Iterate over the joints of urdf_input, and print the transformation matrix
-    for( std::map<std::string, urdf::JointSharedPtr >::iterator it = urdf_input->joints_.begin();
-         it != urdf_input->joints_.end(); it++ )
-    {
-        std::string joint_name = it->second->name;
-        std::string father_link_name = it->second->parent_link_name;
-        std::string child_link_name = it->second->child_link_name;
-        std::cout << "Transformation matrix of frame of link " << child_link_name <<  " expressed in the frame of link " << father_link_name << " : " << std::endl;
-        std::cout << toKdl(it->second->parent_to_joint_origin_transform) << std::endl;
-        std::cout << "Quaternion: " << it->second->parent_to_joint_origin_transform.rotation.x
-                  << " " << it->second->parent_to_joint_origin_transform.rotation.x
-                  << " " << it->second->parent_to_joint_origin_transform.rotation.y
-                  << " " << it->second->parent_to_joint_origin_transform.rotation.z
-                  << " " << it->second->parent_to_joint_origin_transform.rotation.w << std::endl;
-        std::cout << "Axis of joint " << joint_name << " : " << toKdl(it->second->axis) << std::endl;
-
-    }
-    return true;
-}
-
-bool urdf_gazebo_cleanup_remove_massless_root(urdf::ModelInterfaceSharedPtr urdf_input)
-{
-    if( !(urdf_input->getRoot()->inertial) ) {
-            if( !deleteLink(urdf_input,urdf_input->getRoot()->name) ) return EXIT_FAILURE;
-    }
-    return true;
-}
-
-bool urdf_gazebo_cleanup_remove_frames(urdf::ModelInterfaceSharedPtr urdf_input)
-{
-    std::vector<std::string> linksToDelete;
-    std::vector<urdf::LinkSharedPtr > input_links;
-
-    input_links.clear();
-    urdf_input->getLinks(input_links);
-
-    std::cout << "urdf_gazebo_cleanup: : Found " << input_links.size() << " links in input URDF " << std::endl;
-    for(int i=0; i < input_links.size(); i++ )
-    {
-        //Rule 1
-        double mass;
-        double Ixx;
-        double Iyy;
-        double Izz;
-
-        if( input_links[i]->inertial ) {
-            mass = input_links[i]->inertial->mass;
-            Ixx = input_links[i]->inertial->ixx;
-            Iyy = input_links[i]->inertial->iyy;
-            Izz = input_links[i]->inertial->izz;
-        }
-
-        int nrOfChildrens = input_links[i]->child_links.size();
-
-        if( nrOfChildrens == 0 && mass == 0 && Ixx == 0.0 & Iyy == 0.0 && Izz == 0.0 && input_links[i]->parent_joint->type == urdf::Joint::FIXED ) {
-            linksToDelete.push_back(input_links[i]->name);
-        }
-    }
-
-    if( !deleteLinks(urdf_input,linksToDelete,true) ) return false;
-    linksToDelete.resize(0);
-    return true;
-}
-
-
-bool hasEnding (std::string const &fullString, std::string const &ending)
-{
-    if (fullString.length() >= ending.length()) {
-        return (0 == fullString.compare (fullString.length() - ending.length(), ending.length(), ending));
-    } else {
-        return false;
-    }
-}
-
-bool urdf_gazebo_cleanup_transform_FT_sensors(urdf::ModelInterfaceSharedPtr urdf_input)
-{
-    std::vector<urdf::LinkSharedPtr > input_links;
-    input_links.clear();
-
-    urdf_input->getLinks(input_links);
-
-    for(int i=0; i < input_links.size(); i++ )
-    {
-        //Rule 2
-        int nrOfChildrens = input_links[i]->child_links.size();
-
-        std::string joint_name = input_links[i]->parent_joint->name;
-
-        std::string ft_sensor_joint_suffix = "_ft_sensor";
-
-        if( hasEnding(joint_name,ft_sensor_joint_suffix) ) {
-            input_links[i]->parent_joint->type = urdf::Joint::REVOLUTE;
-            if( input_links[i]->parent_joint->limits ) {
-                input_links[i]->parent_joint->limits->clear();
-            } else {
-                input_links[i]->parent_joint->limits.reset(new urdf::JointLimits());
-            }
-        }
-
-    }
-
-    return true;
-}
-
-bool urdf_gazebo_cleanup_regularize_masses(urdf::ModelInterfaceSharedPtr urdf_input, double mass_epsilon, double inertia_epsilon)
-{
-    std::vector<urdf::LinkSharedPtr > input_links;
-    input_links.clear();
-    urdf_input->getLinks(input_links);
-    for(int i=0; i < input_links.size(); i++ )
-    {
-     //Rule 3
-        if( !(input_links[i]->parent_joint) ) {
-            //If the base_link was not removed, avoid adding masses
-            continue;
-        }
-
-        int nrOfChildrens = input_links[i]->child_links.size();
-
-        if( input_links[i]->parent_joint->type != urdf::Joint::FIXED || nrOfChildrens > 0 ) {
-
-            std::cerr << "Corrected mass for link " << input_links[i]->name << std::endl;
-
-            if( input_links[i]->inertial->mass <= mass_epsilon ) {
-                input_links[i]->inertial->mass = mass_epsilon;
-            }
-            if( input_links[i]->inertial->ixx <= inertia_epsilon ) {
-                input_links[i]->inertial->ixx = inertia_epsilon;
-            }
-            if( input_links[i]->inertial->iyy <= inertia_epsilon ) {
-                input_links[i]->inertial->iyy = inertia_epsilon;
-            }
-            if( input_links[i]->inertial->izz <= inertia_epsilon ) {
-                input_links[i]->inertial->izz = inertia_epsilon;
-            }
-        }
-    }
-    return true;
-}
-
-
-bool urdf_gazebo_cleanup_add_model_uri(urdf::ModelInterfaceSharedPtr urdf_input,std::string rule4_prefix)
-{
-    std::vector<urdf::LinkSharedPtr > input_links;
-    input_links.clear();
-    urdf_input->getLinks(input_links);
-
-    for(int i=0; i < input_links.size(); i++ )
-    {
-        //Rule 4
-        for(int j=0; j < input_links[i]->visual_array.size(); j++ ) {
-            if( input_links[i]->visual_array[j]->geometry->type == urdf::Geometry::MESH ) {
-                (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->visual_array[j]->geometry))->filename = rule4_prefix + (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->visual_array[j]->geometry))->filename;
-            }
-        }
-
-        for(int j=0; j < input_links[i]->collision_array.size(); j++ ) {
-            if( input_links[i]->collision_array[j]->geometry->type == urdf::Geometry::MESH ) {
-                (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->collision_array[j]->geometry))->filename = rule4_prefix + (urdf::static_pointer_cast<urdf::Mesh>(input_links[i]->collision_array[j]->geometry))->filename;
-            }
-        }
-        std::cout << "urdf_gazebo_cleanup: Rule 4 applied successfully with prefix " <<  rule4_prefix << " on link " << input_links[i]->name << " ( i : " << i << " ) "  << std::endl;
-    }
-
-    return true;
-}
diff --git a/dh/generator/urdf_utils.h b/dh/generator/urdf_utils.h
deleted file mode 100644
index a933658..0000000
--- a/dh/generator/urdf_utils.h
+++ /dev/null
@@ -1,87 +0,0 @@
-/**
- * Copyright  (C) 2013-2014 CoDyCo Project
- * Author: Silvio Traversaro
- * website: http://www.codyco.eu
- */
-
-#ifndef URDF_UTILS_FUNCTIONS__
-#define URDF_UTILS_FUNCTIONS__
-
-#include <urdf_world/types.h>
-
-
-/**
- * Import meshes from a urdf model to another urdf model.
- * The match between diffent models is done based on link names, taking into account any diffence in the
- * position of the reference frames of the links.
- *
- * @param urdf_input the urdf model for which the mesh are added
- * @param urdf_meshes the urf model from which the mesh are imported
- * @return true if all went ok, false otherwise
- *
- */
-bool urdf_import_meshes(urdf::ModelInterfaceSharedPtr urdf_input, urdf::ModelInterfaceSharedPtr urdf_meshes, bool verbose=false);
-
-/**
- * Get the total mass of a URDF model
- *
- * @param model the considered URDF model
- * @return the total mass of the model
- *
- */
-double getTotalMass(urdf::ModelInterface & model);
-
-/**
- * Import limits from a urdf model to another urdf model.
- * The match between diffent models is done based on joint names.
- *
- * @param urdf_input the urdf model to which the limits are added
- * @param urdf_meshes the urf model from which the limits are imported
- * @return true if all went ok, false otherwise
- *
- */
-bool urdf_import_limits(urdf::ModelInterfaceSharedPtr urdf_input, urdf::ModelInterfaceSharedPtr urdf_meshes);
-
-bool urdf_set_friction_parameters(urdf::ModelInterfaceSharedPtr urdf_input, double damping, double friction);
-
-/**
- * If a link without inertia is in the root, remove it (to handle base_link as commonly found in humanoid models for REP 120)"
- *
- */
-bool urdf_gazebo_cleanup_remove_massless_root(urdf::ModelInterfaceSharedPtr urdf_input);
-
-/**
- * Links with zero mass, with no childrens and connected to their parent with a fixed joint are removed
- *
- */
-bool urdf_gazebo_cleanup_remove_frames(urdf::ModelInterfaceSharedPtr urdf_input);
-
-
-/**
- * Links with zero mass, with no childrens and connected to their parent with a fixed joint are removed
- * The fixed joint connecting two non-leaf links (typically used in iCub for modeling FT sensors) are transformed in rotational joint with no range (to address this issue https://bitbucket.org/osrf/gazebo/issue/618/add-option-to-disable-auto-fixed-joint)
- */
-bool urdf_gazebo_cleanup_transform_FT_sensors(urdf::ModelInterfaceSharedPtr urdf_input);
-
-/**
- * For the links that are not massless_root, frames or subpart of FT sensors links but have zero mass or zero diagonal inertia elements a small mass and a small diagonal inertia matrix
- */
-bool urdf_gazebo_cleanup_regularize_masses(urdf::ModelInterfaceSharedPtr urdf_input, double mass_epsilon, double inertia_epsilon);
-
-/**
- * Add a model uri to the meshes address
- *
- */
-bool urdf_gazebo_cleanup_add_model_uri(urdf::ModelInterfaceSharedPtr urdf_input,
-                                       std::string model_prefix);
-
-bool urdf_print_hom_transformations(urdf::ModelInterfaceSharedPtr urdf_input);
-
-
-
-
-
-
-
-
-#endif
diff --git a/dh/toBeIntegrated/README.md b/dh/toBeIntegrated/README.md
deleted file mode 100644
index 1e375e0..0000000
--- a/dh/toBeIntegrated/README.md
+++ /dev/null
@@ -1 +0,0 @@
-This folder contains old projects saved in `iCub contrib` repository that will be integrated in the model generator in the future.
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/CMakeLists.txt b/dh/toBeIntegrated/icub2gazeboPIDconverters/CMakeLists.txt
deleted file mode 100644
index e77d961..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/CMakeLists.txt
+++ /dev/null
@@ -1,48 +0,0 @@
-# Copyright (C) 2013 RobotCub Consortium
-# Author: Francesco Romano
-# CopyPolicy: Released under the terms of the GNU GPL v2.0.
-
-cmake_minimum_required(VERSION 2.8)
-
-SET(PROJECTNAME iCub2GazeboPIDConverters)
-PROJECT(${PROJECTNAME})
-
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${YARP_MODULE_PATH})
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${ICUB_MODULE_PATH})
-
-find_package(TinyXML QUIET)
-find_package(YARP)
-find_package(ICUB)
-
-if(TinyXML_FOUND)
-    set(TINYXML_INCLUDES ${TinyXML_INCLUDE_DIRS})
-    set(TINYXML_LIBS ${TinyXML_LIBRARIES})
-else(TinyXML_FOUND)
-    #don't work...?
-    set(TINYXML_INCLUDES /usr/local/include)
-    set(TINYXML_LIBS /usr/local/lib/libtinyxml.dylib)
-endif(TinyXML_FOUND)
-
-set(INCLUDE_DIR include)
-#can I specify these two dirs in some other ways?
-set(ROBOTINTERFACE_INCLUDE_DIR ../../../main/src/core/robotInterface)
-set(DEBUGINTERFACE_INCLUDE_DIR ../../../main/src/libraries/icubmod)
-
-
-include_directories(${INCLUDE_DIR} ${ROBOTINTERFACE_INCLUDE_DIR} ${DEBUGINTERFACE_INCLUDE_DIR})
-include_directories(${TINYXML_INCLUDES} ${YARP_INCLUDE_DIRS} ${ICUB_INCLUDE_DIRS})
-
-set(SOURCES src/main.cpp src/ICUBConfigParser.cpp)
-set(HEADERS ${INCLUDE_DIR}/ICUBConfigParser.h ${INCLUDE_DIR}/Constants.h)
-
-set(ROBOTINTERFACE_INCLUDES ${ROBOTINTERFACE_INCLUDE_DIR}/Param.h ${ROBOTINTERFACE_INCLUDE_DIR}/Types.h ${ROBOTINTERFACE_INCLUDE_DIR}/XMLReader.h ${ROBOTINTERFACE_INCLUDE_DIR}/Robot.h ${ROBOTINTERFACE_INCLUDE_DIR}/Action.h ${ROBOTINTERFACE_INCLUDE_DIR}/Device.h ${ROBOTINTERFACE_INCLUDE_DIR}/CalibratorThread.h)
-set(ROBOTINTERFACE_SOURCES ${ROBOTINTERFACE_INCLUDE_DIR}/Action.cpp ${ROBOTINTERFACE_INCLUDE_DIR}/CalibratorThread.cpp ${ROBOTINTERFACE_INCLUDE_DIR}/Device.cpp ${ROBOTINTERFACE_INCLUDE_DIR}/Param.cpp ${ROBOTINTERFACE_INCLUDE_DIR}/Robot.cpp ${ROBOTINTERFACE_INCLUDE_DIR}/Types.cpp ${ROBOTINTERFACE_INCLUDE_DIR}/XMLReader.cpp)
-
-
-SOURCE_GROUP("Source Files" FILES ${SOURCES})
-SOURCE_GROUP("Header Files" FILES ${HEADERS})
-
-add_library(RobotInterface STATIC ${ROBOTINTERFACE_SOURCES} ${DEBUGINTERFACE_INCLUDE_DIR}/debugStream/Debug.cpp)
-
-add_executable(${PROJECTNAME} ${SOURCES} ${HEADERS})
-target_link_libraries(${PROJECTNAME} ${TINYXML_LIBS} ${YARP_LIBRARIES} RobotInterface)
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_head.xml b/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_head.xml
deleted file mode 100644
index 4fdd20b..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_head.xml
+++ /dev/null
@@ -1,58 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>    
-<devices>      
-  <device name="head_mspecs" type="motorspecifications">  
-      <group name="DEVICE">
-          <param name="rel_candevice">head_mc</param> <!-- This is the name of the device in which there are the encoder, joints and pids -->
-      </group>
-  </device>    
-</devices>     
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_arm.xml b/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_arm.xml
deleted file mode 100644
index 1458659..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_arm.xml
+++ /dev/null
@@ -1,171 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>    
-<devices>      
-  <device name="left_arm_mspecs" type="motorspecifications">    
-
-<group name="DEVICE">
-    <param name="rel_candevice">left_arm_mc</param> <!-- This is the name of the device in which there are the encoder, joints and pids -->
-</group>
-
-<group name="MOTORS">
-    <group name="Motor0">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor1">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor2">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor3">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor4">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor5">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor6">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor7">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
- </group>
-
-<group name="COUPLED_JOINTS">
-    <group name="Shoulder">
-        <param name="JointNumber">3</param>
-        <param name="JointIndexes">0 1 2</param>
-        <param name="VelocityTransfMatrixCoefficient">1</param>
-        <param name="VelocityTransfMatrix">((1 0 0) (0 1 0) (1.625 -1.625 1))</param> <!-- Matrix transforming from encoder axis/coordinates to joint coordinates -->
-        <param name="TorqueTransfMatrixCoefficient">0.0001519</param>
-        <param name="TorqueTransfMatrix">((1 0 0) (-0.6153846154 0.6153846154 0) (-0.6153846154 0.6153846154 0.6153846154))</param> <!-- Matrix transforming from joint coordinates to motor coordinates -->
-    </group>
-</group>  
- 
-  </device>    
-</devices>     
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_hand.xml b/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_hand.xml
deleted file mode 100644
index c786e23..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_hand.xml
+++ /dev/null
@@ -1,67 +0,0 @@
-
-<?xml version="1.0" encoding="UTF-8" ?>    
-<devices>      
-  <device name="left_hand_mspecs" type="motorspecifications">   
-      <group name="DEVICE">
-          <param name="rel_candevice">left_hand_mc</param> <!-- This is the name of the device in which there are the encoder, joints and pids -->
-      </group>
-  </device>    
-</devices>     
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_leg.xml b/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_leg.xml
deleted file mode 100644
index 962cf6b..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_left_leg.xml
+++ /dev/null
@@ -1,119 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>    
-<devices>      
-  <device name="left_leg_mspecs" type="motorspecifications">   
-     
-      <group name="DEVICE">
-          <param name="rel_candevice">left_leg_mc</param> <!-- This is the name of the device in which there are the encoder, joints and pids -->
-      </group>
-      
-<group name="MOTORS">
-    <group name="Motor0">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor1">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor2">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor3">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor4">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor5">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
- </group>
- 
-  </device>    
-</devices>     
-
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_arm.xml b/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_arm.xml
deleted file mode 100644
index 128f4b3..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_arm.xml
+++ /dev/null
@@ -1,136 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>    
-<devices>      
-  <device name="right_arm_mspecs" type="motorspecifications">   
-
-      <group name="DEVICE">
-          <param name="rel_candevice">right_arm_mc</param> <!-- This is the name of the device in which there are the encoder, joints and pids -->
-      </group>
-<group name="MOTORS">
-    <group name="Motor0">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor1">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor2">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor3">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor4">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor5">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor6">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor7">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
- </group>
-
-<group name="COUPLED_JOINTS">
-    <group name="Shoulder">
-        <param name="JointNumber">3</param>
-        <param name="JointIndexes">0 1 2</param>
-        <param name="VelocityTransfMatrixCoefficient">1</param>
-        <param name="VelocityTransfMatrix">((1 0 0) (0 1 0) (1.625 -1.625 1))</param> <!-- Matrix transforming from encoder axis/coordinates to joint coordinates -->
-        <param name="TorqueTransfMatrixCoefficient">0.0001519</param>
-        <param name="TorqueTransfMatrix">((1 0 0) (-0.6153846154 0.6153846154 0) (-0.6153846154 0.6153846154 0.6153846154))</param> <!-- Matrix transforming from joint coordinates to motor coordinates -->
-    </group>
-</group>
- 
-  </device>    
-</devices>     
- 
\ No newline at end of file
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_hand.xml b/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_hand.xml
deleted file mode 100644
index 407aa48..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_hand.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-
-<?xml version="1.0" encoding="UTF-8" ?>    
-<devices>      
-  <device name="right_hand_mspecs" type="motorspecifications">   
-      
-      <group name="DEVICE">
-          <param name="rel_candevice">right_hand_mc</param> <!-- This is the name of the device in which there are the encoder, joints and pids -->
-      </group>
-      
-  </device>    
-</devices>     
- 
\ No newline at end of file
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_leg.xml b/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_leg.xml
deleted file mode 100644
index b01d7ab..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_right_leg.xml
+++ /dev/null
@@ -1,123 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>    
-<devices>      
-  <device name="right_leg_mspecs" type="motorspecifications">   
-
-      <group name="DEVICE">
-          <param name="rel_candevice">right_leg_mc</param> <!-- This is the name of the device in which there are the encoder, joints and pids -->
-      </group>
-      
-<group name="MOTORS">
-    <group name="Motor0">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor1">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor2">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor3">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor4">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor5">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
- </group>
-
- 
-  </device>    
-</devices>     
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_torso.xml b/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_torso.xml
deleted file mode 100644
index d0499b3..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/config/icub_torso.xml
+++ /dev/null
@@ -1,116 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>    
-<devices>      
-  <device name="torso_mspecs" type="motorspecifications">   
-      
-      <group name="DEVICE">
-          <param name="rel_candevice">torso_mc</param> <!-- This is the name of the device in which there are the encoder, joints and pids -->
-      </group>
-      
-<group name="MOTORS">
-    <group name="Motor0">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor1">
-        <param name="Resistance">0.664</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.32</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.04096648235185</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.203</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000847</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.015</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00001995802986</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.806</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.223</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-    <group name="Motor2">
-        <param name="Resistance">0.698</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">0.280</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">0.0236</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] -->
-        <param name="Weight">0.126</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">0.00000515</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">8</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">0.0120</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">0.00000876625427</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">0.342</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">0.115</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">100</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">48</param> <!-- * Maximum voltage [V] -->
-    </group>
-</group>
-
-<group name="COUPLED_JOINTS">
-    <group name="Roll_Pitch">
-        <param name="JointNumber">2</param>
-        <param name="JointIndexes">0 1</param>
-        <param name="VelocityTransfMatrixCoefficient">1</param>
-        <param name="VelocityTransfMatrix">((0.5 0.5) (0.5 -0.5))</param> <!-- Matrix transforming from encoder axis/coordinates to joint coordinates -->
-        <param name="TorqueTransfMatrixCoefficient">1</param>
-        <param name="TorqueTransfMatrix">((1 1) (1 -1))</param> <!-- Matrix transforming from joint coordinates to motor coordinates -->
-    </group>
-</group>
-  
-  </device>    
-</devices>     
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/include/Constants.h b/dh/toBeIntegrated/icub2gazeboPIDconverters/include/Constants.h
deleted file mode 100644
index 386301f..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/include/Constants.h
+++ /dev/null
@@ -1,61 +0,0 @@
-//
-//  Contants.h
-//  iCub2GazeboPIDConverters
-//
-//  Created by Francesco Romano on 05/12/13.
-//
-//
-
-#ifndef ICUB2GAZEBOPIDCONVERTERS_CONSTANTS_H
-#define ICUB2GAZEBOPIDCONVERTERS_CONSTANTS_H
-
-#define PI 3.1415926536
-#define RAD_2_DEG(x) ((x) * (180 / PI))
-#define DEG_2_RAD(x) ((x) * (PI / 180))
-
-const std::string DEVICE_TYPE_MOTIONCONTORL = "canmotioncontrol"; /*! Type of device to parse. It contains PIDs, Encoder */
-const std::string DEVICE_TYPE_MOTORSPECIFICATIONS = "motorspecifications"; /*! Type of device to parse. It contains Motor specifications */
-
-/** General section */
-const std::string GENERAL_GROUP_NAME = "GENERAL";
-const std::string GENERAL_JOINTS_PARAM_NAME = "Joints";
-const std::string GENERAL_ENCODERS_PARAM_NAME = "Encoder";
-
-/** PIDs Section */
-const std::string PID_GROUP_NAME = "PIDS";
-const std::string PID_PARAM_BASE_NAME = "Pid"; /*! This will be concatened with the joint index to find the pid specifications */
-
-/**********************/
-/** Motor Specification file */
-
-/** Device section */
-const std::string MOTOR_DEVICE_GROUP_NAME = "DEVICE";
-const std::string MOTOR_DEVICE_RELDEVICE_PARAM_NAME = "rel_candevice";
-
-/* Motor Section */
-const std::string MOTOR_GROUP_NAME = "MOTORS";
-const std::string MOTOR_SUBGROUP_NAME = "Motor"; /*! This will be concatened with the joint index to find the motor specifications */
-
-const std::string MOTOR_RESISTANCE_PARAM_NAME = "Resistance";
-const std::string MOTOR_INDUCTANCE_PARAM_NAME = "Inductance";
-const std::string MOTOR_BACKEMF_PARAM_NAME = "BackEMF";
-const std::string MOTOR_WEIGHT_PARAM_NAME = "Weight";
-const std::string MOTOR_INERTIA_PARAM_NAME = "Inertia";
-const std::string MOTOR_NUMPOLES_PARAM_NAME = "NumMagnPoles";
-const std::string MOTOR_STATICFRICTION_PARAM_NAME = "MaxStaticFriction";
-const std::string MOTOR_VISCOUSDAMPING_PARAM_NAME = "ViscousDamping";
-const std::string MOTOR_PEAKTORQUE_PARAM_NAME = "PeakTorque";
-const std::string MOTOR_STALLTORQUE_PARAM_NAME = "StallTorque";
-const std::string MOTOR_REDUCTION_PARAM_NAME = "ReductionFactor";
-const std::string MOTOR_MAXVOLTAGE_PARAM_NAME = "MaxVoltage";
-
-/** Coupled joints Section*/
-const std::string COUPLEDJOINT_GROUP_NAME = "COUPLED_JOINTS";
-const std::string COUPLEDJOINT_PARAM_JOINT_NUMBER_NAME = "JointNumber";
-const std::string COUPLEDJOINT_PARAM_JOINT_INDEXES_NAME = "JointIndexes";
-const std::string COUPLEDJOINT_PARAM_JOINT_VELMATRIXCOEFF_NAME = "VelocityTransfMatrixCoefficient";
-const std::string COUPLEDJOINT_PARAM_JOINT_VELMATRIX_NAME = "VelocityTransfMatrix";
-const std::string COUPLEDJOINT_PARAM_JOINT_TORQUEMATRIX_NAME = "TorqueTransfMatrix";
-const std::string COUPLEDJOINT_PARAM_JOINT_TORQUEMATRIXCOEFF_NAME = "TorqueTransfMatrixCoefficient";
-
-#endif
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/include/ICUBConfigParser.h b/dh/toBeIntegrated/icub2gazeboPIDconverters/include/ICUBConfigParser.h
deleted file mode 100644
index 55b14d0..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/include/ICUBConfigParser.h
+++ /dev/null
@@ -1,112 +0,0 @@
-// Copyright (C) 2013 RobotCub Consortium
-// Author: Francesco Romano
-// CopyPolicy: Released under the terms of the GNU GPL v2.0.
-
-#ifndef ICUB_CONFIG_PARSER_H
-#define ICUB_CONFIG_PARSER_H
-
-#include "Types.h"
-#include "Robot.h"
-#include "Device.h"
-#include <map>
-#include <vector>
-#include <string>
-#include <yarp/sig/Matrix.h>
-
-namespace icub2gazebo {
-    
-    struct DeviceWrapper;
-    struct MotorParameters;
-    struct PIDValues;
-    struct PIDsMotorWrapper;
-    struct CoupledJointInfo;
-    
-    struct DeviceWrapper {
-        RobotInterface::Device device;
-        
-        int numberOfJoints; /*!< Number of joints in the device */
-        
-        std::map<int, double> encoders; /*!< Maps the index of the joint to its encoder angle value. I use a map because the index number is important (I can't use a vector) */
-        std::map<int, PIDValues> pids; /*!< Maps the index of the joint to its PID values. I use a map because the index number is important (I can't use a vector) */
-        std::map<int, MotorParameters> motors; /*!< Maps the index of the joint to its motor parameters. I use a map because the index number is important (I can't use a vector) */
-        std::vector<CoupledJointInfo> coupledJoints; /*!< List of coupled joints for the device */
-        
-        std::map<int, PIDsMotorWrapper> newPIDs; /*!< Maps the index of the joint to the new computed PID value. I use a map because the index number is important (I can't use a vector) */
-        
-        DeviceWrapper();
-    };
-
-    struct MotorParameters {
-        double motorResistance;
-        double motorInductance;
-        double backEMFConstant;
-        double weight;
-        double inertia;
-        double numberOfPoles;
-        double maxStaticFriction;
-        double viscousDamping;
-        double peakTorque;
-        double stallTorque;
-        double reductionFactor;
-        double maxVoltage; //here?
-        
-        MotorParameters();
-    };
-    
-    struct PIDValues {
-        double p; //they must hold also the updated value which is a floating point number
-        double d;
-        double i;
-        
-        int shiftFactor;
-        int maxPWM;
-        int minPWM;
-        
-        PIDValues();
-    };
-    
-    struct PIDsMotorWrapper {
-        struct PIDValues pidValues;
-        double motorGain;
-        
-        PIDsMotorWrapper();
-    };
-    
-    struct CoupledJointInfo {
-        std::string groupName;
-        unsigned int numberOfJoints; /*!< Number of motors and joints coupled */
-        std::vector<unsigned int> jointIndexes; /*!< indexes of the joints (w.r.t. the device joints) that are coupled */
-        
-        double velocityTransformationMatrixCoefficient;
-        yarp::sig::Matrix velocityTransformationMatrix;
-        double torqueTransformationMatrixCoefficient;
-        yarp::sig::Matrix torqueTransformationMatrix;
-        
-        CoupledJointInfo(unsigned int numberOfJoints);
-    };
-    
-    class iCubConfigParser {
-    private:
-        RobotInterface::Robot _robot;
-        std::map<std::string, DeviceWrapper> _devices;
-        
-        bool parseDevice(const RobotInterface::Device &device, DeviceWrapper*);
-        bool parseGeneralGroup(const RobotInterface::Device &device, DeviceWrapper* deviceWrapper);
-        bool parsePIDs(const RobotInterface::Device &device, DeviceWrapper* wrapper);
-        bool parseMotors(const RobotInterface::Device &device, DeviceWrapper* wrapper);
-        bool parseCoupledJoints(const RobotInterface::Device &device, DeviceWrapper* wrapper);
-        std::string getRelativeDeviceName(const RobotInterface::Device &device);
-        
-        bool computeNewPIDsValues();
-        
-    public:        
-        iCubConfigParser(RobotInterface::Robot &robot);
-        
-        void startParsing();
-        std::string outputPIDsForDevice(std::string deviceName);
-        std::string outputPIDs();
-        
-    };
-}
-
-#endif
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/src/ICUBConfigParser.cpp b/dh/toBeIntegrated/icub2gazeboPIDconverters/src/ICUBConfigParser.cpp
deleted file mode 100644
index d63ee22..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/src/ICUBConfigParser.cpp
+++ /dev/null
@@ -1,401 +0,0 @@
-// Copyright (C) 2013 RobotCub Consortium
-// Author: Francesco Romano
-// CopyPolicy: Released under the terms of the GNU GPL v2.0.
-
-#include "ICUBConfigParser.h"
-
-#include <yarp/os/Property.h>
-#include "Device.h"
-#include "Robot.h"
-#include "Types.h"
-#include "Constants.h"
-#include <string>
-#include <sstream>
-#include <cmath>
-#include <yarp/sig/Vector.h>
-#include <yarp/math/Math.h>
-
-using namespace RobotInterface;
-using namespace icub2gazebo;
-using namespace yarp::math;
-
-DeviceWrapper::DeviceWrapper():numberOfJoints(-1){}
-PIDValues::PIDValues():p(0),d(0),i(0),shiftFactor(0),maxPWM(0),minPWM(0){}
-MotorParameters::MotorParameters():motorResistance(0),motorInductance(0),backEMFConstant(0),weight(0),inertia(0),
-numberOfPoles(0),maxStaticFriction(0),viscousDamping(0),peakTorque(0),stallTorque(0),maxVoltage(0){}
-PIDsMotorWrapper::PIDsMotorWrapper():pidValues(),motorGain(0){}
-
-CoupledJointInfo::CoupledJointInfo(unsigned int _numberOfJoints):
-numberOfJoints(_numberOfJoints > 0 ? _numberOfJoints : 0),
-velocityTransformationMatrix(numberOfJoints, numberOfJoints),
-torqueTransformationMatrix(numberOfJoints, numberOfJoints){
-    if (numberOfJoints > 0) {
-        jointIndexes.reserve(numberOfJoints);
-    }
-}
-
-iCubConfigParser::iCubConfigParser(RobotInterface::Robot &robot)
-{
-    _robot = robot;
-}
-
-void iCubConfigParser::startParsing()
-{
-    for (DeviceList::const_iterator it = _robot.devices().begin(); it != _robot.devices().end(); it++) {
-        if (it->type().compare(DEVICE_TYPE_MOTIONCONTORL) == 0) {
-            DeviceWrapper deviceWrapper;
-            if (parseDevice(*it, &deviceWrapper)) {
-                _devices[it->name()] = deviceWrapper;
-            }
-        }
-    }
-    for (DeviceList::const_iterator it = _robot.devices().begin(); it != _robot.devices().end(); it++) {
-        if (it->type().compare(DEVICE_TYPE_MOTORSPECIFICATIONS) == 0) {
-            //get relative device
-            std::string relDevice = this->getRelativeDeviceName(*it);
-            //find wrapper
-            if (relDevice.empty())
-                continue;
-            DeviceWrapper wrapper = _devices.at(relDevice);
-            std::cout << "Wrapper found: " << relDevice << "\n";
-            parseMotors(*it, &wrapper);
-            parseCoupledJoints(*it, &wrapper);
-            _devices.at(relDevice) = wrapper;
-        }
-    }
-    
-    computeNewPIDsValues();
-}
-
-bool iCubConfigParser::parseDevice(const RobotInterface::Device &device, DeviceWrapper* wrapper)
-{
-    if (!wrapper) return false;
-    wrapper->device = device;
-    
-    parseGeneralGroup(device, wrapper);
-    parsePIDs(device, wrapper);
-    return true;
-}
-
-bool iCubConfigParser::parseGeneralGroup(const RobotInterface::Device &device, DeviceWrapper* wrapper)
-{
-    if (!wrapper) return false;
-    std::string generalProperties = device.findParam(GENERAL_GROUP_NAME);
-    if (!generalProperties.empty()) {
-        yarp::os::Property generals(generalProperties.c_str());
-        wrapper->numberOfJoints = generals.find(GENERAL_JOINTS_PARAM_NAME).asInt();
-        
-        yarp::os::Bottle &encoders = generals.findGroup(GENERAL_ENCODERS_PARAM_NAME);
-        for (int i = 1; i < encoders.size(); i++) {
-            wrapper->encoders[i-1] = encoders.get(i).asDouble();
-        }
-        return true;
-    }
-    return false;
-}
-
-bool iCubConfigParser::parsePIDs(const RobotInterface::Device &device, DeviceWrapper* wrapper)
-{
-    if (!wrapper) return false;
-    yarp::os::Property group;
-    
-    //first: find PIDs group
-    std::string groupStr = findGroup(device.params(), PID_GROUP_NAME);
-    if (!groupStr.empty()) {
-        group.fromString(groupStr);
-        
-        int i = 0;
-        while (true) {
-            std::stringstream pidStream;
-            pidStream << PID_PARAM_BASE_NAME << i;
-            
-            std::string name = pidStream.str();
-            
-            yarp::os::Bottle &pids = group.findGroup(pidStream.str());
-            if (!pids.isNull()) {
-                PIDValues pidValues;
-                pidValues.p = pids.get(1).asDouble();
-                pidValues.d = pids.get(2).asDouble();
-                pidValues.i = pids.get(3).asDouble();
-                pidValues.minPWM = pids.get(4).asInt();
-                pidValues.maxPWM = pids.get(5).asInt();
-                pidValues.shiftFactor = pids.get(6).asInt();
-                wrapper->pids[i] = pidValues;
-            }
-            else {
-                if (wrapper->numberOfJoints <= 0) {
-                    //number of joints not defined (or 0). Because I didn't find the pid#i I break
-                    break;
-                }
-            }
-            i++;
-            if (wrapper->numberOfJoints > 0 &&
-                i >= wrapper->numberOfJoints) {
-                break;
-            }
-        }
-        return true;
-    }
-    return false;
-}
-
-bool iCubConfigParser::parseMotors(const RobotInterface::Device &device, DeviceWrapper* wrapper)
-{
-    if (!wrapper) return false;
-    yarp::os::Property group;
-    std::string groupStr = findGroup(device.params(), MOTOR_GROUP_NAME);
-    if (!groupStr.empty()) {
-        group.fromString(groupStr.c_str());
-        int i = 0;
-        while (true) {
-            std::stringstream motorStream;
-            motorStream << MOTOR_SUBGROUP_NAME << i;
-            
-            yarp::os::Bottle subGroupBottle = group.findGroup(motorStream.str());
-            if (subGroupBottle.isNull()) {
-                if (wrapper->numberOfJoints <= 0) {
-                    //number of joints not defined (or 0). Because I didn't find the motor#i I break
-                    break;
-                }
-                else if (wrapper->numberOfJoints > 0 &&
-                         i >= wrapper->numberOfJoints) {
-                    break;
-                }
-                i++;
-                continue;
-            }
-            yarp::os::Property motor(subGroupBottle.toString().c_str());
-            
-            MotorParameters motorParameters;
-            //fill structure
-            yarp::os::Value val;
-            motorParameters.motorResistance = motor.find(MOTOR_RESISTANCE_PARAM_NAME).asDouble();
-            motorParameters.motorInductance = motor.find(MOTOR_INDUCTANCE_PARAM_NAME).asDouble();
-            motorParameters.backEMFConstant = motor.find(MOTOR_BACKEMF_PARAM_NAME).asDouble();
-            motorParameters.weight = motor.find(MOTOR_WEIGHT_PARAM_NAME).asDouble();
-            motorParameters.inertia = motor.find(MOTOR_INERTIA_PARAM_NAME).asDouble();
-            motorParameters.numberOfPoles = motor.find(MOTOR_NUMPOLES_PARAM_NAME).asDouble();
-            motorParameters.maxStaticFriction = motor.find(MOTOR_STATICFRICTION_PARAM_NAME).asDouble();
-            motorParameters.viscousDamping = motor.find(MOTOR_VISCOUSDAMPING_PARAM_NAME).asDouble();
-            motorParameters.peakTorque = motor.find(MOTOR_PEAKTORQUE_PARAM_NAME).asDouble();
-            motorParameters.stallTorque = motor.find(MOTOR_STALLTORQUE_PARAM_NAME).asDouble();
-            motorParameters.reductionFactor = motor.find(MOTOR_REDUCTION_PARAM_NAME).asDouble();
-            motorParameters.maxVoltage = motor.find(MOTOR_MAXVOLTAGE_PARAM_NAME).asDouble();
-            
-            wrapper->motors[i] = motorParameters;
-            i++;
-            if (wrapper->numberOfJoints > 0 &&
-                i >= wrapper->numberOfJoints) {
-                break;
-            }
-        }
-        return true;
-    }
-    return false;
-}
-
-bool iCubConfigParser::parseCoupledJoints(const RobotInterface::Device &device, DeviceWrapper* wrapper)
-{
-    if (!wrapper) return false;
-    yarp::os::Property group;
-    std::string groupStr = findGroup(device.params(), COUPLEDJOINT_GROUP_NAME);
-    if (!groupStr.empty()) {
-        yarp::os::Bottle groupBottle(groupStr.c_str());
-        //enumerate subgroups
-        for (int i = 0; i < groupBottle.size(); i++) {
-            yarp::os::Bottle *subGroup = groupBottle.get(i).asList();
-            if (!subGroup) continue;
-            int jointNumber = subGroup->find(COUPLEDJOINT_PARAM_JOINT_NUMBER_NAME).asInt();
-            if (jointNumber <= 0) continue;
-            CoupledJointInfo coupledJoint(jointNumber);
-            yarp::os::Bottle& jointIndexes = subGroup->findGroup(COUPLEDJOINT_PARAM_JOINT_INDEXES_NAME);
-            
-            coupledJoint.velocityTransformationMatrixCoefficient = subGroup->find(COUPLEDJOINT_PARAM_JOINT_VELMATRIXCOEFF_NAME).asDouble();
-            coupledJoint.torqueTransformationMatrixCoefficient = subGroup->find(COUPLEDJOINT_PARAM_JOINT_TORQUEMATRIXCOEFF_NAME).asDouble();
-            
-            yarp::os::Bottle& velMatrixBottle = subGroup->findGroup(COUPLEDJOINT_PARAM_JOINT_VELMATRIX_NAME);
-            yarp::os::Bottle& torqueMatrixBottle = subGroup->findGroup(COUPLEDJOINT_PARAM_JOINT_TORQUEMATRIX_NAME);
-
-            if (jointIndexes.isNull() ||
-                (velMatrixBottle.isNull() || velMatrixBottle.size() < 2)  ||
-                (torqueMatrixBottle.isNull() || torqueMatrixBottle.size() < 2)) continue;
-            //remove outer wrapper of the matrix
-            yarp::os::Bottle* velMatrix = velMatrixBottle.get(1).asList();
-            yarp::os::Bottle* torqueMatrix = torqueMatrixBottle.get(1).asList();
-            if (!velMatrix || !torqueMatrix) continue;
-            
-            for (int idx = 0; idx < jointNumber; idx++) {
-                //I have to skip the first element..
-                coupledJoint.jointIndexes.push_back(jointIndexes.get(idx + 1).asInt());
-                
-                yarp::os::Bottle* velMatrixRow = velMatrix->get(idx).asList();
-                yarp::os::Bottle* torqueMatrixRow = torqueMatrix->get(idx).asList();
-                if (!velMatrixRow || !torqueMatrix) continue;
-                
-                std::string row1 = velMatrixRow->toString();
-                std::string row2 = torqueMatrixRow->toString();
-                
-                for (int col = 0; col < jointNumber; col++) {
-                    coupledJoint.velocityTransformationMatrix(idx, col) = velMatrixRow->get(col).asDouble();
-                    coupledJoint.torqueTransformationMatrix(idx, col) = torqueMatrixRow->get(col).asDouble();
-                }
-            }
-            wrapper->coupledJoints.push_back(coupledJoint);
-        }
-        return true;
-    }
-    return false;
-}
-
-bool iCubConfigParser::computeNewPIDsValues()
-{
-    //For each device
-    for (std::map<std::string, DeviceWrapper>::iterator device = _devices.begin(); device != _devices.end(); device++) {
-        //iterate over original PIDS, and access using the same key the motor map.
-        
-        for (std::map<int, PIDValues>::const_iterator originalPID = device->second.pids.begin();
-             originalPID != device->second.pids.end(); originalPID++) {
-            
-            std::map<int, MotorParameters>::const_iterator motorParameters = device->second.motors.find(originalPID->first);
-            if (motorParameters != device->second.motors.end()) {
-                //found motor parameters. Starting conversion
-                PIDValues newPIDs;
-                MotorParameters motorParams = motorParameters->second;
-                
-                //Common unit of measurement for the back EMF constant is Volt per k rpm.
-                //But I need to convert it to SI unit=> V / rad/s
-                //By the way: the constant in SI unit is equal to the motor current torque [Nm/A]
-//                double k_e = motorParams.backEMFConstant * 1e-3 * 60 / (2*PI);
-                
-                //compute motor static gain (considering infinite inertia)
-                //K_openloop = 1/R * (K_phi * N) [Nm / V]
-                double motorOpenLoopGain = (motorParams.reductionFactor * motorParams.backEMFConstant) / motorParams.motorResistance;
-                //K_feedback = K_phi * N * 1/B [V/Nm]
-//                double motorFeedbackGain = (motorParams.reductionFactor * motorParams.backEMFConstant) / motorParams.viscousDamping;
-                // For this gain I consider only the openloop gain => infinite inertia ~= stall torque
-//                double motorGain = motorOpenLoopGain / (1 + motorOpenLoopGain * motorFeedbackGain); // [Nm / V]
-                double motorGain = motorOpenLoopGain;
-                
-                //for each (p, d, and i)
-                double shiftFactor = pow(2, -originalPID->second.shiftFactor);
-                double originalP = originalPID->second.p * shiftFactor;
-                double originalD = originalPID->second.d * shiftFactor;
-                double originalI = originalPID->second.i * shiftFactor;
-                
-                double pwmFactor = motorParams.maxVoltage / originalPID->second.maxPWM;
-                
-                //reading encoder data
-                std::map<int, double>::const_iterator encIterator = device->second.encoders.find(originalPID->first);
-                double delta_enc = encIterator != device->second.encoders.end() ? encIterator->second : 0;
-                
-                //formula to be checked
-                newPIDs.p = originalP * pwmFactor * delta_enc * RAD_2_DEG(1) //pid conversion to [V/rad]
-                * motorGain; // pid in [Nm/rad]
-                newPIDs.d = originalD * pwmFactor * delta_enc * RAD_2_DEG(1) //pid conversion to [V/rad]
-                * motorGain; // pid in [Nm/rad]
-                newPIDs.i = originalI * pwmFactor * delta_enc * RAD_2_DEG(1) //pid conversion to [V/rad]
-                * motorGain; // pid in [Nm/rad]
-                
-                //copy original values for the other variables
-                newPIDs.maxPWM = originalPID->second.maxPWM;
-                newPIDs.minPWM = originalPID->second.minPWM;
-                newPIDs.shiftFactor = originalPID->second.shiftFactor;
-                                
-                PIDsMotorWrapper motorWrapper;
-                motorWrapper.pidValues = newPIDs;
-                motorWrapper.motorGain = motorGain;
-                
-                device->second.newPIDs[motorParameters->first] = motorWrapper;
-            }
-        }
-        //I have to check if there are some coupled joints for this device
-        if (!device->second.coupledJoints.empty()) {
-            //I have already computed the tau joint -> error (in radiants)
-            //Usually tau joints = tau motor.
-            //for coupled joints this is no longer true.
-            //I want to map the pid (joint) constants to their motor constants and only at this point apply the motor constant. Then I have to remap this back to the joint space
-            for (std::vector<CoupledJointInfo>::const_iterator cj = device->second.coupledJoints.begin();
-                 cj != device->second.coupledJoints.end(); cj++) {
-                //build a vector containing all the Ps (and Ds and Is) for the joints composing the coupling
-                yarp::sig::Vector pValues(cj->numberOfJoints);
-                yarp::sig::Vector dValues(cj->numberOfJoints);
-                yarp::sig::Vector iValues(cj->numberOfJoints);
-                
-                for (int idx = 0; idx < cj->numberOfJoints; idx++) {
-                    unsigned int jointIndex = cj->jointIndexes.at(idx);
-                    
-                    icub2gazebo::PIDsMotorWrapper pids = device->second.newPIDs.find(jointIndex)->second;
-                    
-                    pValues[idx] = pids.pidValues.p;
-                    dValues[idx] = pids.pidValues.d;
-                    iValues[idx] = pids.pidValues.i;
-                }
-                yarp::sig::Matrix transfMatrix = cj->torqueTransformationMatrix;
-
-                yarp::sig::Vector pMotor = cj->torqueTransformationMatrixCoefficient * transfMatrix * pValues;
-                yarp::sig::Vector dMotor = cj->torqueTransformationMatrixCoefficient * transfMatrix * dValues;
-                yarp::sig::Vector iMotor = cj->torqueTransformationMatrixCoefficient * transfMatrix * iValues;
-                
-                //Now transform it back to joint coordinates.
-                //simply invert the matrix (which should always be invertible and small so I do not care about performances)
-                yarp::sig::Matrix invMatrix = yarp::math::luinv(transfMatrix);
-                
-                pValues = invMatrix * pMotor;
-                dValues = invMatrix * dMotor;
-                iValues = invMatrix * iMotor;
-                
-                //save it back to PIDs values
-                for (int idx = 0; idx < cj->numberOfJoints; idx++) {
-                    unsigned int jointIndex = cj->jointIndexes.at(idx);
-                    
-                    icub2gazebo::PIDsMotorWrapper pids = device->second.newPIDs.find(jointIndex)->second;
-                    pids.pidValues.p = pValues[idx];
-                    pids.pidValues.d = dValues[idx];
-                    pids.pidValues.i = iValues[idx];
-                }
-            }
-        }
-    }
-    return true;
-}
-
-std::string iCubConfigParser::getRelativeDeviceName(const RobotInterface::Device &device)
-{
-    std::string generalProperties = device.findParam(MOTOR_DEVICE_GROUP_NAME);
-    if (!generalProperties.empty()) {
-        yarp::os::Property generals(generalProperties.c_str());
-        return generals.find(MOTOR_DEVICE_RELDEVICE_PARAM_NAME).asString();
-    }
-    return "";
-}
-
-std::string iCubConfigParser::outputPIDsForDevice(std::string deviceName)
-{
-    std::map<std::string, icub2gazebo::DeviceWrapper>::const_iterator device = _devices.find(deviceName);
-    if (device == _devices.end())
-        return std::string();
-    
-    std::ostringstream oss;
-    for (std::map<int, PIDsMotorWrapper>::const_iterator pidWrapper = device->second.newPIDs.begin(); pidWrapper != device->second.newPIDs.end(); pidWrapper++) {
-        oss <<  PID_PARAM_BASE_NAME << pidWrapper->first << " "
-        << pidWrapper->second.pidValues.p << " " << pidWrapper->second.pidValues.d << " " << pidWrapper->second.pidValues.i << " "
-        << pidWrapper->second.pidValues.maxPWM << " " << pidWrapper->second.pidValues.minPWM << " " << pidWrapper->second.pidValues.shiftFactor << " 0"
-        << std::endl;
-    }
-    return oss.str();
-}
-
-std::string iCubConfigParser::outputPIDs()
-{
-    std::ostringstream oss;
-    for (std::map<std::string, icub2gazebo::DeviceWrapper >::const_iterator it = _devices.begin(); it != _devices.end(); it++) {
-        
-        std::string newPids = this->outputPIDsForDevice(it->first);
-        if (!newPids.empty()) {
-            oss << "device = [" << it->first << "]" << std::endl;
-            oss << newPids;
-            oss << std::endl << std::endl;
-        }
-    }
-    return oss.str();
-}
diff --git a/dh/toBeIntegrated/icub2gazeboPIDconverters/src/main.cpp b/dh/toBeIntegrated/icub2gazeboPIDconverters/src/main.cpp
deleted file mode 100644
index 5d94aec..0000000
--- a/dh/toBeIntegrated/icub2gazeboPIDconverters/src/main.cpp
+++ /dev/null
@@ -1,81 +0,0 @@
-// Copyright (C) 2013 RobotCub Consortium
-// Author: Francesco Romano
-// CopyPolicy: Released under the terms of the GNU GPL v2.0.
-
-/**
- 
- XML read for the motor: * means the parameter is needed for the module
-
- <group name="MOTORS">
-    <group name="Motor0">
-        <param name="Resistance">10</param> <!-- * Motor Resistance [Ohm] -->
-        <param name="Inductance">10</param> <!-- Motor Inductance [mH] -->
-        <param name="BackEMF">10</param> <!-- * Motor Back ElectroMagnetic Force Constant [V/ rad/s] = [Nm/A] -->
-        <param name="Weight">10</param> <!-- Motor (Frameless) weight [Kg] -->
-        <param name="Inertia">10</param> <!-- Motor Inertia [Kg m^2] -->
-        <param name="NumMagnPoles">10</param> <!-- Number of magnetic poles of the motor-->
-        <param name="MaxStaticFriction">10</param> <!-- Max static friction [Nm] -->
-        <param name="ViscousDamping">10</param> <!-- * Viscous damping [Nm / rad/s] -->
-        <param name="PeakTorque">10</param> <!-- Peak torque [Nm] -->
-        <param name="StallTorque">10</param> <!-- Stall torque [Nm] -->
-        <param name="ReductionFactor">10</param> <!-- * Transmission reduction -->
-        <param name="MaxVoltage">10</param> <!-- * Maximum voltage [V] -->
-    </group>
- </group>
- 
-XML for coupled joints:
- 
- <group name="COUPLED_JOINTS">
-     <group name="Shoulder"><!-- Name of the coupling. It is not used-->
-         <param name="JointNumber">3</param> <!-- Number of joints forming coupled -->
-         <param name="JointIndexes">0 1 2</param><!-- Indexes of the joints forming the coupling. They must be present in the device xml file (e.g. in the PID section). The order must be consistent with the matrix row order -->
-         <param name="VelocityTransfMatrixCoefficient">1</param> <!-- scalar coefficient for the velocity transformation matrix -->
-         <param name="VelocityTransfMatrix">((1 0 0) (0 1 0) (1.625 -1.625 1))</param> <!-- Matrix transforming from encoder axis/coordinates to joint coordinates, i.e. JointPosition = T * encoderPositions-->
-         <param name="TorqueTransfMatrixCoefficient">0.0001519</param> <!-- scalar coefficient for the torque transformation matrix -->
-         <param name="TorqueTransfMatrix">((1 0 0) (-0.6153846154 0.6153846154 0) (-0.6153846154 0.6153846154 0.6153846154))</param> <!-- Matrix transforming from torque in motor coordinates to torque in joint coordinates, i.e. Torque_motor = T * Torque_joints-->
-     </group>
- </group>
- 
- NOTES on implementation:
- This application parses the iCub XML configuration file.
- In the iCub repository the ``robotInterface'' module does the same thing.
- Unfortunately it is not written as a library (linked by the module) but as a standalone module.
- Because I need the very same files I copied them (in the robotInterface dir in include and src). When the parser will be made as a standalone library it will be possible to remvove those files.
- Because I didn't want to include the debugStream file (I was not able to add it as a library but I should have added it hardcoded in the CMakeLists.txt) I created a "fake" file ``Debug.h'' which tries to hide the original debug library. Again, when the original iCub will (?) be more modular it will be possible to remove also the Debug.h file.
- */
-
-#include "XMLReader.h"
-#include "Robot.h"
-
-#include "ICUBConfigParser.h"
-
-#include <yarp/os/ResourceFinder.h>
-
-using namespace RobotInterface;
-
-int main(int argc, char ** argv) 
-{
-    //add ResourceFinder to locate iCub config file (?)
-    yarp::os::ResourceFinder resourceFinder;
-    resourceFinder.configure(argc, argv);
-    
-    yarp::os::Value file = resourceFinder.find("inputFile");
-    if (file.isNull()) {
-        std::cerr << "No input file provided." << std::endl;
-        exit(-1);
-    }
-    
-    std::cout << "Start parsing " << file.asString() << std::endl;
-    
-    XMLReader xmlReader;
-    Robot robot = xmlReader.getRobot(file.asString());
-    
-    icub2gazebo::iCubConfigParser parser(robot);
-    parser.startParsing();
-    
-    std::cout << std::endl << std::endl << "Computing updated PIDs" << std::endl << parser.outputPIDs();
-    
-    return 0;
-    
-    
-}