diff --git a/ergoCubSN001/calibrators/left_arm-calib.xml b/ergoCubSN001/calibrators/left_arm-calib.xml
index 9aa905a2ce..d718065174 100644
--- a/ergoCubSN001/calibrators/left_arm-calib.xml
+++ b/ergoCubSN001/calibrators/left_arm-calib.xml
@@ -6,32 +6,32 @@
 	<xi:include href="../general.xml" />
 
 	<group name="GENERAL">
-		<param name="joints">4</param>		<!-- the number of joints of the robot part -->
+		<param name="joints">13</param>		<!-- the number of joints of the robot part -->
 		<param name="deviceName"> Left_Arm_Calibrator </param>
 	</group>
 
 	<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             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>
+		<param name="positionHome">            5                 30              0           10        0            0             0             0.00        5.00        5.00         5.00        5.00           5.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            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            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          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>
+		<param name="calibrationType">        10                 10              10          10        12           12            12            12           14          12           14          14             14      </param>
+		<param name="calibration1">           4000               -3000           -3000       4000      24917        14878         17235         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            0           0            0           0              1       </param>
+		<param name="calibration4">           0                  0               0           0         0            0             0             0            0           0            32768       0              0       </param>
+		<param name="calibration5">           0                  0               0           0         0            0             0             0            6850        0            48450       37450          56800   </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          5.0         5.0          5.0         5.0            5.0     </param>
+		<param name="startupVelocity">        10.0               10.0            10.0        10.0      10.0         10.0          10.0          100.0        100.0       100.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             5            5           5            5           5              5       </param>
 	</group>
 
 <!--	<param name="CALIB_ORDER"> (4 5 6) (3) (2) (0) (1) (7 8) (9) (10 11 12) </param> --> <!-- Don't remove this line -->
-	<param name="CALIB_ORDER"> (3) (2) (0) (1) </param> 
+	<param name="CALIB_ORDER">  (4 5 6) (3) (2) (0) (1) (7 8) (9) (10 11 12) </param> 
 
 	<action phase="startup" level="10" type="calibrate">
 		<param name="target">left_arm-mc_remapper</param>
diff --git a/ergoCubSN001/calibrators/right_arm-calib.xml b/ergoCubSN001/calibrators/right_arm-calib.xml
index 36808c3086..c35c661369 100644
--- a/ergoCubSN001/calibrators/right_arm-calib.xml
+++ b/ergoCubSN001/calibrators/right_arm-calib.xml
@@ -6,33 +6,33 @@
 	<xi:include href="../general.xml" />
 
 	<group name="GENERAL">
-		<param name="joints">4</param>		<!-- the number of joints of the robot part -->
+		<param name="joints">13</param>		<!-- the number of joints of the robot part -->
 		<param name="deviceName"> Right_Arm_Calibrator </param>
 	</group>
 
 	<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            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>
+		<param name="positionHome">            5                30              0           10        0             0              0             0.00        5.00        5.00         5.00        5.00        5.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   <!--    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            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          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>
+		<param name="calibrationType">         10                10             10          10        12            12             12           12           14          12           14          14          14         </param>
+		<param name="calibration1">           -4000              3000           3000       -4000      1795          16373          11969        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            0           0            1           0           0          </param>
+		<param name="calibration4">            0                 0              0           0         0             0              0            0            0           0            32768       0           0          </param>
+		<param name="calibration5">            0                 0              0           0         0             0              0            0            25030       0            1230        32470       18350      </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          5.0         5.0          5.0         5.0         5.0        </param>
+		<param name="startupVelocity">         10.0              10.0           10.0        10.0      10.0          10.0           10.0         100.0        100.0       100.0        100.0       100.0       100.0      </param>
+		<param name="startupMaxPwm">           8000              8000           8000        12000     16000         16000          16000        0            0           0            0           0           0          </param>
+		<param name="startupPosThreshold">     2                 2              2           2         2             2              2            5            5           5            5           5           5          </param>
 	</group>
 
 <!--	<param name="CALIB_ORDER"> (4 5 6) (3) (2) (0) (1) (7 8) (9) (10 11 12) </param> --> <!-- Don't remove this line -->
-	<param name="CALIB_ORDER"> (3) (2) (0) (1) </param>
+	<param name="CALIB_ORDER">(4 5 6) (3) (2) (0) (1) (7 8) (9) (10 11 12) </param> 
 
 	
 	<action phase="startup" level="10" type="calibrate">
diff --git a/ergoCubSN001/ergocub_all.xml b/ergoCubSN001/ergocub_all.xml
index 3a5a18c893..331251cdac 100644
--- a/ergoCubSN001/ergocub_all.xml
+++ b/ergoCubSN001/ergocub_all.xml
@@ -9,17 +9,17 @@
 
     <devices>
 
-    <!-- POS4 
+    <!-- POS4 -->
     <xi:include href="wrappers/POS/right_hand-pos_wrapper4.xml" />
-    <xi:include href="hardware/POS/right_hand-pos4.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" /> -->
+    <xi:include href="hardware/POS/left_hand-pos4.xml" /> 
 
