diff --git a/ergoCubSN000/ft.xml b/ergoCubSN000/ft.xml
index 5aa745ca63..555e18a222 100644
--- a/ergoCubSN000/ft.xml
+++ b/ergoCubSN000/ft.xml
@@ -1,7 +1,7 @@
 <?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="iCubGenovergoCubSN000" build="1" portprefix="ergocub" xmlns:xi="http://www.w3.org/2001/XInclude">
+<robot name="ergoCubSN000" build="1" portprefix="ergocub" xmlns:xi="http://www.w3.org/2001/XInclude">
 
     <params>
     <xi:include href="hardware/electronics/pc104.xml" />
diff --git a/ergoCubSN000/general.xml b/ergoCubSN000/general.xml
index 2cbeb1f408..5be004046e 100644
--- a/ergoCubSN000/general.xml
+++ b/ergoCubSN000/general.xml
@@ -5,7 +5,7 @@
   
   <group name="GENERAL">
       <param name="skipCalibration">    false  </param>
-      <param name="useRawEncoderData">  false   </param>
+      <param name="useRawEncoderData">  false  </param>
       <param name="useLimitedPWM">      false  </param>
       <param name="verbose">            false  </param>
   </group>
diff --git a/ergoCubSN001/battery_test.xml b/ergoCubSN001/battery_test.xml
new file mode 100644
index 0000000000..58ffefe469
--- /dev/null
+++ b/ergoCubSN001/battery_test.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="ergoCubSN001" build="1" portprefix="/ergocub" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+  <devices>
+    <xi:include href="hardware/battery/battery.xml" />
+    <xi:include href="wrappers/battery/battery.xml" />
+  </devices>
+
+</robot>
diff --git a/ergoCubSN001/calibrators/left_arm-calib.xml b/ergoCubSN001/calibrators/left_arm-calib.xml
index 18b2778e77..fa1384a758 100644
--- a/ergoCubSN001/calibrators/left_arm-calib.xml
+++ b/ergoCubSN001/calibrators/left_arm-calib.xml
@@ -12,19 +12,19 @@
 
 	<group name="HOME">
 	<!--                                shoulder-pitch    shoulder-roll    shoulder-yaw    elbow    wrist-yaw    wrist-roll    wrist-pitch    thumb_add    thumb_oc    index_add    index_oc    middle_oc    ring_pinky_oc -->
-		<param name="positionHome">            5                 30              0           10         0            0             0             0.00         0.00        0.00         0.00        0.00           0.00    </param>
+		<param name="positionHome">            5                 30              0           10         0            0             0             30.00        0.00        0.00         0.00        0.00           0.00    </param>
 		<param name="velocityHome">            10                10              10          10         10           10            10            40.00        40.00       40.00        40.00       40.00          40.00    </param>
 	</group>
 	<group name="CALIBRATION">
 		<param name="calibrationType">        10                 10              10          10         12           12            12            12           14          12           14          14             14       </param>
-		<param name="calibration1">           4000               -3000           -3000       4000       17565        8161          21461         0            150         0            150         150            150      </param>
-		<param name="calibration2">	          0                  0               0           0          0            0             0             0            10923       0            14564       13653          12743    </param>
+		<param name="calibration1">           4000               -3000           -3000       4000       17565        8161          21461         0            300         0            300         300            300      </param>
+		<param name="calibration2">	          0                  0               0           0          0            0             0             0            0           0            0           0              0        </param>
 		<param name="calibration3">           0                  0               0           0          0            0             0             0            1           0            0           0              1        </param>
-		<param name="calibration4">           0                  0               0           0          0            0             0             0            180         0            0           0              0        </param>
-		<param name="calibration5">           0                  0               0           0          0            0             0             0            -273.0      0            -261.0      -259.0         -296.0   </param>
+		<param name="calibration4">           0                  0               0           0          0            0             0             0            32768       0            0           0              0        </param>
+		<param name="calibration5">           0                  0               0           0          0            0             0             0            18277       0            47003       47513          54558    </param>
 		<param name="calibrationZero">        35                -15             -52          -5         0            0             0             0            0           0            0           0              0        </param>
 		<param name="calibrationDelta">       0                  0               0           0          0            0             0             0            0           0            0           0              0        </param>
-		<param name="startupPosition">        34                 50              -10         90         0.0          0.0           0.0           0.0          0.0         0.0          0.0         0.0            0.0      </param>
+		<param name="startupPosition">        34                 50              -10         90         0.0          0.0           0.0           0.0          4.0         0.0          4.0         4.0            4.0      </param>
 		<param name="startupVelocity">        10.0               10.0            10.0        10.0       10.0         10.0          10.0          100.0        100.0       0.0          100         100.0          100.0    </param>
 		<param name="startupMaxPwm">          8000               8000            8000        8000       16000        16000         16000         0            0           0            0           0              0        </param>
 		<param name="startupPosThreshold">    2                  2               2           2          2            2             2             90           5          90            5           5              5        </param>
diff --git a/ergoCubSN001/calibrators/right_arm-calib.xml b/ergoCubSN001/calibrators/right_arm-calib.xml
index daf7065bae..8bb1555157 100644
--- a/ergoCubSN001/calibrators/right_arm-calib.xml
+++ b/ergoCubSN001/calibrators/right_arm-calib.xml
@@ -13,19 +13,19 @@
 	<group name="HOME">
 	<!-- For calib6 to set calibration5, i.e. target just multiply desidered pos in deg by 182,044444 (2^(16)/360) -->
     <!--                                shoulder-pitch    shoulder-roll    shoulder-yaw    elbow    wrist-yaw    wrist-roll    wrist-pitch    thumb_add    thumb_oc    index_add    index_oc    middle_oc    ring_pinky_oc -->
-		<param name="positionHome">            5                30              0           10         0             0              0            0.00        0.00        30.00        0.00        0.00        0.00       </param>
+		<param name="positionHome">            5                30              0           10         0             0              0            30.00        0.00        30.00        0.00        0.00        0.00       </param>
 		<param name="velocityHome">            10               10              10          10         10            10             10           40.00        40.00       40.00        40.00       40.00       40.00       </param>
 	</group>
 	<group name="CALIBRATION">
 		<param name="calibrationType">         10                10             10          10         12            12             12           12           14          12           14          14          14          </param>
-		<param name="calibration1">           -4000              3000           3000       -4000       19023         28993          36030        0            150         0            150         150         150         </param>
-		<param name="calibration2">	       0                 0              0           0          0             0              0            0            9000        0            10923       13653       9000       </param>
+		<param name="calibration1">           -4000              3000           3000       -4000       11796         13254          16440        0            300         0            300         300         300         </param>
+		<param name="calibration2">	           0                 0              0           0          0             0              0            0            0           0            0           0           0           </param>
 		<param name="calibration3">            0                 0              0           0          0             0              0            0            1           0            1           0           0           </param>
