diff --git a/urdf/ergoCub/conf/ergocub.xml b/urdf/ergoCub/conf/ergocub.xml
index cbcdc31b..252e8706 100644
--- a/urdf/ergoCub/conf/ergocub.xml
+++ b/urdf/ergoCub/conf/ergocub.xml
@@ -19,8 +19,6 @@
-
-
@@ -37,7 +35,6 @@
-
-
+
diff --git a/urdf/ergoCub/conf/sensors/gazebo_ergocub_lidar.ini b/urdf/ergoCub/conf/sensors/gazebo_ergocub_lidar.ini
index b6ffbff9..d7ce67f6 100644
--- a/urdf/ergoCub/conf/sensors/gazebo_ergocub_lidar.ini
+++ b/urdf/ergoCub/conf/sensors/gazebo_ergocub_lidar.ini
@@ -3,9 +3,9 @@ disableImplicitNetworkWrapper
period 10
[SENSOR]
-max_distance 25.0
+max_distance 5.0
min_distance 0.2
max_angle 360
min_angle 0
-resolution 0.225
+resolution 0.5
allow_infinity 1
diff --git a/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf b/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf
new file mode 100644
index 00000000..6dfdeca8
--- /dev/null
+++ b/urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf
@@ -0,0 +1,3420 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+true
+
+true
+
+
+true
+
+true
+
+
+
+ model://ergoCub/conf/gazebo_ergocub_torso.ini
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+true
+
+true
+
+
+true
+
+true
+
+
+true
+
+true
+
+
+
+ 0
+ 100000
+
+ 4
+ 18000000
+ 100
+ 100
+ 0.0001
+ 1
+ 1
+ 0 0 0
+
+
+
+ 0
+ 100000
+
+ 4
+ 18000000
+ 100
+ 100
+ 0.0001
+ 1
+ 1
+ 0 0 0
+
+
+
+model://ergoCub/conf/gazebo_ergocub_left_leg.ini
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+true
+
+true
+
+
+true
+
+true
+
+
+true
+
+true
+
+
+
+ 0
+ 100000
+
+ 4
+ 18000000
+ 100
+ 100
+ 0.0001
+ 1
+ 1
+ 0 0 0
+
+
+
+ 0
+ 100000
+
+ 4
+ 18000000
+ 100
+ 100
+ 0.0001
+ 1
+ 1
+ 0 0 0
+
+
+
+model://ergoCub/conf/gazebo_ergocub_right_leg.ini
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+
+model://ergoCub/conf/gazebo_ergocub_head.ini
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+
+model://ergoCub/conf/gazebo_ergocub_left_arm.ini
+-0.52 0.52 0 0.785 0 0 0.0
+
+
+
+
+model://ergoCub/conf/gazebo_ergocub_left_hand.ini
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+
+model://ergoCub/conf/gazebo_ergocub_right_arm.ini
+-0.52 0.52 0 0.785 0 0 0.0
+
+
+
+
+model://ergoCub/conf/gazebo_ergocub_right_hand.ini
+
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+ 1
+
+
+
+ model://ergoCub/conf/ergocub.xml
+
+
+
+
+ 1
+ 100
+
+ sensor
+ child_to_parent
+
+ 0.0 0.0 0.0 -3.1415923328972943 2.923917580150936e-08 -1.5707962214346844
+
+ model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini
+
+
+
+
+
+
+ sensor
+ child_to_parent
+
+
+
+
+
+ 1
+ 100
+
+ sensor
+ child_to_parent
+
+ 0.0 0.0 0.0 -3.1415924092528633 -2.2277470853981803e-08 1.5707964070692688
+
+ model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini
+
+
+
+
+
+
+ sensor
+ child_to_parent
+
+
+
+
+
+ 1
+ 100
+
+ sensor
+ child_to_parent
+
+ 0.0 0.0 0.0 3.141592653589793 -0.0 2.6179934742070086
+
+ model://ergoCub/conf/FT/gazebo_ergocub_left_leg_ft.ini
+
+
+
+
+
+
+ sensor
+ child_to_parent
+
+
+
+
+
+ 1
+ 100
+
+ sensor
+ parent_to_child
+
+ 0.0 0.0 0.022299999999999986 0.0 -0.0 -2.0943952105869315
+
+ model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini
+
+
+
+
+
+
+ sensor
+ parent_to_child
+
+
+
+
+
+ 1
+ 100
+
+ sensor
+ parent_to_child
+
+ 0.0 0.0 0.022299999999999986 0.0 -0.0 -2.0943952105869315
+
+ model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini
+
+
+
+
+
+
+ sensor
+ parent_to_child
+
+
+
+
+
+ 1
+ 100
+
+ sensor
+ child_to_parent
+
+ 0.0 0.0 0.0 3.141592653589793 0.0 -2.6179934742070086
+
+ model://ergoCub/conf/FT/gazebo_ergocub_right_leg_ft.ini
+
+
+
+
+
+
+ sensor
+ child_to_parent
+
+
+
+
+
+ 1
+ 100
+
+ sensor
+ parent_to_child
+
+ 0.0 0.0 0.022299999999999986 0.0 -0.0 -2.0943952105869315
+
+ model://ergoCub/conf/FT/gazebo_ergocub_right_foot_front_ft.ini
+
+
+
+
+
+
+ sensor
+ parent_to_child
+
+
+
+
+
+ 1
+ 100
+
+ sensor
+ parent_to_child
+
+ 0.0 0.0 0.022299999999999986 0.0 -0.0 -2.0943952105869315
+
+ model://ergoCub/conf/FT/gazebo_ergocub_right_foot_rear_ft.ini
+
+
+
+
+
+
+ sensor
+ parent_to_child
+
+
+
+
+
+ 1
+ 100
+ -0.004299998 -0.0055 0.03823499999999996 3.141592653589793 -0.0 1.5707963267948966
+
+ model://ergoCub/conf/gazebo_ergocub_inertial.ini
+
+
+
+
+
+
+
+
+
+ 1
+ 30
+ 0.00751550026595621 0.010000000000000005 -4.911144457775407e-08 -1.5707962931078618 8.881784197001252e-16 -1.5707963267948954
+
+ 0 0 0 -1.57079 -1.57079 3.14159
+ 1.57079
+
+ 0
+ 0
+ 0
+ 0
+ 0
+ 319.5 239.5
+
+
+ 640
+ 480
+ R8G8B8
+
+
+ 0.175
+ 3000
+
+
+ false
+
+ model://ergoCub/conf/sensors/gazebo_ergocub_rgbd_camera.ini
+
+
+
+
+
+
+
+
+
+ 1
+ 30
+ 0.00751550026595621 -0.0115 -4.911144457775407e-08 -1.5707962931078618 8.881784197001252e-16 -1.5707963267948954
+
+ 0 0 0 -1.57079 -1.57079 3.14159
+ 1.2217
+
+ 0
+ 0
+ 0
+ 0
+ 0
+ 319.5 239.5
+
+
+ 640
+ 480
+ R8G8B8
+
+
+ 0.02
+ 3000
+
+
+ false
+
+ model://ergoCub/conf/gazebo_ergocub_rgb_camera.ini
+
+
+
+
+
+
+
+
+
+ 1
+ 100
+ 0.00751550026595621 0.010000000000000005 -4.911144457775407e-08 -1.5707962931078618 8.881784197001252e-16 -1.5707963267948954
+
+ model://ergoCub/conf/gazebo_ergocub_realsense_inertial.ini
+
+
+
+
+
+
+
+
+
+ 1
+ 100
+ 4.99999999999997e-05 0.0 0.13480199999999992 0.0 -0.0 1.5707963267948966
+ 1
+ true
+ 40
+
+
+
+ 360
+ 2
+ 0
+ 6.28
+
+
+
+ 0.04
+ 5
+ 0.01
+
+
+ gaussian
+ 0
+ 0.005
+
+
+
+ model://ergoCub/conf/sensors/gazebo_ergocub_lidar.ini
+
+
+
+
+
+
+
+
+ 0.0 0.0 0.9 0.0 0.0 0.0
+
+