-    <!-- POS2 
+    <!-- POS2 -->
     <xi:include href="wrappers/POS/right_hand-pos_wrapper2.xml" />
-    <xi:include href="hardware/POS/right_hand-pos2.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" /> -->
+    <xi:include href="hardware/POS/left_hand-pos2.xml" /> 
 
     <!-- motor controllers wrappers -->
     <xi:include href="wrappers/motorControl/left_leg-mc_wrapper.xml" />
@@ -51,16 +51,16 @@
     <!-- 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-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" /> -->
+    <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-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" /> -->
+    <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" />
@@ -70,9 +70,9 @@
     <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 -->
+    <!-- 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/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" />
@@ -82,9 +82,9 @@
     <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 -->
+    <!-- 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/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" />
 
@@ -103,29 +103,29 @@
     <xi:include href="hardware/inertials/right_arm-eb3-j2_3-inertial.xml" />
     <xi:include href="wrappers/inertials/right_arm-inertials_wrapper.xml" /> -->
 
-    <!-- FT SENSORS - IMU -->
+    <!-- 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/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 -->
+    <!-- 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/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 BAT -->
+    <!-- BATTERY BAT 
     <xi:include href="wrappers/battery/battery_bat.xml" /> 
-    <xi:include href="hardware/battery/battery_bat.xml" /> 
+    <xi:include href="hardware/battery/battery_bat.xml" />  -->
     
-    <!-- BATTERY BMS -->
+    <!-- BATTERY BMS 
     <xi:include href="wrappers/battery/battery_bms.xml" /> 
-    <xi:include href="hardware/battery/battery_bms.xml" /> 
+    <xi:include href="hardware/battery/battery_bms.xml" /> -->
 
     <!--  CALIBRATORS -->
     <xi:include href="calibrators/torso-calib.xml" />
diff --git a/ergoCubSN001/ergocub_wbd.xml b/ergoCubSN001/ergocub_wbd.xml
index 21c6739229..ca89763e15 100644
--- a/ergoCubSN001/ergocub_wbd.xml
+++ b/ergoCubSN001/ergocub_wbd.xml
@@ -4,22 +4,22 @@
 <robot name="ergoCubSN001" xmlns:xi="http://www.w3.org/2001/XInclude">
 
     <params>
-    <xi:include href="hardware/electronics/pc104.xml" />
+        <xi:include href="hardware/electronics/pc104.xml" />
     </params>
 
     <devices>
 
-    <!-- POS4
+    <!-- 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" /> -->
+    <xi:include href="hardware/POS/left_hand-pos4.xml" />
 
-    <!-- POS2
+    <!-- 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" /> -->
+    <xi:include href="hardware/POS/left_hand-pos2.xml" />
 
     <!-- motor controllers wrappers -->
     <xi:include href="wrappers/motorControl/left_leg-mc_wrapper.xml" />
@@ -51,16 +51,16 @@
     <!-- 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-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" /> -->
+    <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-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" /> -->
+    <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" />
@@ -71,8 +71,8 @@
     <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_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" />
@@ -83,8 +83,8 @@
     <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_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" />
 
@@ -104,16 +104,16 @@
     <xi:include href="wrappers/inertials/right_arm-inertials_wrapper.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_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_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" />
diff --git a/ergoCubSN001/estimators/wholebodydynamics.xml b/ergoCubSN001/estimators/wholebodydynamics.xml
index 9d213c63fb..fbb8ec2936 100644
--- a/ergoCubSN001/estimators/wholebodydynamics.xml
+++ b/ergoCubSN001/estimators/wholebodydynamics.xml
@@ -27,7 +27,7 @@
   </group>
 
   <group name="multipleAnalogSensorsNames">
-    <param name="SixAxisForceTorqueSensorsNames">(l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)</param>
+    <param name="SixAxisForceTorqueSensorsNames">(l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft)</param>
   </group>
 
 
@@ -57,10 +57,10 @@
       <elem name="torso">torso-eb5-j0_2-mc</elem>
       <elem name="left_upper_arm-j0">left_arm-eb2-j0_1-mc</elem>
       <elem name="left_upper_arm-j2">left_arm-eb4-j2_3-mc</elem>
-      <!-- <elem name="left_lower_arm">left_arm-eb31-j4_6-mc</elem> -->
+      <elem name="left_lower_arm">left_arm-eb31-j4_6-mc</elem>
       <elem name="right_upper_arm-j0">right_arm-eb1-j0_1-mc</elem>
       <elem name="right_upper_arm-j2">right_arm-eb3-j2_3-mc</elem>
-      <!-- <elem name="right_lower_arm">right_arm-eb30-j4_6-mc</elem> -->
+      <elem name="right_lower_arm">right_arm-eb30-j4_6-mc</elem>
       <elem name="left_upper_leg-j0">left_leg-eb8-j0_3-mc</elem>
       <elem name="left_lower_leg-j2">left_leg-eb9-j4_5-mc</elem>
       <elem name="right_upper_leg-j0">right_leg-eb6-j0_3-mc</elem>
@@ -73,8 +73,6 @@
       <!-- ft -->
       <elem name="l_leg_ft_sensors">left_leg-FT_remapper</elem>
       <elem name="r_leg_ft_sensors">right_leg-FT_remapper</elem>