-		<param name="calibration4">            0                 0              0           0          0             0              0            0            180         0            0           0           0           </param>
-		<param name="calibration5">            0                 0              0           0          0             0              0            0            -184.0      0            -294.0      -247.0      -65.0       </param>
+		<param name="calibration4">            0                 0              0           0          0             0              0            0            32768       0            0           0           0           </param>
+		<param name="calibration5">            0                 0              0           0          0             0              0            0            2366        0            53484       45147       12415        </param>
 		<param name="calibrationZero">         35               -15            -52         -5          0             0              0            0            0           0            0           0           0           </param>
 		<param name="calibrationDelta">        0                 0              0           0          0             0              0            0            0           0            0           0           0           </param>
-		<param name="startupPosition">         34                50             -10         90         0.0           0.0            0.0          0.0          0.0         0.0          0.0         0.0         0           </param>
+		<param name="startupPosition">         34                50             -10         90         0.0           0.0            0.0          0.0          4.0         0.0          4.0         4.0         4.0         </param>
 		<param name="startupVelocity">         10.0              10.0           10.0        10.0       10.0          10.0           10.0         30.0         30.0        30.0         30.0        30.0        30.0        </param>
 		<param name="startupMaxPwm">           8000              8000           8000        8000       16000         16000          16000        0            0           0            0           0           0           </param>
 		<param name="startupPosThreshold">     2                 2              2           2          2             2              2            90           5           90           5           5           5           </param>
diff --git a/ergoCubSN001/calibrators/torso-calib.xml b/ergoCubSN001/calibrators/torso-calib.xml
index a40d1264ed..7866ae5dc4 100644
--- a/ergoCubSN001/calibrators/torso-calib.xml
+++ b/ergoCubSN001/calibrators/torso-calib.xml
@@ -39,7 +39,7 @@
 
  <!-- Joint 1 has been removed from the calibration order for motor safety -->
  <!-- <param name="CALIB_ORDER">(0) (1) (2) </param> --> <!-- Don't remove this line -->
-		<param name="CALIB_ORDER">(0) (2) </param>
+		<param name="CALIB_ORDER">(0) (1) (2) </param>
 
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">torso-eb5-j0_2-mc</param>
diff --git a/ergoCubSN001/ergocub_all.xml b/ergoCubSN001/ergocub_all.xml
index 18a5194099..65b427f080 100644
--- a/ergoCubSN001/ergocub_all.xml
+++ b/ergoCubSN001/ergocub_all.xml
@@ -94,6 +94,8 @@
     <xi:include href="wrappers/inertials/head-imuFilter.xml" />
     <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
     <xi:include href="hardware/inertials/head-inertial.xml" />
+    <xi:include href="hardware/inertials/waist-inertial.xml" />
+    <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" />
 
     <!-- IMU - MTB4 BOARDS -->
     <xi:include href="hardware/inertials/left_arm-eb4-j2_3-inertial.xml" />
@@ -112,6 +114,10 @@
     <xi:include href="wrappers/inertials/right_arm-imu_wrapper.xml" />
     <xi:include href="wrappers/inertials/left_foot-imu_wrapper.xml" />
     <xi:include href="wrappers/inertials/right_foot-imu_wrapper.xml" />
+    
+    <!-- BATTERY-->
+    <xi:include href="hardware/battery/battery.xml" />
+    <xi:include href="wrappers/battery/battery.xml" />
 
     <!--  CALIBRATORS -->
     <xi:include href="calibrators/torso-calib.xml" />
diff --git a/ergoCubSN001/ergocub_all_ros2.xml b/ergoCubSN001/ergocub_all_ros2.xml
index 64ed429777..70963eb8ef 100644
--- a/ergoCubSN001/ergocub_all_ros2.xml
+++ b/ergoCubSN001/ergocub_all_ros2.xml
@@ -96,6 +96,13 @@
     <xi:include href="wrappers/inertials/head-imuFilter.xml" />
     <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
     <xi:include href="hardware/inertials/head-inertial.xml" />
+    <xi:include href="hardware/inertials/waist-inertial.xml" />
+    <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" />
+
+    
+    <!-- BATTERY-->
+    <xi:include href="hardware/battery/battery.xml" />
+    <xi:include href="wrappers/battery/battery.xml" />
 
     <!--  CALIBRATORS -->
     <xi:include href="calibrators/torso-calib.xml" />
diff --git a/ergoCubSN001/ergocub_wbd.xml b/ergoCubSN001/ergocub_wbd.xml
index b7c021fdb5..920c42d3b1 100644
--- a/ergoCubSN001/ergocub_wbd.xml
+++ b/ergoCubSN001/ergocub_wbd.xml
@@ -93,6 +93,8 @@
     <xi:include href="wrappers/inertials/head-imuFilter.xml" />
     <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
     <xi:include href="hardware/inertials/head-inertial.xml" />
+    <xi:include href="hardware/inertials/waist-inertial.xml" />
+    <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" />
 
     <!-- FT SENSORS - IMU -->
     <xi:include href="hardware/inertials/left_arm-eb2-j0_1-inertial.xml" />
@@ -109,6 +111,10 @@
     <xi:include href="wrappers/inertials/right_leg-imu_wrapper.xml" />
     <xi:include href="wrappers/inertials/left_foot-imu_wrapper.xml" />
     <xi:include href="wrappers/inertials/right_foot-imu_wrapper.xml" />
+    
+    <!-- BATTERY-->
+    <xi:include href="hardware/battery/battery.xml" />
+    <xi:include href="wrappers/battery/battery.xml" />
 
     <!--  CALIBRATORS -->
     <xi:include href="calibrators/torso-calib.xml" />
