diff --git a/ergoCubSN000/CMakeLists.txt b/ergoCubSN000/CMakeLists.txt
index 6c9a23c8ba..70527cde80 100644
--- a/ergoCubSN000/CMakeLists.txt
+++ b/ergoCubSN000/CMakeLists.txt
@@ -9,6 +9,5 @@ yarp_install(DIRECTORY estimators DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/
 yarp_install(DIRECTORY calibrators DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
 yarp_install(DIRECTORY wrappers DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
 yarp_install(DIRECTORY hardware DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
-yarp_install(DIRECTORY camera DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
 yarp_install(DIRECTORY sensors  DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
 yarp_install(DIRECTORY telemetry  DESTINATION ${ICUBCONTRIB_ROBOTS_INSTALL_DIR}/${appname})
diff --git a/ergoCubSN000/camera/ServerGrabberDualDragon.ini b/ergoCubSN000/camera/ServerGrabberDualDragon.ini
deleted file mode 100644
index 6d9b0b953e..0000000000
--- a/ergoCubSN000/camera/ServerGrabberDualDragon.ini
+++ /dev/null
@@ -1,9 +0,0 @@
-device grabberDual
-capabilities COLOR
-name /icub/cam
-split true
-twoCameras true
-left_config  camera/dragonfly2_config_left.ini
-right_config camera/dragonfly2_config_right.ini
-
-
diff --git a/ergoCubSN000/camera/ServerGrabberDualDragon640_480.ini b/ergoCubSN000/camera/ServerGrabberDualDragon640_480.ini
deleted file mode 100644
index 24e2ca4617..0000000000
--- a/ergoCubSN000/camera/ServerGrabberDualDragon640_480.ini
+++ /dev/null
@@ -1,7 +0,0 @@
-device grabberDual
-capabilities COLOR
-name /icub/cam
-split true
-twoCameras true
-left_config  camera/dragonfly2_config_left640_480.ini
-right_config camera/dragonfly2_config_right640_480.ini
diff --git a/ergoCubSN000/camera/ServerGrabberDualDragonBayer.ini b/ergoCubSN000/camera/ServerGrabberDualDragonBayer.ini
deleted file mode 100644
index d94d114ceb..0000000000
--- a/ergoCubSN000/camera/ServerGrabberDualDragonBayer.ini
+++ /dev/null
@@ -1,9 +0,0 @@
-device grabberDual
-capabilities RAW
-name /icub/cam
-split true
-twoCameras true
-left_config  camera/dragonfly2_config_left_bayer_320_240.ini
-right_config camera/dragonfly2_config_right_bayer_320_240.ini
-
-
diff --git a/ergoCubSN000/camera/ServerGrabberDualDragonBayer640_480.ini b/ergoCubSN000/camera/ServerGrabberDualDragonBayer640_480.ini
deleted file mode 100644
index 6cb9c9435b..0000000000
--- a/ergoCubSN000/camera/ServerGrabberDualDragonBayer640_480.ini
+++ /dev/null
@@ -1,9 +0,0 @@
-device grabberDual
-capabilities RAW
-name /icub/cam
-split true
-twoCameras true
-left_config  camera/dragonfly2_config_left_bayer_640_480.ini
-right_config camera/dragonfly2_config_right_bayer_640_480.ini
-
-
diff --git a/ergoCubSN000/camera/dragonfly2_config_left.ini b/ergoCubSN000/camera/dragonfly2_config_left.ini
deleted file mode 100644
index 66a1aff7d7..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_left.ini
+++ /dev/null
@@ -1,19 +0,0 @@
-device dragonfly2
-width 1024
-height 768
-video_type 10
-white_balance 0.561 0.631
-gain 0.0
-shutter 0.512
-name /icub/cam/left
-brightness 0
-DR2
-stamp 
-sharpness 0.518
-hue 0.479
-gamma 0.399
-saturation 0.467
-framerate 15
-#d 1
-guid 00b09d0100c77485
-#<64bit global identifier, without the leading 0x> then remove the d option
diff --git a/ergoCubSN000/camera/dragonfly2_config_left640_480.ini b/ergoCubSN000/camera/dragonfly2_config_left640_480.ini
deleted file mode 100644
index a63b2e3148..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_left640_480.ini
+++ /dev/null
@@ -1,18 +0,0 @@
-device dragonfly2
-width 640
-height 480
-video_type 2
-white_balance 0.514 0.610
-gain 0.0
-shutter 0.554
-name /icub/cam/left
-brightness 0
-DR2
-stamp
-sharpness 0.5
-hue 0.48
-gamma 0.4
-saturation 0.420
-framerate 25
-#d 1
-guid 00b09d0100c77485
diff --git a/ergoCubSN000/camera/dragonfly2_config_leftLowRes.ini b/ergoCubSN000/camera/dragonfly2_config_leftLowRes.ini
deleted file mode 100644
index 67dd150838..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_leftLowRes.ini
+++ /dev/null
@@ -1,19 +0,0 @@
-device dragonfly2
-width 320
-height 240
-video_type 1
-white_balance 0.561 0.631
-gain 0.0
-shutter 0.512
-name /icub/cam/left
-brightness 0
-DR2
-stamp 
-sharpness 0.518
-hue 0.479
-gamma 0.399
-saturation 0.467
-framerate 30
-#d 1
-guid 00b09d0100c77485
-#<64bit global identifier, without the leading 0x> then remove the d option
diff --git a/ergoCubSN000/camera/dragonfly2_config_left_bayer_320_240.ini b/ergoCubSN000/camera/dragonfly2_config_left_bayer_320_240.ini
deleted file mode 100644
index 6df2ee422e..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_left_bayer_320_240.ini
+++ /dev/null
@@ -1,20 +0,0 @@
-device dragonfly2raw
-width 320
-height 240
-video_type 3
-white_balance 0.477 0.514
-gain 0.0
-shutter 0.666
-name /icub/cam/left
-brightness 0
-DR2
-stamp 
-sharpness 1.0
-hue 0.48
-gamma 0.4
-saturation 0.271
-framerate 30
-use_network_time 1
-#d 1
-guid 00b09d0100c77485 
-#<64bit global identifier, without the leading 0x> then remove the d option
diff --git a/ergoCubSN000/camera/dragonfly2_config_left_bayer_640_480.ini b/ergoCubSN000/camera/dragonfly2_config_left_bayer_640_480.ini
deleted file mode 100644
index bd30720164..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_left_bayer_640_480.ini
+++ /dev/null
@@ -1,20 +0,0 @@
-device dragonfly2raw
-width 640
-height 480
-video_type 3
-white_balance 0.477 0.514
-gain 0.0
-shutter 0.666
-name /icub/cam/left
-brightness 0
-DR2
-stamp 
-sharpness 1.0
-hue 0.48
-gamma 0.4
-saturation 0.271
-framerate 30
-use_network_time 1
-#d 1
-guid 00b09d0100c77485 
-#<64bit global identifier, without the leading 0x> then remove the d option
diff --git a/ergoCubSN000/camera/dragonfly2_config_right.ini b/ergoCubSN000/camera/dragonfly2_config_right.ini
deleted file mode 100644
index 80a9fdce4b..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_right.ini
+++ /dev/null
@@ -1,19 +0,0 @@
-device dragonfly2
-width 1024
-height 768
-video_type 10
-white_balance 0.561 0.627
-gain 0.0
-shutter 0.635
-name /icub/cam/right
-brightness 0
-DR2
-stamp
-sharpness 0.499
-hue 0.479
-gamma 0.399
-saturation 0.449
-framerate 15
-#d 0
-guid 00b09d0100bbb395
-#<64bit global identifier, without the leading 0x> then remove the d option
diff --git a/ergoCubSN000/camera/dragonfly2_config_right640_480.ini b/ergoCubSN000/camera/dragonfly2_config_right640_480.ini
deleted file mode 100644
index e85f5f7fed..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_right640_480.ini
+++ /dev/null
@@ -1,18 +0,0 @@
-device dragonfly2
-width 640
-height 480
-video_type 2
-white_balance 0.514 0.610
-gain 0.0
-shutter 0.554
-name /icub/cam/right
-brightness 0
-DR2
-stamp
-sharpness 0.5
-hue 0.48
-gamma 0.4
-saturation 0.420
-framerate 25
-#d 0
-guid 00b09d0100bbb395
diff --git a/ergoCubSN000/camera/dragonfly2_config_rightLowRes.ini b/ergoCubSN000/camera/dragonfly2_config_rightLowRes.ini
deleted file mode 100644
index f948329a60..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_rightLowRes.ini
+++ /dev/null
@@ -1,19 +0,0 @@
-device dragonfly2
-width 320
-height 240
-video_type 1
-white_balance 0.561 0.627
-gain 0.0
-shutter 0.635
-name /icub/cam/right
-brightness 0
-DR2
-stamp
-sharpness 0.499
-hue 0.479
-gamma 0.399
-saturation 0.449
-framerate 30
-#d 0
-guid 00b09d0100bbb395
-#<64bit global identifier, without the leading 0x> then remove the d option
diff --git a/ergoCubSN000/camera/dragonfly2_config_right_bayer_320_240.ini b/ergoCubSN000/camera/dragonfly2_config_right_bayer_320_240.ini
deleted file mode 100644
index 1ca322116e..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_right_bayer_320_240.ini
+++ /dev/null
@@ -1,20 +0,0 @@
-device dragonfly2raw
-width 320
-height 240
-video_type 3
-white_balance 0.477 0.514
-gain 0.0
-shutter 0.666
-name /icub/cam/right
-brightness 0
-DR2
-stamp 
-sharpness 1.0
-hue 0.48
-gamma 0.4
-saturation 0.271
-framerate 30
-use_network_time 1
-#d 0
-guid 00b09d0100bbb395
-#<64bit global identifier, without the leading 0x> then remove the d option
diff --git a/ergoCubSN000/camera/dragonfly2_config_right_bayer_640_480.ini b/ergoCubSN000/camera/dragonfly2_config_right_bayer_640_480.ini
deleted file mode 100644
index 56b8fe7c3f..0000000000
--- a/ergoCubSN000/camera/dragonfly2_config_right_bayer_640_480.ini
+++ /dev/null
@@ -1,20 +0,0 @@
-device dragonfly2raw
-width 640
-height 480
-video_type 3
-white_balance 0.477 0.514 
-gain 0.0 
-shutter 0.666
-name /icub/cam/right  
-brightness 0
-DR2
-stamp
-sharpness 1.0
-hue 0.48
-gamma 0.4
-saturation 0.271
-framerate 30
-use_network_time 1
-#d 0
-guid 00b09d0100bbb395
-#<64bit global identifier, without the leading 0x> then remove the d option
diff --git a/ergoCubSN000/ergocub_head.xml b/ergoCubSN000/head.xml
similarity index 86%
rename from ergoCubSN000/ergocub_head.xml
rename to ergoCubSN000/head.xml
index 0ec8518202..d524230d40 100644
--- a/ergoCubSN000/ergocub_head.xml
+++ b/ergoCubSN000/head.xml
@@ -17,6 +17,11 @@
     <xi:include href="hardware/motorControl/head-eb21-j2_3-mc.xml" />
 
     <xi:include href="calibrators/head-calib.xml" />
+
+    <!-- Head sensors-->
+    <xi:include href="sensors/realsense.xml" />
+    <xi:include href="sensors/rplidar.xml" />
+
   </devices>
 
 </robot>
diff --git a/ergoCubSN000/sensors.xml b/ergoCubSN000/sensors.xml
new file mode 100644
index 0000000000..4304365250
--- /dev/null
+++ b/ergoCubSN000/sensors.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN000" build="1" portprefix="/ergocub" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+  <devices>
+    <xi:include href="sensors/rplidar.xml" />
+    <xi:include href="sensors/realsense.xml" />
+  </devices>
+
+</robot>
diff --git a/ergoCubSN000/sensors/RealSense_conf.ini b/ergoCubSN000/sensors/RealSense_conf.ini
deleted file mode 100644
index 82f8b437dd..0000000000
--- a/ergoCubSN000/sensors/RealSense_conf.ini
+++ /dev/null
@@ -1,13 +0,0 @@
-device       RGBDSensorWrapper
-subdevice    realsense2withIMU
-name         /depthCamera
-
-[SETTINGS]
-depthResolution (640 480)    #Note the parentesys
-rgbResolution   (640 480)
-framerate       30
-enableEmitter   true
-alignmentFrame  RGB
-
-[HW_DESCRIPTION]
-clipPlanes (0.2 10.0)
diff --git a/ergoCubSN000/sensors/realsense.xml b/ergoCubSN000/sensors/realsense.xml
new file mode 100644
index 0000000000..18fd952ada
--- /dev/null
+++ b/ergoCubSN000/sensors/realsense.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+ <devices>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="realsenseD450" type="realsense2">
+        <group name="SETTINGS">
+            <param name="depthResolution"> (640 480) </param>
+            <param name="rgbResolution">  (640 480) </param>
+            <param name="framerate">    30  </param>
+            <param name="enableEmitter"> true </param>
+            <param name="alignmentFrame"> RGB </param>
+        </group>
+        <group name="HW_DESCRIPTION">
+            <param name="clipPlanes"> (0.2 10.0) </param>
+        </group>
+        <group name="QUANT_PARAM">
+            <param name="depth_quant"> 2 </param>
+        </group>
+        <param name="rotateImage180"> false </param>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="RGBDWrapperyarp" type="rgbdSensor_nws_yarp">
+        <param name="period"> 0.03 </param>
+        <param name="name">    /ergocub/depthCamera </param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="subdevicergbd"> realsenseD450 </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach" />
+    </device>
+
+ </devices>
+
diff --git a/ergoCubSN000/sensors/rplidar.xml b/ergoCubSN000/sensors/rplidar.xml
new file mode 100644
index 0000000000..40084679b1
--- /dev/null
+++ b/ergoCubSN000/sensors/rplidar.xml
@@ -0,0 +1,37 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<devices>
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="rpLidarS2" type="rpLidar4">
+       <group name="GENERAL">
+            <param name="name">                rpLidar4      </param>
+            <param name="serial_baudrate">     1000000       </param>
+            <param name="serial_port">         /dev/ttyUSB0  </param>
+            <param name="sample_buffer_life">  1             </param>
+       </group>
+       <group name="SENSOR">
+            <param name="max_distance">    30.0         </param>
+            <param name="min_distance">    0.1          </param>
+            <param name="max_angle">       360          </param>
+            <param name="min_angle">       0            </param>
+            <param name="resolution">      0.12         </param>
+            <param name="allow_infinity">  1            </param>
+        </group>
+        <group name="RPLIDAR">
+            <param name="scan_mode">  DenseBoost        </param>
+        </group>
+    </device>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="laser_wrapper_yarp" type="rangefinder2D_nws_yarp">
+        <param name="period">  0.01      </param>
+        <param name="name">    /ergocub/laser:o  </param>
+        <action phase="startup" level="10" type="attach">
+           <paramlist name="networks">
+              <elem name="laser"> rpLidarS2 </elem>
+           </paramlist>
+        </action>
+      <action phase="shutdown" level="15" type="detach"/>
+    </device>
+
+</devices>
+