-      <elem name="l_arm_ft_sensors">left_arm-eb2-j0_1-strain</elem>
-      <elem name="r_arm_ft_sensors">right_arm-eb1-j0_1-strain</elem>
     </paramlist>
   </action>
 
diff --git a/ergoCubSN001/extra/scripts/additionalCommands.sh b/ergoCubSN001/extra/scripts/additionalCommands.sh
index 1ed97e3940..55cf419332 100644
--- a/ergoCubSN001/extra/scripts/additionalCommands.sh
+++ b/ergoCubSN001/extra/scripts/additionalCommands.sh
@@ -22,7 +22,7 @@ export ROBOTS_CONFIGURATION_DIR=${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/src/robots-c
 alias gotoRobotsConfigurationFolder='cd $ROBOTS_CONFIGURATION_DIR'
 
 #Automatic Joypad configuration
-alias connectToJoypad='sudo expect ${ADDITIONAL_COMMANDS_DIR}/bluetoothConnect.sh 28:9A:4B:08:E1:1B'
+alias connectToJoypad='sudo expect ${ADDITIONAL_COMMANDS_DIR}/bluetoothConnect.sh 28:9A:4B:0A:5F:6D'
 
 # Go to DCM folder
 alias dcmFolder='cd ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/src/walking-controllers/src/WalkingModule/app/robots/${YARP_ROBOT_NAME}'
diff --git a/ergoCubSN001/hardware/POS/left_hand-pos2.xml b/ergoCubSN001/hardware/POS/left_hand-pos2.xml
index e3d1bfe274..7453a8479e 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">            1                       </param>
+                    <param name="minor">            2                       </param>
                     <param name="build">            0                       </param>
                 </group>
             </group>
@@ -40,8 +40,8 @@
                 <group name="CALIBRATION">
                     <!-- it is not mandatory. if not present, sensors have defaults: TYPE:decideg, ROT:zero, 0.0, false -->
                     <param name="type">             TYPE:decideg     TYPE:decideg          </param>
-                    <param name="rotation">         ROT:zero         ROT:zero              </param>
-                    <param name="offset">           -230.0           -325.0                </param>
+                    <param name="rotation">         ROT:plus180      ROT:zero              </param>
+                    <param name="offset">           -209.0           -161.0                </param>
                     <param name="invertDirection">  true             true                  </param>
                 </group>
 
diff --git a/ergoCubSN001/hardware/POS/left_hand-pos4.xml b/ergoCubSN001/hardware/POS/left_hand-pos4.xml
index 1d3b5b1044..4da127f0ce 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">            1                       </param>
+                    <param name="minor">            2                       </param>
                     <param name="build">            0                       </param>
                 </group>
             </group>
@@ -40,9 +40,9 @@
                 <group name="CALIBRATION">
                     <!-- 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">           -273            -261             -259             -296              </param>
-                    <param name="invertDirection">  true            false            false            true              </param>
+                    <param name="rotation">         ROT:zero        ROT:zero         ROT:zero         ROT:zero          </param>
+                    <param name="offset">           0               0                0                0                 </param>
+                    <param name="invertDirection">  false           false            false            false             </param>
                 </group>
             </group>
 
diff --git a/ergoCubSN001/hardware/POS/right_hand-pos2.xml b/ergoCubSN001/hardware/POS/right_hand-pos2.xml
index e7ae9a3934..346b7003f5 100644
--- a/ergoCubSN001/hardware/POS/right_hand-pos2.xml
+++ b/ergoCubSN001/hardware/POS/right_hand-pos2.xml
@@ -20,7 +20,7 @@
                 </group>
                 <group name="FIRMWARE">
                     <param name="major">            2                       </param>
-                    <param name="minor">            0                       </param>
+                    <param name="minor">            2                       </param>
                     <param name="build">            0                       </param>
                 </group>
             </group>
@@ -40,9 +40,9 @@
                 <group name="CALIBRATION">
                     <!-- it is not mandatory. if not present, sensors have defaults: TYPE:decideg, ROT:zero, 0.0, false -->
                     <param name="type">             TYPE:decideg     TYPE:decideg          </param>
-                    <param name="rotation">         ROT:zero         ROT:zero              </param>
-                    <param name="offset">           -200.0           -270.0                </param>
-                    <param name="invertDirection">  false            false                  </param>
+                    <param name="rotation">         ROT:plus180      ROT:zero              </param>
+                    <param name="offset">           -91.0            -291.0                </param>
+                    <param name="invertDirection">  false            false                 </param>
                 </group>
 
             </group>
diff --git a/ergoCubSN001/hardware/POS/right_hand-pos4.xml b/ergoCubSN001/hardware/POS/right_hand-pos4.xml
index 09b907e580..aab05a0fef 100644
--- a/ergoCubSN001/hardware/POS/right_hand-pos4.xml
+++ b/ergoCubSN001/hardware/POS/right_hand-pos4.xml
@@ -20,7 +20,7 @@
                 </group>
                 <group name="FIRMWARE">
                     <param name="major">            2                       </param>
-                    <param name="minor">            0                       </param>
+                    <param name="minor">            2                       </param>
                     <param name="build">            0                       </param>
                 </group>
             </group>
@@ -39,10 +39,10 @@
 
                 <group name="CALIBRATION">
                     <!-- 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">           -184.0          -294             -247             -65             </param>
-                    <param name="invertDirection">  true            true             false            false             </param>
+                    <param name="type">             TYPE:decideg    TYPE:decideg     TYPE:decideg     TYPE:decideg     </param>
+                    <param name="rotation">         ROT:zero        ROT:zero         ROT:zero         ROT:zero         </param>
+                    <param name="offset">           0.0             0.0              0.0              0.0                </param>
+                    <param name="invertDirection">  false           false            false            false            </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 7ce7cbefcc..dbbe388518 100644
--- a/ergoCubSN001/hardware/inertials/left_arm-eb4-j2_3-inertial.xml
+++ b/ergoCubSN001/hardware/inertials/left_arm-eb4-j2_3-inertial.xml
@@ -15,15 +15,15 @@
             <group name="PROPERTIES">
             
                 <group name="CANBOARDS">
-                    <param name="type">                 mtb4                </param>
+                    <param name="type">                 mtb4c                </param>
 
                     <group name="PROTOCOL">
                         <param name="major">            2                   </param>    
                         <param name="minor">            0                   </param>     
                     </group>                    
                     <group name="FIRMWARE">
-                        <param name="major">            1                   </param>    
-                        <param name="minor">            21                   </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 d113bffae0..d47389bf12 100644
--- a/ergoCubSN001/hardware/inertials/right_arm-eb3-j2_3-inertial.xml
+++ b/ergoCubSN001/hardware/inertials/right_arm-eb3-j2_3-inertial.xml
@@ -16,15 +16,15 @@
             <group name="PROPERTIES">
             
                 <group name="CANBOARDS">
-                    <param name="type">                 mtb4                </param>
+                    <param name="type">                 mtb4c                </param>
 
                     <group name="PROTOCOL">
                         <param name="major">            2                   </param>    
                         <param name="minor">            0                   </param>     
                     </group>                    
                     <group name="FIRMWARE">
-                        <param name="major">            1                   </param>    
-                        <param name="minor">            21                   </param> 
+                        <param name="major">            2                   </param>    
+                        <param name="minor">            2                   </param> 
                         <param name="build">            0                   </param>
                     </group>
                 </group>
diff --git a/ergoCubSN001/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml b/ergoCubSN001/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
index dd4aedd0d8..0caed0015c 100644
--- a/ergoCubSN001/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
+++ b/ergoCubSN001/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
@@ -25,8 +25,8 @@
     <group name="LIMITS">
         <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">          -100            -100            -100            -100        </param>
+        <param name="rotorPosMin">          -26500          -26500          -26500          -26500     </param>
+        <param name="rotorPosMax">          -100            -100            -100            -100       </param>
     </group>
 
     <group name="COUPLINGS">
diff --git a/ergoCubSN001/hardware/mechanicals/left_arm-eb31-j4_6-mec.xml b/ergoCubSN001/hardware/mechanicals/left_arm-eb31-j4_6-mec.xml
index 27f5733ac3..79bfd9c0f4 100644
--- a/ergoCubSN001/hardware/mechanicals/left_arm-eb31-j4_6-mec.xml
+++ b/ergoCubSN001/hardware/mechanicals/left_arm-eb31-j4_6-mec.xml
@@ -19,8 +19,8 @@
     </group>
 
     <group name="LIMITS">
-        <param name="hardwareJntPosMax">     92            52            32          </param>
-        <param name="hardwareJntPosMin">    -92           -62           -32          </param>
+        <param name="hardwareJntPosMax">     90            30            20          </param>
+        <param name="hardwareJntPosMin">    -90           -20           -20          </param>
         <param name="rotorPosMin">           0             0             0           </param>
         <param name="rotorPosMax">           0             0             0           </param>
     </group>
@@ -64,8 +64,8 @@
         <param name= "numberofsets"> 1 </param>
         <group name="JOINTSET_0">
             <param name="listofjoints">  0 1 2         </param>
-            <param name="constraint">    none          </param>
-            <param name="param1">        0             </param>
+            <param name="constraint">    ergocubwrist  </param> <!-- none for calibration -->
+            <param name="param1">        20            </param>
             <param name="param2">        0             </param>
         </group>
     </group>
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 833e32f61d..c0c77c0cba 100644
--- a/ergoCubSN001/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
+++ b/ergoCubSN001/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
@@ -26,8 +26,8 @@
         <!-- After run 1st round of calibration check best rotorPosMin and rotorPosMin on motor position in the yarpmotorgui and set them (just do that one time)-->
         <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">          -100           -100            -100            -100       </param>
+        <param name="rotorPosMin">          -26500         -26500          -26500          -26500      </param>
+        <param name="rotorPosMax">          -100           -100            -100            -100        </param>
     </group>
 
     <group name="COUPLINGS">
diff --git a/ergoCubSN001/hardware/mechanicals/right_arm-eb24-j11_12-mec.xml b/ergoCubSN001/hardware/mechanicals/right_arm-eb24-j11_12-mec.xml
index 379acdf79b..4f92517da4 100644
--- a/ergoCubSN001/hardware/mechanicals/right_arm-eb24-j11_12-mec.xml
+++ b/ergoCubSN001/hardware/mechanicals/right_arm-eb24-j11_12-mec.xml
@@ -23,8 +23,8 @@
     </group>
 
     <group name="LIMITS">
-        <param name="hardwareJntPosMax">    90              20      </param>
-        <param name="hardwareJntPosMin">    0               6       </param>
+        <param name="hardwareJntPosMax">    90              15      </param>
+        <param name="hardwareJntPosMin">    0               0       </param>
         <param name="rotorPosMin">          0               0       </param>
         <param name="rotorPosMax">          0               0       </param>
     </group>
diff --git a/ergoCubSN001/hardware/mechanicals/right_arm-eb30-j4_6-mec.xml b/ergoCubSN001/hardware/mechanicals/right_arm-eb30-j4_6-mec.xml
index 5a8a391cd7..715b8f70a4 100644
--- a/ergoCubSN001/hardware/mechanicals/right_arm-eb30-j4_6-mec.xml
+++ b/ergoCubSN001/hardware/mechanicals/right_arm-eb30-j4_6-mec.xml
@@ -19,8 +19,8 @@
     </group>
 
     <group name="LIMITS">
-        <param name="hardwareJntPosMax">     92            52            32          </param>
-        <param name="hardwareJntPosMin">    -92           -62           -32          </param>
+        <param name="hardwareJntPosMax">     90            30            20          </param>
+        <param name="hardwareJntPosMin">    -90           -20           -20          </param>
         <param name="rotorPosMin">           0             0             0           </param>
         <param name="rotorPosMax">           0             0             0           </param>
     </group>
@@ -64,9 +64,9 @@
         <param name= "numberofsets"> 1 </param>
         <group name="JOINTSET_0">
             <param name="listofjoints">  0 1 2         </param>
-            <param name="constraint">    none          </param>
-            <param name="param1">        0             </param>
-            <param name="param2">        0             </param>
+            <param name="constraint">    ergocubwrist  </param> <!-- none for calibration -->
+            <param name="param1">        20             </param>
+            <param name="param2">        1             </param>
         </group>
     </group>
 
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 824f46cd5a..f070f93768 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
@@ -13,8 +13,8 @@
     <!-- joint number                           0                   1                   2                   3                   -->
     <!-- joint name                             thumb               index               medium              pinky               -->    
     <group name="LIMITS">
-        <param name="jntPosMin">                4                   4                   4                   4                  </param>
-        <param name="jntPosMax">                72                  85                  85                  85                 </param>
+        <param name="jntPosMin">                0                   0                   0                   0                  </param>
+        <param name="jntPosMax">                90                  90                  90                  90                 </param>
         <param name="jntVelMax">                1000                1000                1000                1000               </param>
         <param name="motorOverloadCurrents">    2000                2000                2000                2000               </param>
         <param name="motorNominalCurrents">     1100                1100                1100                1100                </param>
@@ -48,9 +48,9 @@
         <param name="outputType">               pwm             </param>
         <param name="fbkControlUnits">          metric_units    </param>
         <param name="outputControlUnits">       machine_units   </param>
-        <param name="kp">                       -246        -246        -246        -381    </param>
+        <param name="kp">                       -246        -246        -246        -246    </param>
         <param name="kd">                       0           0           0           0       </param>
-        <param name="ki">                       -33         -33         -33         -21     </param>
+        <param name="ki">                       -33         -33         -33         -33     </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>
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 1e6ac3c1ee..c85ccb6157 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">            1                   </param>
+                    <param name="minor">            2                   </param>
                     <param name="build">            0                   </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 e03b3661cf..d4de344d3c 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb25-j11_12-mc.xml
@@ -13,8 +13,8 @@
     <!-- joint number                           0                        1                           -->
     <!-- joint name                             hand_thumb_add     hand_index_add         -->
     <group name="LIMITS">
-        <param name="jntPosMin">                0                        8            </param>
-        <param name="jntPosMax">                85                       21           </param>
+        <param name="jntPosMin">                0                        0            </param>
+        <param name="jntPosMax">                90                       15           </param>
         <param name="jntVelMax">                1000                     1000         </param>
         <param name="motorOverloadCurrents">    2000                     2000         </param>
         <param name="motorNominalCurrents">     700                      700          </param>
@@ -48,9 +48,9 @@
         <param name="outputType">               pwm                     </param>
         <param name="fbkControlUnits">          metric_units            </param>
         <param name="outputControlUnits">       machine_units           </param>
-        <param name="kp">                       -200        -150        </param>
-        <param name="kd">                       -10         0           </param>
-        <param name="ki">                       -10         -30         </param>
+        <param name="kp">                       150         200         </param>
+        <param name="kd">                       0           0           </param>
+        <param name="ki">                       10          10          </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 cc18171b89..3dc0a6e1f2 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">            1                   </param>
+                    <param name="minor">            2                   </param>
                     <param name="build">            0                   </param>
                 </group>
             </group>
diff --git a/ergoCubSN001/hardware/motorControl/left_arm-eb31-j4_6-mc.xml b/ergoCubSN001/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
index 5f8912e975..f01d67fe38 100644
--- a/ergoCubSN001/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
@@ -12,8 +12,8 @@
     <!-- joint number                           0                   1                   2               -->
     <!-- joint name -->
     <group name="LIMITS">
-        <param name="jntPosMax">                90                  50                  30                  </param>
-        <param name="jntPosMin">               -90                 -60                 -30                  </param>
+        <param name="jntPosMax">                90                  30                  20                  </param>
+        <param name="jntPosMin">               -90                 -20                 -20                  </param>
         <param name="jntVelMax">                90                  90                  90                  </param>
         <param name="motorNominalCurrents">     200                 200                 200                </param>
         <param name="motorPeakCurrents">        200                 200                 200                </param>
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 a8e163fec6..fbc6933f6d 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
@@ -34,10 +34,10 @@
                 </group>
 
                 <group name="ENCODER1">
-                    <param name="type">             aea                 aea                 aea         </param>
+                    <param name="type">             aea3                aea3                aea3        </param>
                     <param name="port">             CONN:J5_X1          CONN:J5_X2          CONN:J5_X3  </param>
                     <param name="position">         atjoint             atjoint             atjoint     </param>
-                    <param name="resolution">       4096                4096                4096        </param>
+                    <param name="resolution">       16384               16384               16384       </param>
                     <param name="tolerance">        0.4                 0.4                 0.4         </param>
                 </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 cd11859cac..9c7d1f910c 100644
--- a/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc.xml
@@ -53,7 +53,7 @@
         <param name="kp">                       -5000               7000        -3000   -10000      </param>
         <param name="kd">                       0                   0           0        0       </param>
         <param name="ki">                       -1500               5000        -200    -1500       </param>
-        <param name="maxOutput">               12000                10000       12000    16000      </param>
+        <param name="maxOutput">               12000               12000       12000    16000      </param>
         <param name="maxInt">                   1500                5000        750      2000      </param>
         <param name="stictionUp">               0                   0           0        0       </param>
         <param name="stictionDown">             0                   0           0        0       </param>
diff --git a/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc_service.xml b/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc_service.xml
index d70d5e3204..e94d8afbb5 100644
--- a/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc_service.xml
+++ b/ergoCubSN001/hardware/motorControl/left_leg-eb8-j0_3-mc_service.xml
@@ -36,7 +36,7 @@
                 </group>
                 
                 <group name="ENCODER1">  
-                    <param name="type">             amo                 amo         amo        amo       </param>  
+                    <param name="type">             none                amo         amo        amo       </param>  
                     <param name="port">             CONN:P6             CONN:P8     CONN:P7    CONN:P9   </param>
                     <param name="position">         atjoint             atjoint     atjoint    atjoint   </param>
                     <param name="resolution">       16384               16384       16384      -16384    </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 c6d1bdad36..8edfd39972 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
@@ -13,8 +13,8 @@
     <!-- joint number                           0                   1                   2                   3                   -->
     <!-- joint name                             thumb               index               medium              pinky               -->    
     <group name="LIMITS">
-        <param name="jntPosMin">                4                   4                   4                   4                  </param>
-        <param name="jntPosMax">                70                  85                  85                  85                 </param>
+        <param name="jntPosMin">                0                   0                   0                   0                  </param>
+        <param name="jntPosMax">                90                  90                  90                  90                 </param>
         <param name="jntVelMax">                1000                1000                1000                1000               </param>
         <param name="motorOverloadCurrents">    2000                2000                2000                2000               </param>
         <param name="motorNominalCurrents">     1100                1100                1100                1100                </param>
@@ -48,9 +48,9 @@
         <param name="outputType">               pwm             </param>
         <param name="fbkControlUnits">          metric_units    </param>
         <param name="outputControlUnits">       machine_units   </param>
-        <param name="kp">                       -246       -246        -246         -381    </param>
+        <param name="kp">                       -246       -246        -246         -246    </param>
         <param name="kd">                       0           0           0           0       </param>
-        <param name="ki">                       -33         -33         -33         -21     </param>
+        <param name="ki">                       -33         -33         -33         -33     </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>
diff --git a/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc_service.xml b/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc_service.xml
index d8776e60f4..924f2d9139 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb22-j7_10-mc_service.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb22-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">            2                   </param>
                     <param name="build">            0                   </param>
                 </group>
             </group>
@@ -64,7 +64,7 @@
 
                 <group name="ENCODER1">
                     <param name="type">             pos                 pos                       pos                       pos                       </param>
-                    <param name="port">             POS:hand_thumb_oc      POS:hand_index_oc            POS:hand_middle_oc            POS:hand_ring_pinky_oc            </param>
+                    <param name="port">             POS:hand_thumb_oc   POS:hand_index_oc         POS:hand_middle_oc        POS:hand_ring_pinky_oc    </param>
                     <param name="position">         atjoint             atjoint                   atjoint                   atjoint                   </param>
                     <param name="resolution">       65535               65535                     65535                     65535                     </param>
                     <param name="tolerance">        5                   5                         5                         5                         </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 dfd732903d..d4018e62d5 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc.xml
@@ -13,8 +13,8 @@
     <!-- joint number                           0                        1                           -->
     <!-- joint name                             hand_thumb_add     hand_index_add         -->
     <group name="LIMITS">
-        <param name="jntPosMin">                0                        7            </param>
-        <param name="jntPosMax">                85                       18           </param>
+        <param name="jntPosMin">                0                        0            </param>
+        <param name="jntPosMax">                90                       15           </param>
         <param name="jntVelMax">                1000                     1000         </param>
         <param name="motorOverloadCurrents">    2000                     2000         </param>
         <param name="motorNominalCurrents">     700                      700          </param>
@@ -48,11 +48,11 @@
         <param name="outputType">               pwm                     </param>
         <param name="fbkControlUnits">          metric_units            </param>
         <param name="outputControlUnits">       machine_units           </param>
-        <param name="kp">                       200         150         </param>
+        <param name="kp">                       -150        -200         </param>
         <param name="kd">                       0           0           </param>
-        <param name="ki">                       10          30          </param>
-        <param name="maxOutput">                1500        3360        </param>
-        <param name="maxInt">                   1000        3360        </param>
+        <param name="ki">                       -10         -10          </param>
+        <param name="maxOutput">                3360        3360        </param>
+        <param name="maxInt">                   1000        1000        </param>
         <param name="stictionUp">               0           0           </param>
         <param name="stictionDown">             0           0           </param>
         <param name="kff">                      0           0           </param>
diff --git a/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc_service.xml b/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc_service.xml
index 07d760941f..fd408aa48c 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb24-j11_12-mc_service.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb24-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">            2                   </param>
                     <param name="build">            0                   </param>
                 </group>
             </group>
diff --git a/ergoCubSN001/hardware/motorControl/right_arm-eb3-j2_3-mc_service.xml b/ergoCubSN001/hardware/motorControl/right_arm-eb3-j2_3-mc_service.xml
index 1d57172238..42b378b9f3 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb3-j2_3-mc_service.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb3-j2_3-mc_service.xml
@@ -36,7 +36,7 @@
                 </group>
                 
                 <group name="ENCODER1">  
-                    <param name="type">             amo                 amo                 </param>  
+                    <param name="type">             none                none                </param>  
                     <param name="port">             CONN:P6             CONN:P8             </param>
                     <param name="position">         atjoint             atjoint             </param> 
                     <param name="resolution">      -16384              -16384               </param>
diff --git a/ergoCubSN001/hardware/motorControl/right_arm-eb30-j4_6-mc.xml b/ergoCubSN001/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
index 6f234f2a6b..1468d26b05 100644
--- a/ergoCubSN001/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
@@ -12,8 +12,8 @@
     <!-- joint number                           0                   1                   2               -->
     <!-- joint name -->
     <group name="LIMITS">
-        <param name="jntPosMax">                90                  50                  30                  </param>
-        <param name="jntPosMin">               -90                 -60                 -30                  </param>
+        <param name="jntPosMax">                90                  30                  20                  </param>
+        <param name="jntPosMin">               -90                 -20                 -20                  </param>
         <param name="jntVelMax">                90                  90                  90                  </param>
         <param name="motorNominalCurrents">     200                 200                 200                </param>
         <param name="motorPeakCurrents">        200                 200                 200                </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 5b225bf858..bf94b3c6b9 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
@@ -34,10 +34,10 @@
                 </group>
 
                 <group name="ENCODER1">
-                    <param name="type">             aea                 aea                 aea         </param>
+                    <param name="type">             aea3                aea3                aea3        </param>
                     <param name="port">             CONN:J5_X1          CONN:J5_X2          CONN:J5_X3  </param>
                     <param name="position">         atjoint             atjoint             atjoint     </param>
-                    <param name="resolution">       4096                4096                4096        </param>
+                    <param name="resolution">       16384               16384               16384       </param>
                     <param name="tolerance">        0.4                 0.4                 0.4         </param>
                 </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 8a6d95e063..1f00de6edc 100644
--- a/ergoCubSN001/hardware/motorControl/right_leg-eb6-j0_3-mc.xml
+++ b/ergoCubSN001/hardware/motorControl/right_leg-eb6-j0_3-mc.xml
@@ -53,7 +53,7 @@
         <param name="kp">                       5000                -25000    3000     10000    </param>
         <param name="kd">                       0                   0         0        0      </param>
         <param name="ki">                       1500                -5000     200      1500    </param>
-        <param name="maxOutput">               12000               10000     12000    16000    </param>
+        <param name="maxOutput">               12000               12000     12000    16000    </param>
         <param name="maxInt">                   1500                5000      2000     2000   </param>
         <param name="stictionUp">               0                   0         0        0      </param>
         <param name="stictionDown">             0                   0         0        0     </param>
diff --git a/ergoCubSN001/la.xml b/ergoCubSN001/la.xml
index 83a76b50bb..548f707f26 100644
--- a/ergoCubSN001/la.xml
+++ b/ergoCubSN001/la.xml
@@ -9,13 +9,17 @@
 
     <devices>
 
-    <!-- POS4 
-    <xi:include href="wrappers/POS/left_hand-pos_wrapper4.xml" /> 
-    <xi:include href="hardware/POS/left_hand-pos4.xml" /> -->
+<!-- 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/left_hand-pos_wrapper2.xml" /> 
-    <xi:include href="hardware/POS/left_hand-pos2.xml" /> -->
+    <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_arm-mc_wrapper.xml" />
@@ -25,9 +29,9 @@
     <!-- 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-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" /> -->
+    <xi:include href="hardware/motorControl/left_arm-eb25-j11_12-mc.xml" /> 
 
 
     <!--  CALIBRATORS -->
diff --git a/ergoCubSN001/ra.xml b/ergoCubSN001/ra.xml
index c4ab909940..89ca408bba 100644
--- a/ergoCubSN001/ra.xml
+++ b/ergoCubSN001/ra.xml
@@ -9,13 +9,13 @@
 
     <devices>
 
-    <!-- POS4 
+    <!-- POS4 -->
     <xi:include href="wrappers/POS/right_hand-pos_wrapper4.xml" /> 
-    <xi:include href="hardware/POS/right_hand-pos4.xml" />-->
+    <xi:include href="hardware/POS/right_hand-pos4.xml" />
 
-    <!-- POS2 
+    <!-- POS2 -->
     <xi:include href="wrappers/POS/right_hand-pos_wrapper2.xml" /> 
-    <xi:include href="hardware/POS/right_hand-pos2.xml" />-->
+    <xi:include href="hardware/POS/right_hand-pos2.xml" />
 
     <!-- motor controllers wrappers -->
     <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" />
@@ -25,9 +25,9 @@
     <!-- 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-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" />-->
+    <xi:include href="hardware/motorControl/right_arm-eb24-j11_12-mc.xml" />
 
 
     <!--  CALIBRATORS --> 
diff --git a/ergoCubSN001/wrappers/motorControl/left_arm-mc_remapper.xml b/ergoCubSN001/wrappers/motorControl/left_arm-mc_remapper.xml
index 59be41f79b..56f9e020f4 100644
--- a/ergoCubSN001/wrappers/motorControl/left_arm-mc_remapper.xml
+++ b/ergoCubSN001/wrappers/motorControl/left_arm-mc_remapper.xml
@@ -2,15 +2,15 @@
 <!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="left_arm-mc_remapper" type="controlboardremapper">
-    <param name="axesNames">(l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow)</param><!--,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)</param>-->
-    <param name="joints"> 4 </param>
+    <param name="axesNames">(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)</param>
+    <param name="joints"> 13 </param>
     <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
             <elem name="left_arm_joints1"> left_arm-eb2-j0_1-mc   </elem>
             <elem name="left_arm_joints2"> left_arm-eb4-j2_3-mc   </elem>