diff --git a/ergoCubSN001/ergocub_wbd_ros2.xml b/ergoCubSN001/ergocub_wbd_ros2.xml
new file mode 100644
index 0000000000..48cb5f1138
--- /dev/null
+++ b/ergoCubSN001/ergocub_wbd_ros2.xml
@@ -0,0 +1,137 @@
+<?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="ergoCubSN001" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <params>
+    <xi:include href="hardware/electronics/pc104.xml" />
+    </params>
+
+    <devices>
+
+    <!-- POS4 -->
+    <xi:include href="wrappers/POS/right_hand-pos_wrapper4.xml" />
+    <xi:include href="hardware/POS/right_hand-pos4.xml" />
+    <xi:include href="wrappers/POS/left_hand-pos_wrapper4.xml" />
+    <xi:include href="hardware/POS/left_hand-pos4.xml" />
+
+    <!-- POS2 -->
+    <xi:include href="wrappers/POS/right_hand-pos_wrapper2.xml" />
+    <xi:include href="hardware/POS/right_hand-pos2.xml" />
+    <xi:include href="wrappers/POS/left_hand-pos_wrapper2.xml" />
+    <xi:include href="hardware/POS/left_hand-pos2.xml" />
+
+    <!-- motor controllers wrappers -->
+    <xi:include href="wrappers/motorControl/left_leg-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/right_leg-mc_wrapper.xml" />
+
+    <xi:include href="wrappers/motorControl/left_leg-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/right_leg-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" />
+
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/left_arm-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/torso-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/torso-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/head-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/head-mc_remapper.xml" />
+
+    <!-- HEAD -->
+    <xi:include href="hardware/motorControl/head-eb20-j0_1-mc.xml" />
+    <xi:include href="hardware/motorControl/head-eb21-j2_3-mc.xml" />
+
+    <!-- TORSO -->
+    <xi:include href="hardware/motorControl/torso-eb5-j0_2-mc.xml" />
+
+    <!-- LEFT ARM -->
+    <xi:include href="hardware/motorControl/left_arm-eb2-j0_1-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb4-j2_3-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb31-j4_6-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb23-j7_10-mc.xml" />
+    <xi:include href="hardware/motorControl/left_arm-eb25-j11_12-mc.xml" />
+
+    <!-- RIGHT ARM -->
+    <xi:include href="hardware/motorControl/right_arm-eb1-j0_1-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb3-j2_3-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb30-j4_6-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb22-j7_10-mc.xml" />
+    <xi:include href="hardware/motorControl/right_arm-eb24-j11_12-mc.xml" />
+
+    <!-- LEFT LEG -->
+    <xi:include href="hardware/motorControl/left_leg-eb8-j0_3-mc.xml" />
+    <xi:include href="hardware/motorControl/left_leg-eb9-j4_5-mc.xml" />
+
+    <!-- RIGHT LEG -->
+    <xi:include href="hardware/motorControl/right_leg-eb6-j0_3-mc.xml" />
+    <xi:include href="hardware/motorControl/right_leg-eb7-j4_5-mc.xml" />
+
+    <!-- FT SENSORS -->
+    <xi:include href="hardware/FT/left_arm-eb2-j0_1-strain.xml" />
+    <xi:include href="hardware/FT/right_arm-eb1-j0_1-strain.xml" />
+    <xi:include href="hardware/FT/left_leg-eb9-j4_5-strain.xml" />
+    <xi:include href="hardware/FT/right_leg-eb7-j4_5-strain.xml" />
+    <xi:include href="hardware/FT/left_leg-eb8-j0_3-strain.xml" />
+    <xi:include href="hardware/FT/right_leg-eb6-j0_3-strain.xml" />
+
+    <!-- FT SENSORS - REMAPPERS -->
+    <xi:include href="wrappers/FT/left_leg-FT_remapper.xml" />
+    <xi:include href="wrappers/FT/right_leg-FT_remapper.xml" />
+
+    <!-- FT SENSORS - MULTIPLE ANALOG SENSOR SERVERS -->
+    <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/left_leg-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/right_leg-FT_wrapper.xml" />
+
+    <!-- INERTIAL SENSOR -->
+    <xi:include href="wrappers/inertials/head-imuFilter_wrapper.xml" />
+    <xi:include href="wrappers/inertials/head-imuFilter.xml" />
+    <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
+    <xi:include href="hardware/inertials/head-inertial.xml" />
+
+    <!-- FT SENSORS - IMU -->
+    <xi:include href="hardware/inertials/left_arm-eb2-j0_1-inertial.xml" />
+    <xi:include href="hardware/inertials/right_arm-eb1-j0_1-inertial.xml" />
+    <xi:include href="hardware/inertials/left_leg-eb8-j0_3-inertial.xml" />
+    <xi:include href="hardware/inertials/right_leg-eb6-j0_3-inertial.xml" />
+    <xi:include href="hardware/inertials/left_leg-eb9-j4_5-inertial.xml" />
+    <xi:include href="hardware/inertials/right_leg-eb7-j4_5-inertial.xml" />
+
+    <!-- FT SENSORS - IMU - MULTIPLE ANALOG SENSOR SERVERS -->
+    <xi:include href="wrappers/inertials/left_arm-imu_wrapper.xml" />
+    <xi:include href="wrappers/inertials/right_arm-imu_wrapper.xml" />
+    <xi:include href="wrappers/inertials/left_leg-imu_wrapper.xml" />
+    <xi:include href="wrappers/inertials/right_leg-imu_wrapper.xml" />
+    <xi:include href="wrappers/inertials/left_foot-imu_wrapper.xml" />
+    <xi:include href="wrappers/inertials/right_foot-imu_wrapper.xml" />
+    
+    <!-- BATTERY-->
+    <xi:include href="hardware/battery/battery.xml" />
+    <xi:include href="wrappers/battery/battery.xml" />
+
+    <!--  CALIBRATORS -->
+    <xi:include href="calibrators/torso-calib.xml" />
+    <xi:include href="calibrators/left_leg-calib.xml" />
+    <xi:include href="calibrators/right_leg-calib.xml" />
+    <xi:include href="calibrators/left_arm-calib.xml" />
+    <xi:include href="calibrators/right_arm-calib.xml" />
+    <xi:include href="calibrators/head-calib.xml" />
+    <!-- ROS2 -->
+    <!--<xi:include href="wrappers/inertials/imu_ros2.xml" />-->
+    <xi:include href="wrappers/motorControl/alljoints-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/alljoints-mc_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/left_arm-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/right_arm-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/left_foot-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/right_foot-FT_nws_ros2.xml" />
+
+    <!-- estimators -->
+    <xi:include href="estimators/wholebodydynamics.xml" />
+
+  </devices>
+</robot>
diff --git a/ergoCubSN001/estimators/wholebodydynamics.xml b/ergoCubSN001/estimators/wholebodydynamics.xml
index c279561309..039f2b41b3 100644
--- a/ergoCubSN001/estimators/wholebodydynamics.xml
+++ b/ergoCubSN001/estimators/wholebodydynamics.xml
@@ -3,7 +3,7 @@
 
 
     <device xmlns:xi="http://www.w3.org/2001/XInclude" name="wholebodydynamics" type="wholebodydynamics">