-          <!--  <elem name="left_arm_joints3"> left_arm-eb31-j4_6-mc  </elem>
+            <elem name="left_arm_joints3"> left_arm-eb31-j4_6-mc  </elem>
             <elem name="left_arm_joints4"> left_arm-eb23-j7_10-mc </elem>
-            <elem name="left_arm_joints5"> left_arm-eb25-j11_12-mc </elem> -->
+            <elem name="left_arm_joints5"> left_arm-eb25-j11_12-mc </elem> 
         </paramlist>
     </action>
     <action phase="shutdown" level="20" type="detach" />
diff --git a/ergoCubSN001/wrappers/motorControl/right_arm-mc_remapper.xml b/ergoCubSN001/wrappers/motorControl/right_arm-mc_remapper.xml
index 6ef3eb0b9f..f538ee05f7 100644
--- a/ergoCubSN001/wrappers/motorControl/right_arm-mc_remapper.xml
+++ b/ergoCubSN001/wrappers/motorControl/right_arm-mc_remapper.xml
@@ -2,15 +2,15 @@
 <!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="right_arm-mc_remapper" type="controlboardremapper">
-    <param name="axesNames">(r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow)</param><!--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)</param>-->
-    <param name="joints"> 4 </param>
+    <param name="axesNames">(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)</param>
+    <param name="joints"> 13 </param>
     <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
             <elem name="right_arm_joints1"> right_arm-eb1-j0_1-mc    </elem>
             <elem name="right_arm_joints2"> right_arm-eb3-j2_3-mc    </elem>        
-         <!--   <elem name="right_arm_joints3"> right_arm-eb30-j4_6-mc   </elem>
+            <elem name="right_arm_joints3"> right_arm-eb30-j4_6-mc   </elem>
             <elem name="right_arm_joints4"> right_arm-eb22-j7_10-mc  </elem>
-            <elem name="right_arm_joints5"> right_arm-eb24-j11_12-mc </elem> -->
+            <elem name="right_arm_joints5"> right_arm-eb24-j11_12-mc </elem> 
         </paramlist>
     </action>
     <action phase="shutdown" level="20" type="detach" />