-        <param name="axesNames">(torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_roll,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_roll,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_roll,l_wrist_pitch,l_wrist_yaw,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_roll,r_wrist_pitch,r_wrist_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         <param name="modelFile">model.urdf</param>
         <param name="fixedFrameGravity">(0,0,-9.81)</param>
 	<param name="defaultContactFrames">(l_hand_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param>
@@ -34,7 +34,7 @@
         <group name="GRAVITY_COMPENSATION">
             <param name="enableGravityCompensation">true</param>
             <param name="gravityCompensationBaseLink">root_link</param>
-            <param name="gravityCompensationAxesNames">(torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+            <param name="gravityCompensationAxesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch,neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
         </group>
 
 
diff --git a/ergoCubSN001/hardware/FT/left_arm-eb2-j0_1-strain.xml b/ergoCubSN001/hardware/FT/left_arm-eb2-j0_1-strain.xml
index b7ec136d8b..ce9c2c6c42 100644
--- a/ergoCubSN001/hardware/FT/left_arm-eb2-j0_1-strain.xml
+++ b/ergoCubSN001/hardware/FT/left_arm-eb2-j0_1-strain.xml
@@ -25,7 +25,7 @@
                     </group>                    
                     <group name="FIRMWARE">
                         <param name="major">            2                       </param>    
-                        <param name="minor">            2                       </param> 
+                        <param name="minor">            2                       </param>
                         <param name="build">            0                       </param>
                     </group>
                 </group>
diff --git a/ergoCubSN001/hardware/FT/left_leg-eb8-j0_3-strain.xml b/ergoCubSN001/hardware/FT/left_leg-eb8-j0_3-strain.xml
index 68572dfdbb..3355b9a6ab 100644
--- a/ergoCubSN001/hardware/FT/left_leg-eb8-j0_3-strain.xml
+++ b/ergoCubSN001/hardware/FT/left_leg-eb8-j0_3-strain.xml
@@ -38,7 +38,7 @@
             </group>
 
             <group name="SETTINGS">
-                <param name="enabledSensors">       l_leg_ft               </param>
+                <param name="enabledSensors">       l_leg_ft              </param>
                 <param name="ftPeriod">             2         		                    </param>
                 <param name="temperaturePeriod">    1000      		                   </param>
                 <param name="useCalibration">       true      		              </param>
diff --git a/ergoCubSN001/hardware/FT/left_leg-eb9-j4_5-strain.xml b/ergoCubSN001/hardware/FT/left_leg-eb9-j4_5-strain.xml
index 26f172e9bc..4396145c23 100644
--- a/ergoCubSN001/hardware/FT/left_leg-eb9-j4_5-strain.xml
+++ b/ergoCubSN001/hardware/FT/left_leg-eb9-j4_5-strain.xml
@@ -24,13 +24,13 @@
                     </group>                    
                     <group name="FIRMWARE">
                         <param name="major">            2   		            </param>    
-                        <param name="minor">            2  		            </param> 
+                        <param name="minor">            2 		            </param>
                         <param name="build">            0  		       </param>
                     </group>
                 </group>
                 
                 <group name="SENSORS">
-                    <param name="id">                   l_foot_rear_ft	l_foot_front_ft  </param>
+                    <param name="id">                   l_foot_rear_ft	l_foot_front_ft   </param>
                     <param name="board">                 strain2		strain2             </param>
                     <param name="location">             CAN2:13		CAN2:14                 </param>
                 </group>                
diff --git a/ergoCubSN001/hardware/FT/right_arm-eb1-j0_1-strain.xml b/ergoCubSN001/hardware/FT/right_arm-eb1-j0_1-strain.xml
index 94d9bd9671..81627ec384 100644
--- a/ergoCubSN001/hardware/FT/right_arm-eb1-j0_1-strain.xml
+++ b/ergoCubSN001/hardware/FT/right_arm-eb1-j0_1-strain.xml
@@ -25,13 +25,13 @@
                     </group>                    
                     <group name="FIRMWARE">
                         <param name="major">            2                       </param>    
-                        <param name="minor">            2                       </param> 
+                        <param name="minor">            2                       </param>
                         <param name="build">            0                       </param>
                     </group>
                 </group>
                 
                 <group name="SENSORS">
-                    <param name="id">                   r_arm_ft  </param>
+                    <param name="id">                   r_arm_ft   </param>
                     <param name="board">                 strain2             </param>
                     <param name="location">             CAN2:13                 </param>
                 </group>                
diff --git a/ergoCubSN001/hardware/FT/right_leg-eb7-j4_5-strain.xml b/ergoCubSN001/hardware/FT/right_leg-eb7-j4_5-strain.xml
index 2b866a63e3..e341f9a886 100644
--- a/ergoCubSN001/hardware/FT/right_leg-eb7-j4_5-strain.xml
+++ b/ergoCubSN001/hardware/FT/right_leg-eb7-j4_5-strain.xml
@@ -24,13 +24,13 @@
                     </group>                    
                     <group name="FIRMWARE">
                         <param name="major">            2   		            </param>    
-                        <param name="minor">            2  		            </param> 
+                        <param name="minor">            2  		            </param>
                         <param name="build">            0  		       </param>
                     </group>
                 </group>
                 
                 <group name="SENSORS">
-                    <param name="id">                   r_foot_rear_ft	r_foot_front_ft  </param>
+                    <param name="id">                   r_foot_rear_ft	r_foot_front_ft   </param>
                     <param name="board">                 strain2		strain2             </param>
                     <param name="location">             CAN2:13		CAN2:14                 </param>
                 </group>                
diff --git a/ergoCubSN001/hardware/POS/left_hand-pos2.xml b/ergoCubSN001/hardware/POS/left_hand-pos2.xml
index 9706e866e0..e3d1bfe274 100644
--- a/ergoCubSN001/hardware/POS/left_hand-pos2.xml
+++ b/ergoCubSN001/hardware/POS/left_hand-pos2.xml
@@ -20,7 +20,7 @@
                 </group>
                 <group name="FIRMWARE">
                     <param name="major">            2                       </param>
-                    <param name="minor">            0                       </param>
+                    <param name="minor">            1                       </param>
                     <param name="build">            0                       </param>
                 </group>
             </group>
@@ -56,4 +56,4 @@
 
     </group>
 
-</device>
\ No newline at end of file
+</device>
diff --git a/ergoCubSN001/hardware/POS/left_hand-pos4.xml b/ergoCubSN001/hardware/POS/left_hand-pos4.xml
index f201e542eb..1d3b5b1044 100644
--- a/ergoCubSN001/hardware/POS/left_hand-pos4.xml
+++ b/ergoCubSN001/hardware/POS/left_hand-pos4.xml
@@ -20,7 +20,7 @@
                 </group>
                 <group name="FIRMWARE">
                     <param name="major">            2                       </param>
-                    <param name="minor">            0                       </param>
+                    <param name="minor">            1                       </param>
                     <param name="build">            0                       </param>
                 </group>
             </group>
@@ -44,7 +44,6 @@
                     <param name="offset">           -273            -261             -259             -296              </param>
                     <param name="invertDirection">  true            false            false            true              </param>
                 </group>
-
             </group>
 
         </group>
@@ -56,4 +55,4 @@
 
     </group>
 
-</device>
\ No newline at end of file
+</device>
diff --git a/ergoCubSN001/hardware/battery/battery.xml b/ergoCubSN001/hardware/battery/battery.xml
new file mode 100644
index 0000000000..9de6e5dfc9
--- /dev/null
+++ b/ergoCubSN001/hardware/battery/battery.xml
@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE device PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="canbattery" type="embObjBattery">
+
+    <xi:include href="../../general.xml" />
+    <xi:include href="../electronics/left_arm-eb2-j0_1-eln.xml" />
+
+    <group name="SERVICE">
+
+        <param name="type"> eomn_serv_AS_battery </param>
+
+        <group name="PROPERTIES">
+
+            <group name="CANBOARDS">
+                <param name="type">                 bms             </param>
+
+                <group name="PROTOCOL">
+                    <param name="major">            0                   </param>
+                    <param name="minor">            0                   </param>
+                </group>
+                <group name="FIRMWARE">
+                    <param name="major">            1                   </param>
+                    <param name="minor">            2                   </param>
+                    <param name="build">            0                   </param>
+                </group>
+            </group>
+
+            <group name="SENSORS">
+                <param name="id">                   battery1        </param>
+                <param name="board">                bms             </param>
+                <param name="location">             CAN1:1          </param>
+            </group>
+
+        </group>
+
+
+        <group name="SETTINGS">
+            <param name="enabledSensors">            battery1        </param>
+            <param name="acquisitionRate">           1000            </param>   <!-- msec -->
+        </group>
+
+
+    </group>
+
+</device>
+
diff --git a/ergoCubSN001/hardware/inertials/head-inertial.xml b/ergoCubSN001/hardware/inertials/head-inertial.xml
index 08f1257b10..bb91c07b93 100644
--- a/ergoCubSN001/hardware/inertials/head-inertial.xml
+++ b/ergoCubSN001/hardware/inertials/head-inertial.xml
@@ -24,8 +24,8 @@
                     </group>
                     <group name="FIRMWARE">
                         <param name="major">            1                   </param>
-                        <param name="minor">            2                   </param>
-                        <param name="build">            3                   </param>
+                        <param name="minor">            3                   </param>
+                        <param name="build">            0                   </param>
                     </group>
                 </group>
 
diff --git a/ergoCubSN001/hardware/inertials/left_arm-eb4-j2_3-inertial.xml b/ergoCubSN001/hardware/inertials/left_arm-eb4-j2_3-inertial.xml
index 85011de4e5..7ce7cbefcc 100644
--- a/ergoCubSN001/hardware/inertials/left_arm-eb4-j2_3-inertial.xml
+++ b/ergoCubSN001/hardware/inertials/left_arm-eb4-j2_3-inertial.xml
@@ -23,8 +23,8 @@
                     </group>                    
                     <group name="FIRMWARE">
                         <param name="major">            1                   </param>    
-                        <param name="minor">            4                   </param> 
-                        <param name="build">            7                   </param>
+                        <param name="minor">            21                   </param> 
+                        <param name="build">            0                   </param>
                     </group>
                 </group>
 
@@ -55,4 +55,4 @@
             
         </group>        
         
-    </device>
\ No newline at end of file
+    </device>
diff --git a/ergoCubSN001/hardware/inertials/right_arm-eb1-j0_1-inertial.xml b/ergoCubSN001/hardware/inertials/right_arm-eb1-j0_1-inertial.xml
index ae0a7dc0c1..eb2d0a4e33 100644
--- a/ergoCubSN001/hardware/inertials/right_arm-eb1-j0_1-inertial.xml
+++ b/ergoCubSN001/hardware/inertials/right_arm-eb1-j0_1-inertial.xml
@@ -17,13 +17,13 @@
                     <param name="type">             eobrd_strain2           </param>
 
                     <group name="PROTOCOL">
-                        <param name="major">            0                   </param>
+                        <param name="major">            2                   </param>
                         <param name="minor">            0                   </param>
                     </group>
 
                     <group name="FIRMWARE">
-                        <param name="major">            0                   </param>
-                        <param name="minor">            0                   </param>
+                        <param name="major">            2                   </param>
+                        <param name="minor">            2                   </param>
                         <param name="build">            0                   </param>
                     </group>
                 </group>
diff --git a/ergoCubSN001/hardware/inertials/right_arm-eb3-j2_3-inertial.xml b/ergoCubSN001/hardware/inertials/right_arm-eb3-j2_3-inertial.xml
index d7fc20121f..d113bffae0 100644
--- a/ergoCubSN001/hardware/inertials/right_arm-eb3-j2_3-inertial.xml
+++ b/ergoCubSN001/hardware/inertials/right_arm-eb3-j2_3-inertial.xml
@@ -24,8 +24,8 @@
                     </group>                    
                     <group name="FIRMWARE">
                         <param name="major">            1                   </param>    
-                        <param name="minor">            4                   </param> 
-                        <param name="build">            7                   </param>
+                        <param name="minor">            21                   </param> 
+                        <param name="build">            0                   </param>
                     </group>
                 </group>
 
@@ -56,4 +56,4 @@
             
         </group>        
         
-    </device>
\ No newline at end of file
+    </device>
diff --git a/ergoCubSN001/hardware/inertials/waist-inertial.xml b/ergoCubSN001/hardware/inertials/waist-inertial.xml
new file mode 100644
index 0000000000..ad82a697ca
--- /dev/null
+++ b/ergoCubSN001/hardware/inertials/waist-inertial.xml
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+  <device type="xsensmt" name="waist-inertial">
+    <param name="serial">  /dev/ttyS0   </param>
+    <param name="xsensmt_period">0.01</param>
+    <param name="xsensmt_euler_period">0.005</param>
+    <param name="xsensmt_gyro_period">0.005</param>
+    <param name="xsensmt_acc_period">0.005</param>
+    <param name="xsensmt_mag_period">0.01</param>
+  </device>
\ No newline at end of file
diff --git a/ergoCubSN001/hardware/mechanicals/left_leg-eb9-j4_5-mec.xml b/ergoCubSN001/hardware/mechanicals/left_leg-eb9-j4_5-mec.xml
index 59657a7494..ba50863ff7 100644
--- a/ergoCubSN001/hardware/mechanicals/left_leg-eb9-j4_5-mec.xml
+++ b/ergoCubSN001/hardware/mechanicals/left_leg-eb9-j4_5-mec.xml
@@ -34,7 +34,7 @@
         <param name="HasRotorEncoder">           1             1   </param>
         <param name="HasRotorEncoderIndex">      1             1   </param>
         <param name="HasSpeedEncoder">           0             0   </param>
-        <param name="RotorIndexOffset">          152           209 </param>
+        <param name="RotorIndexOffset">          152           33  </param>
         <param name="MotorPoles">                12            8   </param>
     </group>
 
diff --git a/ergoCubSN001/hardware/mechanicals/right_arm-eb1-j0_1-mec.xml b/ergoCubSN001/hardware/mechanicals/right_arm-eb1-j0_1-mec.xml
index 1dd7ab1b40..4081a40c6e 100644
--- a/ergoCubSN001/hardware/mechanicals/right_arm-eb1-j0_1-mec.xml
+++ b/ergoCubSN001/hardware/mechanicals/right_arm-eb1-j0_1-mec.xml
@@ -11,7 +11,7 @@
         <param name="Encoder">                  182.044             182.044             </param>
         <param name="fullscalePWM">             32000               32000               </param>
         <param name="ampsToSensor">             1000.0              1000.0              </param>
-        <param name="Gearbox_M2J">              -100.00             -160.00             </param>
+        <param name="Gearbox_M2J">              -100.00             -100.00             </param>
         <param name="Gearbox_E2J">              64                  64                  </param>
         <param name="useMotorSpeedFbk">         1                   1                   </param>
         <param name="MotorType">                "MOOG-BL-C2900576"  "MOOG-BL-C2900576"  </param>
diff --git a/ergoCubSN001/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml b/ergoCubSN001/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
index ffd4461d08..833e32f61d 100644
--- a/ergoCubSN001/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
+++ b/ergoCubSN001/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
@@ -27,7 +27,7 @@
         <param name="hardwareJntPosMax">    90              90              90              90         </param>
         <param name="hardwareJntPosMin">    0               0               0               0          </param>
         <param name="rotorPosMin">          -26000          -26000          -26000          -26000     </param>
-        <param name="rotorPosMax">          -6500           -100            -100            -100       </param>
+        <param name="rotorPosMax">          -100           -100            -100            -100       </param>
     </group>
 
     <group name="COUPLINGS">
diff --git a/ergoCubSN001/hardware/motorControl/left_arm-eb2-j0_1-mc.xml b/ergoCubSN001/hardware/motorControl/left_arm-eb2-j0_1-mc.xml
index 6fce7f483a..8c32c2ab10 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb2-j0_1-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb2-j0_1-mc.xml
@@ -12,7 +12,7 @@
     <!-- joint name                          shoulder_pitch        shoulder_roll        -->
     <group name="LIMITS">
         <param name="jntPosMax">                14                  130                 </param>
-        <param name="jntPosMin">                -88                 0                  </param>
+        <param name="jntPosMin">                -130                 0                  </param>
         <param name="jntVelMax">                120                 120                 </param>
         <param name="motorNominalCurrents">     5000                5000                </param>
         <param name="motorPeakCurrents">        12000               12000               </param>
diff --git a/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc.xml b/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
index 0531c9472c..824f46cd5a 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
@@ -17,7 +17,7 @@
         <param name="jntPosMax">                72                  85                  85                  85                 </param>
         <param name="jntVelMax">                1000                1000                1000                1000               </param>
         <param name="motorOverloadCurrents">    2000                2000                2000                2000               </param>
-        <param name="motorNominalCurrents">     700                 700                 700                 700                </param>
+        <param name="motorNominalCurrents">     1100                1100                1100                1100                </param>
         <param name="motorPeakCurrents">        1500                1500                1500                1500               </param>    
         <param name="motorPwmLimit">            3360                3360                3360                3360               </param>
     </group>       
@@ -48,10 +48,10 @@
         <param name="outputType">               pwm             </param>
         <param name="fbkControlUnits">          metric_units    </param>
         <param name="outputControlUnits">       machine_units   </param>
-        <param name="kp">                       -30         -30         -30         -30     </param>
+        <param name="kp">                       -246        -246        -246        -381    </param>
         <param name="kd">                       0           0           0           0       </param>
-        <param name="ki">                       -10         -10         -10         -10     </param>
-        <param name="maxOutput">                3360        3360        3360        3360    </param>
+        <param name="ki">                       -33         -33         -33         -21     </param>
+        <param name="maxOutput">                2700        2700        2700        2700    </param>
         <param name="maxInt">                   1000        1000        1000        1000    </param>
         <param name="stictionUp">               0           0           0           0       </param>
         <param name="stictionDown">             0           0           0           0       </param>
diff --git a/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc_service.xml b/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc_service.xml
index 3c7f7561f7..1e6ac3c1ee 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc_service.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc_service.xml
@@ -21,7 +21,7 @@
                 </group>
                 <group name="FIRMWARE">
                     <param name="major">            2                   </param>
-                    <param name="minor">            0                   </param>
+                    <param name="minor">            1                   </param>
                     <param name="build">            0                   </param>
                 </group>
             </group>
@@ -43,7 +43,7 @@
                         <!-- it is not mandatory. if not present, sensors have defaults: TYPE:decideg, ROT:zero, 0.0, false -->
                         <param name="type">             TYPE:decideg    TYPE:decideg     TYPE:decideg    TYPE:decideg                   </param>
                         <param name="rotation">         ROT:plus180     ROT:zero         ROT:zero        ROT:zero                       </param>
-                        <param name="offset">           -190.0          -305.0           -230.0          -55.0                          </param>
+                        <param name="offset">           -273            -305.0           -230.0          -55.0                          </param>
                         <param name="invertDirection">  true            true             false           false                          </param>
                     </group>
                 </group>
diff --git a/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc.xml b/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc.xml
index 903ee5a189..e03b3661cf 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc.xml
@@ -48,9 +48,9 @@
         <param name="outputType">               pwm                     </param>
         <param name="fbkControlUnits">          metric_units            </param>
         <param name="outputControlUnits">       machine_units           </param>
-        <param name="kp">                       -60         -150          </param>
-        <param name="kd">                       0           0           </param>
-        <param name="ki">                       -20         -30          </param>
+        <param name="kp">                       -200        -150        </param>
+        <param name="kd">                       -10         0           </param>
+        <param name="ki">                       -10         -30         </param>
         <param name="maxOutput">                3360        3360        </param>
         <param name="maxInt">                   1000        1000        </param>
         <param name="stictionUp">               0           0           </param>
diff --git a/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc_service.xml b/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc_service.xml
index c7515ed49c..cc18171b89 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc_service.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc_service.xml
@@ -21,7 +21,7 @@
                 </group>
                 <group name="FIRMWARE">
                     <param name="major">            2                   </param>
-                    <param name="minor">            0                   </param>
+                    <param name="minor">            1                   </param>
                     <param name="build">            0                   </param>
                 </group>
             </group>
diff --git a/ergoCubSN001/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml b/ergoCubSN001/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
index 30ea7c5c37..a8e163fec6 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
@@ -20,9 +20,9 @@
                     <param name="minor">            0           </param>
                 </group>
                 <group name="FIRMWARE">
-                    <param name="major">            1           </param>
-                    <param name="minor">            2           </param>
-                    <param name="build">            12           </param>
+                    <param name="major">            2           </param>
+                    <param name="minor">            0           </param>
+                    <param name="build">            4           </param>
                 </group>
             </group>
 
diff --git a/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc.xml b/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc.xml
index 07e690ea7c..b521a93ce8 100644
--- a/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc.xml
@@ -12,7 +12,7 @@
     <!-- joint number                           0                   1           2         3 -->
     <!-- joint name -->
     <group name="LIMITS">
-        <param name="jntPosMax">                 106                 108       78        5      </param>
+        <param name="jntPosMax">                 102                 108       78        5      </param>
         <param name="jntPosMin">                -42                 -15      -78        -103    </param>
         <param name="jntVelMax">                240                 240       240       240    </param>
         <param name="motorNominalCurrents">     15000               15000     5000      10000  </param>
diff --git a/ergoCubSN001/hardware/motorControl/right_arm-eb1-j0_1-mc.xml b/ergoCubSN001/hardware/motorControl/right_arm-eb1-j0_1-mc.xml
index d54bf91663..0196c189a4 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb1-j0_1-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb1-j0_1-mc.xml
@@ -13,7 +13,7 @@
     <!-- joint name                          shoulder_pitch            shoulder_roll    -->
     <group name="LIMITS">
         <param name="jntPosMax">                14                  130                </param>
-        <param name="jntPosMin">                -88                 0                  </param>
+        <param name="jntPosMin">                -130                 0                  </param>
         <param name="jntVelMax">                120                 120                 </param>
         <param name="motorNominalCurrents">     5000                5000                </param>
         <param name="motorPeakCurrents">        12000               12000               </param>
diff --git a/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc.xml b/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
index 22cdd20cf8..c6d1bdad36 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
@@ -13,11 +13,11 @@
     <!-- joint number                           0                   1                   2                   3                   -->
     <!-- joint name                             thumb               index               medium              pinky               -->    
     <group name="LIMITS">
-        <param name="jntPosMin">                0                   0                   0                   0                  </param>
-        <param name="jntPosMax">                70                  63                  85                  85                 </param>
+        <param name="jntPosMin">                4                   4                   4                   4                  </param>
+        <param name="jntPosMax">                70                  85                  85                  85                 </param>
         <param name="jntVelMax">                1000                1000                1000                1000               </param>
         <param name="motorOverloadCurrents">    2000                2000                2000                2000               </param>
-        <param name="motorNominalCurrents">     700                 700                 700                 700                </param>
+        <param name="motorNominalCurrents">     1100                1100                1100                1100                </param>
         <param name="motorPeakCurrents">        1500                1500                1500                1500               </param>    
         <param name="motorPwmLimit">            3360                3360                3360                3360               </param>
     </group>       
@@ -48,10 +48,10 @@
         <param name="outputType">               pwm             </param>
         <param name="fbkControlUnits">          metric_units    </param>
         <param name="outputControlUnits">       machine_units   </param>
-        <param name="kp">                       -60         -60         -60         -60    </param>
+        <param name="kp">                       -246       -246        -246         -381    </param>
         <param name="kd">                       0           0           0           0       </param>
-        <param name="ki">                       -20         -20         -20         -20     </param>
-        <param name="maxOutput">                3360        3360        3360        3360    </param>
+        <param name="ki">                       -33         -33         -33         -21     </param>
+        <param name="maxOutput">                2700        2700        2700        2700    </param>
         <param name="maxInt">                   1000        1000        1000        1000    </param>
         <param name="stictionUp">               0           0           0           0       </param>
         <param name="stictionDown">             0           0           0           0       </param>
diff --git a/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc.xml b/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc.xml
index 26c45224e2..dfd732903d 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc.xml
@@ -48,10 +48,10 @@
         <param name="outputType">               pwm                     </param>
         <param name="fbkControlUnits">          metric_units            </param>
         <param name="outputControlUnits">       machine_units           </param>
-        <param name="kp">                       60          150          </param>
+        <param name="kp">                       200         150         </param>
         <param name="kd">                       0           0           </param>
-        <param name="ki">                       20          30          </param>
-        <param name="maxOutput">                3360        3360        </param>
+        <param name="ki">                       10          30          </param>
+        <param name="maxOutput">                1500        3360        </param>
         <param name="maxInt">                   1000        3360        </param>
         <param name="stictionUp">               0           0           </param>
         <param name="stictionDown">             0           0           </param>
diff --git a/ergoCubSN001/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml b/ergoCubSN001/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml
index ce7bc76c45..5b225bf858 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml
@@ -20,9 +20,9 @@
                     <param name="minor">            0           </param>
                 </group>
                 <group name="FIRMWARE">
-                    <param name="major">            1           </param>
-                    <param name="minor">            2           </param>
-                    <param name="build">            12          </param>
+                    <param name="major">            2           </param>
+                    <param name="minor">            0           </param>
+                    <param name="build">            4           </param>
                 </group>
             </group>
 
diff --git a/ergoCubSN001/hardware/motorControl/right_leg-eb6-j0_3-mc.xml b/ergoCubSN001/hardware/motorControl/right_leg-eb6-j0_3-mc.xml
index 9b1398737f..605f6820ce 100644
--- a/ergoCubSN001/hardware/motorControl/right_leg-eb6-j0_3-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/right_leg-eb6-j0_3-mc.xml
@@ -12,7 +12,7 @@
     <!-- joint number                           0                   1           0         3 -->
     <!-- joint name -->
     <group name="LIMITS">
-        <param name="jntPosMax">                 106                 108       78        5      </param>
+        <param name="jntPosMax">                 102                 108       78        5      </param>
         <param name="jntPosMin">                -42                 -15      -78        -103    </param>
         <param name="jntVelMax">                240                 240       240      240    </param>
         <param name="motorNominalCurrents">     15000               15000     5000     10000  </param>
diff --git a/ergoCubSN001/hardware/motorControl/torso-eb5-j0_2-mc.xml b/ergoCubSN001/hardware/motorControl/torso-eb5-j0_2-mc.xml
index ed57f0cf7a..666ba04157 100644
--- a/ergoCubSN001/hardware/motorControl/torso-eb5-j0_2-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/torso-eb5-j0_2-mc.xml
@@ -16,8 +16,8 @@
         <param name="jntPosMax">                23                  45                  43                  </param>
         <param name="jntPosMin">                -23                 -18                 -43                 </param>
         <param name="jntVelMax">                120                 120                 120                 </param>
-        <param name="motorNominalCurrents">     3770                3770                5000                </param>
-        <param name="motorPeakCurrents">        14000               14000               10000                </param>
+        <param name="motorNominalCurrents">     3770                16000                5000               </param>
+        <param name="motorPeakCurrents">        14000               17000               10000               </param>
         <param name="motorOverloadCurrents">    18000               18000               15000               </param>
         <param name="motorPwmLimit">            16000               16000               16000               </param>
     </group>
diff --git a/ergoCubSN001/sensors/realsense_ros2.xml b/ergoCubSN001/sensors/realsense_ros2.xml
new file mode 100644
index 0000000000..ef81480cf8
--- /dev/null
+++ b/ergoCubSN001/sensors/realsense_ros2.xml
@@ -0,0 +1,48 @@
+<?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">    /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>
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude"  name="depthWrapR" type="rgbdSensor_nws_ros2">
+        <param name="period">0.033</param>
+        <param name="node_name">rgbdsensor_ros2_node</param>
+        <param name="color_topic_name">/camera_rgbd/color/image_rect_color</param>
+        <param name="depth_topic_name">/camera_rgbd/depth/image_rect</param>
+        <param name="color_frame_id">realsense_rgb_frame</param>
+        <param name="depth_frame_id">realsense_rgb_frame</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+            <elem name="the_device"> realsenseD450 </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach" />
+    </device>
+
+</devices>
\ No newline at end of file
diff --git a/ergoCubSN001/sensors/rplidar_ros2.xml b/ergoCubSN001/sensors/rplidar_ros2.xml
new file mode 100644
index 0000000000..01f44e8282
--- /dev/null
+++ b/ergoCubSN001/sensors/rplidar_ros2.xml
@@ -0,0 +1,51 @@
+<?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>
+
+
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="lidarWrapR" type="rangefinder2D_nws_ros2">
+        <param name="period"> 0.1 </param>
+        <param name="node_name">    rangefinder_ros2_node    </param>
+        <param name="topic_name">   /scan   </param>
+        <param name="frame_id">    head_laser_frame   </param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+            <elem name="the_device"> rpLidarS2 </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="5" type="detach" />
+    </device>
+
+</devices>
\ No newline at end of file
diff --git a/ergoCubSN001/sensors_ros2.xml b/ergoCubSN001/sensors_ros2.xml
new file mode 100644
index 0000000000..d14b292f22
--- /dev/null
+++ b/ergoCubSN001/sensors_ros2.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="ergoCubSN001" build="1" portprefix="/ergocub" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <devices>
+        <xi:include href="sensors/rplidar_ros2.xml" /> 
+        <xi:include href="sensors/realsense_ros2.xml" />
+    </devices>
+
+</robot>
\ No newline at end of file
diff --git a/ergoCubSN001/wrappers/battery/battery.xml b/ergoCubSN001/wrappers/battery/battery.xml
new file mode 100644
index 0000000000..131d6908c5
--- /dev/null
+++ b/ergoCubSN001/wrappers/battery/battery.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="battery_wrapper" type="battery_nws_yarp">
+    <param name="period">       1.0                 </param>
+    <param name="name">       /ergocub/battery      </param>
+
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+            <elem name="battery">  canbattery </elem>
+        </paramlist>
+    </action>
+
+    <action phase="shutdown" level="5" type="detach" />
+</device>
+
diff --git a/ergoCubSN001/wrappers/inertials/waist-inertials_wrapper.xml b/ergoCubSN001/wrappers/inertials/waist-inertials_wrapper.xml
new file mode 100644
index 0000000000..6fb6bcc482
--- /dev/null
+++ b/ergoCubSN001/wrappers/inertials/waist-inertials_wrapper.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+ <device xmlns:xi="http://www.w3.org/2001/XInclude" name="waist-inertials_wrapper" type="multipleanalogsensorsserver">
+      <param name="period">       5                  </param>
+      <param name="name">  /ergocub/waist/inertials  </param>
+
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <!-- The name of the element can be any string (we use SetOfIMUs to better express its nature).
+                     Its value must match the device name in the corresponding body_part-jx_y-inertials.xml file
+                     or in body_part-ebX-inertials.xml -->
+                <elem name="SetOfIMUs"> waist-inertial </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="5" type="detach" />
+    </device> 
diff --git a/ergoCubSN001/wrappers/motorControl/alljoints-mc_remapper.xml b/ergoCubSN001/wrappers/motorControl/alljoints-mc_remapper.xml
index e9b6054d2e..364f1cad0c 100644
--- a/ergoCubSN001/wrappers/motorControl/alljoints-mc_remapper.xml
+++ b/ergoCubSN001/wrappers/motorControl/alljoints-mc_remapper.xml
@@ -2,13 +2,14 @@
 <!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
 
 <device xmlns:xi="http://www.w3.org/2001/XInclude" name="alljoints-mc_remapper" type="controlboardremapper">
-    <!--
-    These are the regular parameters 
+
+    <!-- These are the regular parameters -->
     <param name="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_oc,l_index_add,l_index_oc,l_middle_oc,l_ring_pinky_oc,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_oc,r_index_add,r_index_oc,r_middle_oc,r_ring_pinky_oc,torso_roll,torso_pitch,torso_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
-    -->
-    <!-- These are the parameters with torso_pitch removed -->
-    <param name="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_oc,l_index_add,l_index_oc,l_middle_oc,l_ring_pinky_oc,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_oc,r_index_add,r_index_oc,r_middle_oc,r_ring_pinky_oc,torso_roll,torso_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
-    <param name="joints"> 44 </param>
+
+    <!-- <!-\- These are the parameters with torso_pitch removed -\-> -->
+    <!-- <param name="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_oc,l_index_add,l_index_oc,l_middle_oc,l_ring_pinky_oc,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_oc,r_index_add,r_index_oc,r_middle_oc,r_ring_pinky_oc,torso_roll,torso_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param> -->
+
+    <param name="joints"> 45 </param>
     <action phase="startup" level="6" type="attach">
         <paramlist name="networks">
             <elem name="head_joints">       head-mc_remapper </elem>
@@ -20,4 +21,4 @@
         </paramlist>
     </action>
     <action phase="shutdown" level="19" type="detach" />
-</device>
\ No newline at end of file
+</device>