From 106c29dc1bb8a6c314c641d84513f6a801c501ce Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Thu, 17 Nov 2022 11:15:47 +0100
Subject: [PATCH 1/9] ergoCubSN000: add xml files for commanding left and right
 arm until the wrist

TO BE TESTED
---
 ergoCubSN000/calibrators/left_arm-calib.xml   |  43 ++++----
 ...4_7-eln.xml => left_arm-eb31-j4_6-eln.xml} |  30 +++---
 .../electronics/right_arm-eb30-j4_6-eln.xml   |  40 +++++++
 .../mechanicals/left_arm-eb24-j4_7-mec.xml    |  79 --------------
 .../mechanicals/left_arm-eb31-j4_6-mec.xml    |  73 +++++++++++++
 .../mechanicals/right_arm-eb27-j4_7-mec.xml   |  76 -------------
 .../mechanicals/right_arm-eb30-j4_6-mec.xml   |  73 +++++++++++++
 .../motorControl/left_arm-eb24-j4_7-mc.xml    |  72 -------------
 .../left_arm-eb24-j4_7-mc_service.xml         |  51 ---------
 .../motorControl/left_arm-eb31-j4_6-mc.xml    | 100 ++++++++++++++++++
 .../left_arm-eb31-j4_6-mc_service.xml         |  58 ++++++++++
 .../motorControl/right_arm-eb27-j4_7-mc.xml   |  72 -------------
 .../right_arm-eb27-j4_7-mc_service.xml        |  47 --------
 .../motorControl/right_arm-eb30-j4_6-mc.xml   | 100 ++++++++++++++++++
 .../right_arm-eb30-j4_6-mc_service.xml        |  58 ++++++++++
 .../motorControl/left_arm-mc_remapper.xml     |   6 +-
 .../motorControl/right_arm-mc_remapper.xml    |   6 +-
 17 files changed, 543 insertions(+), 441 deletions(-)
 rename ergoCubSN000/hardware/electronics/{left_arm-eb24-j4_7-eln.xml => left_arm-eb31-j4_6-eln.xml} (75%)
 create mode 100644 ergoCubSN000/hardware/electronics/right_arm-eb30-j4_6-eln.xml
 delete mode 100644 ergoCubSN000/hardware/mechanicals/left_arm-eb24-j4_7-mec.xml
 create mode 100644 ergoCubSN000/hardware/mechanicals/left_arm-eb31-j4_6-mec.xml
 delete mode 100644 ergoCubSN000/hardware/mechanicals/right_arm-eb27-j4_7-mec.xml
 create mode 100644 ergoCubSN000/hardware/mechanicals/right_arm-eb30-j4_6-mec.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb24-j4_7-mc.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb24-j4_7-mc_service.xml
 create mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
 create mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb27-j4_7-mc.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb27-j4_7-mc_service.xml
 create mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
 create mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml

diff --git a/ergoCubSN000/calibrators/left_arm-calib.xml b/ergoCubSN000/calibrators/left_arm-calib.xml
index 2a16f45a76..8476108940 100644
--- a/ergoCubSN000/calibrators/left_arm-calib.xml
+++ b/ergoCubSN000/calibrators/left_arm-calib.xml
@@ -6,34 +6,31 @@
                 <xi:include href="../general.xml" />
 
 		<group name="GENERAL">
-		    <param name="joints">16</param> <!-- the number of joints of the robot part -->
+		    <param name="joints">14</param> <!-- the number of joints of the robot part -->
 		    <param name="deviceName"> Left_Arm_Calibrator </param>
 		</group>
 
 <group name="HOME">
-<param name="positionHome">            5         30         0          10           0     0      0    35    15    65     0     0     0      0      0     0 </param>
-<param name="velocityHome">           10         10         10         10           30    30     30   60    30    30    30    30    30     30     30    30  </param>
-</group>                                                                        
-                                                                                
-<group name="CALIBRATION">                                                      
-<param name="calibrationType">        12          12          12         12          5       12      12      7       7       6       6       6       6       6       6       6       </param> <!-- 
-<param name="calibration1">           -54795      22399       44679      53215    -1500     -8335   10671    0       0       0       0       0       0       0       0       0       </param> -->
-<param name="calibration1">           -24629      11873       21733      50433    -1500     -8335   10671    0       0       0       0       0       0       0       0       0       </param>
-<param name="calibration2">	      0           0           0          0        16384      0       0       0       0       9102    9102    9102    9102    9102    9102    10000    </param>
-<param name="calibration3">           0           0           0          0           0       0       0       0       0       -1      1       -1      1      -1       1       1      </param>
-<param name="calibration4">           0           0           0          0           0       0       0       2045    448     240     484     243     483     255     480     725     </param>
-<param name="calibration5">           0           0           0          0           0       0       0       2140    2712    0       9       15      8      20      45      125     </param>
-<param name="calibrationZero">        0           0           0          0           0       0       0       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       0       0       0       </param>
-                                                                                                                             
-<param name="startupPosition">        -35        30           0         45           0       0       0       25      45      0       0       0       0       0       0       0       </param>
-<param name="startupVelocity">        10.0       10.0         10.0      10.0         30      30      30      60      100     100     100     100     100     100     100     100     </param>
-<param name="startupMaxPwm">          8000       8000         8000      8000         2000    0       0       0       0       0       0       0       0       0       0       0       </param>
-<param name="startupPosThreshold">    2          2            2          2           90      90      90      90      90      90      90      90      90      90      90      90      </param>
+<param name="positionHome">            5         30         0          10         0        0       0       30.00    30.00    30.00    30.00    30.00    30.00    30.00  </param>
+<param name="velocityHome">           10         10         10         10         10       10      10      40.00    40.00    40.00    40.00    40.00    40.00    40.00  </param>
+</group>
+<group name="CALIBRATION">
+<param name="calibrationType">        12         12         12         12         12       12      12      12       12       12       12       12       12       12      </param>
+<param name="calibration1">           -41179     7782       11224      25717      6865     29533   30289   0        0        0        0        0        0        0      </param>
+<param name="calibration2">	          0           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        0        0     </param>
+<param name="calibration4">           0           0           0          0           0       0       0       0       0       0        0        0        0        0    </param>
+<param name="calibration5">           0           0           0          0           0       0       0       0       0       0        0        0        0          0     </param>
+<param name="calibrationZero">        0           0           0          0           0       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         0      </param>
+
+<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0        30.0        30.0    30.0    30.0    30.0    30.0    30.0   </param>
+<param name="startupVelocity">        10.0       10.0         10.0      10.0       30.0    30.0    30.0       40.0        40.0    40.0    40.0    40.0    40.0    40.0  </param>
+<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000      0           0       0       0       0       0       0      </param>
+<param name="startupPosThreshold">    2          2            2          2         2       2       2          10          10      10      10      10      10      10    </param>
 </group>
 
-<param name="CALIB_ORDER"> (1) (0) (2) (3) (4) (5 6) (7) (8 9) (11 13) (10 12 14 15)</param> <!-- removed joint 14 -->
-<!-- <param name="CALIB_ORDER">(1) (0) (2) (3) (4) (5 6) (7) (8 9) (11 13) (10 12 14 15) </param> -->
+<param name="CALIB_ORDER">  (1) (0) (2) (3) (5 6 7) (8 9 10 11 12 13)</param>
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">left_arm-mc_remapper</param>
 		</action>
@@ -47,4 +44,4 @@
 	</device>
 
 
- 
+
diff --git a/ergoCubSN000/hardware/electronics/left_arm-eb24-j4_7-eln.xml b/ergoCubSN000/hardware/electronics/left_arm-eb31-j4_6-eln.xml
similarity index 75%
rename from ergoCubSN000/hardware/electronics/left_arm-eb24-j4_7-eln.xml
rename to ergoCubSN000/hardware/electronics/left_arm-eb31-j4_6-eln.xml
index aabfdfd1b2..561e74e8a0 100644
--- a/ergoCubSN000/hardware/electronics/left_arm-eb24-j4_7-eln.xml
+++ b/ergoCubSN000/hardware/electronics/left_arm-eb31-j4_6-eln.xml
@@ -4,37 +4,37 @@
 <params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
 
     <xi:include href="./pc104.xml" />
-    
+
     <group name="ETH_BOARD">
 
         <group name="ETH_BOARD_PROPERTIES">
-            <param name="IpAddress">                10.0.1.24               </param>
+            <param name="IpAddress">                10.0.1.31                </param>
             <param name="IpPort">                   12345                   </param>
-            <param name="Type">                     mc4plus                 </param>
+            <param name="Type">                     amc                    </param>
             <param name="maxSizeRXpacket">          768                     </param>
             <param name="maxSizeROP">               384                     </param>
         </group>
 
         <group name="ETH_BOARD_SETTINGS">
-            <param name="Name">                     "left_arm-eb24-j4_7"    </param> 
+            <param name="Name">                     "left_arm-eb31-j4_6"        </param>
             <group name="RUNNINGMODE">
                 <param name="period">                   1000                </param>
                 <param name="maxTimeOfRXactivity">      400                 </param>
-                <param name="maxTimeOfDOactivity">      300                 </param>   
-                <param name="maxTimeOfTXactivity">      300                 </param>                
-                <param name="TXrateOfRegularROPs">      3                   </param> 
-            </group>              
-        </group>                 
-        
+                <param name="maxTimeOfDOactivity">      300                 </param>
+                <param name="maxTimeOfTXactivity">      300                 </param>
+                <param name="TXrateOfRegularROPs">      5                   </param>
+            </group>
+        </group>
+
         <group name="ETH_BOARD_ACTIONS">
             <group name="MONITOR_ITS_PRESENCE">
-                <param name="enabled">                  true                </param> 
-                <param name="timeout">                  0.020               </param> 
-                <param name="periodOfMissingReport">    60.0                </param> 
+                <param name="enabled">                  true                </param>
+                <param name="timeout">                  0.020               </param>
+                <param name="periodOfMissingReport">    60.0                </param>
             </group>
         </group>
 
-    </group>      
-    
+    </group>
+
 </params>
 
diff --git a/ergoCubSN000/hardware/electronics/right_arm-eb30-j4_6-eln.xml b/ergoCubSN000/hardware/electronics/right_arm-eb30-j4_6-eln.xml
new file mode 100644
index 0000000000..6ad9318d31
--- /dev/null
+++ b/ergoCubSN000/hardware/electronics/right_arm-eb30-j4_6-eln.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
+
+    <xi:include href="./pc104.xml" />
+
+    <group name="ETH_BOARD">
+
+        <group name="ETH_BOARD_PROPERTIES">
+            <param name="IpAddress">                10.0.1.30               </param>
+            <param name="IpPort">                   12345                   </param>
+            <param name="Type">                     amc                    </param>
+            <param name="maxSizeRXpacket">          768                     </param>
+            <param name="maxSizeROP">               384                     </param>
+        </group>
+
+        <group name="ETH_BOARD_SETTINGS">
+            <param name="Name">                     "right_arm-eb30-j4_6"        </param>
+            <group name="RUNNINGMODE">
+                <param name="period">                   1000                </param>
+                <param name="maxTimeOfRXactivity">      400                 </param>
+                <param name="maxTimeOfDOactivity">      300                 </param>
+                <param name="maxTimeOfTXactivity">      300                 </param>
+                <param name="TXrateOfRegularROPs">      5                   </param>
+            </group>
+        </group>
+
+        <group name="ETH_BOARD_ACTIONS">
+            <group name="MONITOR_ITS_PRESENCE">
+                <param name="enabled">                  true                </param>
+                <param name="timeout">                  0.020               </param>
+                <param name="periodOfMissingReport">    60.0                </param>
+            </group>
+        </group>
+
+    </group>
+
+</params>
+
diff --git a/ergoCubSN000/hardware/mechanicals/left_arm-eb24-j4_7-mec.xml b/ergoCubSN000/hardware/mechanicals/left_arm-eb24-j4_7-mec.xml
deleted file mode 100644
index ca7a8e22cb..0000000000
--- a/ergoCubSN000/hardware/mechanicals/left_arm-eb24-j4_7-mec.xml
+++ /dev/null
@@ -1,79 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
- 
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-    <group name="GENERAL">
-        <param name="MotioncontrolVersion"> 6 </param>
-        <param name="Joints"> 4 </param> <!-- the number of joints of the robot part -->
-
-        <!-- joint number in sub-part           0                 1              2              3           -->
-        <param name="AxisName">                 "l_wrist_prosup" "l_wrist_pitch" "l_wrist_yaw"  "l_hand_finger" </param> 
-        <param name="AxisType">                 "revolute"       "revolute"      "revolute"     "revolute"      </param>
-        <param name="AxisMap">                  0                1               2              3               </param>   
-        <param name="Encoder">                  182.044          182.044         182.044        182.044         </param>
-        <param name="fullscalePWM">             3360            3360          3360          3360          </param>
-        <param name="ampsToSensor">             1000.0          1000.0        1000.0        1000.0        </param>
-        <param name="Gearbox_M2J">              -100             159             159            256              </param>
-        <param name="Gearbox_E2J">   	          1             1                1              1               </param>
-        <param name="useMotorSpeedFbk">           1             1                1              1               </param>
-        <param name="MotorType">                 "DC"         "DC"               "DC"           "DC"               </param>
-        <param name="Verbose">                  0                                                               </param>
-    </group>
-    
-    <group name="LIMITS">
-        <param name="hardwareJntPosMax">         90          30             35             60          </param>
-        <param name="hardwareJntPosMin">        -90         -80            -15             10          </param> 
-        <param name="rotorPosMax">              0           0               0              0           </param>
-        <param name="rotorPosMin">              0           0               0              0           </param> 
-   </group>
-
-
-    <group name="COUPLINGS"> 
-                
-        <param name ="matrixJ2M"> 
-            1.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000
-            0.000  -1.000  +1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-           
-        <param name ="matrixM2J"> 
-            1.000   0.000   0.000   0.000
-            0.000   1.000  +1.000   0.000
-            0.000   0.000  +1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-
-        <param name ="matrixE2J">  
-            1.000   0.000   0.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000   0.000   0.000
-            0.000   0.000   1.000   0.000   0.000   0.000
-            0.000   0.000   0.000   1.000   0.000   0.000   
-        </param>
-                
-    </group>      
-
-    <group name="JOINTSET_CFG"> 
-        <param name= "numberofsets"> 3</param>
-         <group name="JOINTSET_0">
-                    <param name="listofjoints">  0             </param>
-                    <param name="constraint">    none          </param>
-                    <param name="param1">        0             </param>
-                    <param name="param2">        0             </param>
-            </group> 
-            <group name="JOINTSET_1">
-                    <param name="listofjoints">   1 2           </param>
-                    <param name="constraint">     none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-            <group name="JOINTSET_2">
-                    <param name="listofjoints">   3             </param>
-                    <param name="constraint">     none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-    </group>              
-          
-    
-</params>
diff --git a/ergoCubSN000/hardware/mechanicals/left_arm-eb31-j4_6-mec.xml b/ergoCubSN000/hardware/mechanicals/left_arm-eb31-j4_6-mec.xml
new file mode 100644
index 0000000000..ad35459fc6
--- /dev/null
+++ b/ergoCubSN000/hardware/mechanicals/left_arm-eb31-j4_6-mec.xml
@@ -0,0 +1,73 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
+    <group name="GENERAL">
+        <param name="MotioncontrolVersion"> 6 </param>
+        <param name="Joints"> 3 </param>
+        <param name="AxisMap">               0             1             2           </param>
+        <param name="AxisName">         "l_wrist_yaw"   "l_wrist_roll" "l_wrist_pitch"     </param>
+        <param name="AxisType">         "revolute"    "revolute"    "revolute"       </param>
+        <param name="Encoder">          182.044       182.044       182.044          </param>
+        <param name="fullscalePWM">     32000         32000         32000            </param>
+        <param name="ampsToSensor">     1000.0        1000.0        1000.0           </param>
+        <param name="Gearbox_M2J">     -384.44       -384.44       -384.44           </param>
+        <param name="Gearbox_E2J">      1.778         1.778         1.778            </param>
+        <param name="useMotorSpeedFbk"> 0             0             0                </param>
+        <param name="MotorType">        "FAULHABER"   "FAULHABER"   "FAULHABER"      </param>
+        <param name="Verbose">      0   </param>
+    </group>
+
+    <group name="LIMITS">
+        <param name="hardwareJntPosMax">     92            52            32          </param>
+        <param name="hardwareJntPosMin">    -92           -62           -32          </param>
+        <param name="rotorPosMin">           0             0             0           </param>
+        <param name="rotorPosMax">           0             0             0           </param>
+    </group>
+
+    <group name="AMCBLDC">
+        <param name="HasHallSensor">         1             1             1           </param>
+        <param name="HasTempSensor">         0             0             0           </param>
+        <param name="HasRotorEncoder">       0             0             0           </param>
+        <param name="HasRotorEncoderIndex">  0             0             0           </param>
+        <param name="HasSpeedEncoder">       0             0             0           </param>
+        <param name="RotorIndexOffset">      0             0             0           </param>
+        <param name="MotorPoles">            14            14            14          </param>
+   </group>
+
+    <group name="COUPLINGS">
+
+        <param name ="matrixJ2M">
+            1.0 0.0 0.0 0.0
+            0.0 1.0 0.0 0.0
+            0.0 0.0 1.0 0.0
+            0.0 0.0 0.0 1.0
+        </param>
+
+        <param name ="matrixM2J">
+            1.0 0.0 0.0 0.0
+            0.0 1.0 0.0 0.0
+            0.0 0.0 1.0 0.0
+            0.0 0.0 0.0 1.0
+        </param>
+
+        <param name ="matrixE2J">
+            1.00    0.00    0.00    0.00    0.00    0.00
+            0.00    1.00    0.00    0.00    0.00    0.00
+            0.00    0.00    1.00    0.00    0.00    0.00
+            0.00    0.00    0.00    1.00    0.00    0.00
+        </param>
+
+    </group>
+
+    <group name="JOINTSET_CFG">
+        <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>
+        </group>
+    </group>
+
+</params>
diff --git a/ergoCubSN000/hardware/mechanicals/right_arm-eb27-j4_7-mec.xml b/ergoCubSN000/hardware/mechanicals/right_arm-eb27-j4_7-mec.xml
deleted file mode 100644
index 879055ae8d..0000000000
--- a/ergoCubSN000/hardware/mechanicals/right_arm-eb27-j4_7-mec.xml
+++ /dev/null
@@ -1,76 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
- 
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-    <group name="GENERAL">
-        <param name="MotioncontrolVersion"> 6 </param>
-        <param name="Joints"> 4 </param> <!-- the number of joints of the robot part -->
-
-        <!-- joint number in sub-part           0                 1              2              3           -->
-        <param name="AxisName">                 "r_wrist_prosup" "r_wrist_pitch" "r_wrist_yaw"  "r_hand_finger"  </param> 
-        <param name="AxisType">                 "revolute"       "revolute"      "revolute"     "revolute"      </param>
-        <param name="AxisMap">                  0                1               2              3               </param>   <!-- joint as effectively mapped onto the hw of the board -->
-        <param name="Encoder">                  182.044          182.044         182.044        182.044         </param>
-        <param name="fullscalePWM">            3360            3360             3360            3360            </param>
-        <param name="ampsToSensor">            1000.0          1000.0           1000.0          1000.0          </param>
-        <param name="Gearbox_M2J">              100               159             159            256            </param>
-        <param name="Gearbox_E2J">   	          1               1                 1              1            </param>
-        <param name="useMotorSpeedFbk">           1               1                 1              1            </param>
-        <param name="MotorType">                "DC"             "DC"              "DC"           "DC"          </param>
-        <param name="Verbose">                  0                                                               </param>
-    </group>
-    
-    <group name="LIMITS">
-        <param name="hardwareJntPosMax">         90          30             35             60          </param>
-        <param name="hardwareJntPosMin">        -97         -80            -15             10          </param> 
-        <param name="rotorPosMax">              0           0               0              0           </param>
-        <param name="rotorPosMin">              0           0               0              0           </param> 
-    </group>
-
-    <group name="COUPLINGS"> 
-                
-        <param name ="matrixJ2M"> 
-            1.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000
-            0.000  -1.000  +1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-           
-        <param name ="matrixM2J"> 
-            1.000   0.000   0.000   0.000
-            0.000   1.000  +1.000   0.000
-            0.000   0.000  +1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-
-        <param name ="matrixE2J">  
-            1.000   0.000   0.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000   0.000   0.000
-            0.000   0.000   1.000   0.000   0.000   0.000
-            0.000   0.000   0.000   1.000   0.000   0.000   
-        </param>
-                
-    </group>      
-
-    <group name="JOINTSET_CFG"> 
-        <param name= "numberofsets"> 3</param>
-         <group name="JOINTSET_0">
-                    <param name="listofjoints">  0             </param>
-                    <param name="constraint">    none          </param>
-                    <param name="param1">        0             </param>
-                    <param name="param2">        0             </param>
-            </group> 
-            <group name="JOINTSET_1">
-                    <param name="listofjoints">   1 2           </param>
-                    <param name="constraint">     none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-            <group name="JOINTSET_2">
-                    <param name="listofjoints">   3             </param>
-                    <param name="constraint">     none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-    </group>              
-</params>
diff --git a/ergoCubSN000/hardware/mechanicals/right_arm-eb30-j4_6-mec.xml b/ergoCubSN000/hardware/mechanicals/right_arm-eb30-j4_6-mec.xml
new file mode 100644
index 0000000000..c0209d3f7c
--- /dev/null
+++ b/ergoCubSN000/hardware/mechanicals/right_arm-eb30-j4_6-mec.xml
@@ -0,0 +1,73 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
+    <group name="GENERAL">
+        <param name="MotioncontrolVersion"> 6 </param>
+        <param name="Joints"> 3 </param>
+        <param name="AxisMap">               0             1             2           </param>
+        <param name="AxisName">         "r_wrist_yaw"   "r_wrist_roll" "r_wrist_pitch"     </param>
+        <param name="AxisType">         "revolute"    "revolute"    "revolute"       </param>
+        <param name="Encoder">          182.044       182.044       182.044          </param>
+        <param name="fullscalePWM">     32000         32000         32000            </param>
+        <param name="ampsToSensor">     1000.0        1000.0        1000.0           </param>
+        <param name="Gearbox_M2J">     -384.44       -384.44       -384.44           </param>
+        <param name="Gearbox_E2J">      1.778         1.778         1.778            </param>
+        <param name="useMotorSpeedFbk"> 0             0             0                </param>
+        <param name="MotorType">        "FAULHABER"   "FAULHABER"   "FAULHABER"      </param>
+        <param name="Verbose">      0   </param>
+    </group>
+
+    <group name="LIMITS">
+        <param name="hardwareJntPosMax">     92            52            32          </param>
+        <param name="hardwareJntPosMin">    -92           -62           -32          </param>
+        <param name="rotorPosMin">           0             0             0           </param>
+        <param name="rotorPosMax">           0             0             0           </param>
+    </group>
+
+    <group name="AMCBLDC">
+        <param name="HasHallSensor">         1             1             1           </param>
+        <param name="HasTempSensor">         0             0             0           </param>
+        <param name="HasRotorEncoder">       0             0             0           </param>
+        <param name="HasRotorEncoderIndex">  0             0             0           </param>
+        <param name="HasSpeedEncoder">       0             0             0           </param>
+        <param name="RotorIndexOffset">      0             0             0           </param>
+        <param name="MotorPoles">            14            14            14          </param>
+   </group>
+
+    <group name="COUPLINGS">
+
+        <param name ="matrixJ2M">
+            1.0 0.0 0.0 0.0
+            0.0 1.0 0.0 0.0
+            0.0 0.0 1.0 0.0
+            0.0 0.0 0.0 1.0
+        </param>
+
+        <param name ="matrixM2J">
+            1.0 0.0 0.0 0.0
+            0.0 1.0 0.0 0.0
+            0.0 0.0 1.0 0.0
+            0.0 0.0 0.0 1.0
+        </param>
+
+        <param name ="matrixE2J">
+            1.00    0.00    0.00    0.00    0.00    0.00
+            0.00    1.00    0.00    0.00    0.00    0.00
+            0.00    0.00    1.00    0.00    0.00    0.00
+            0.00    0.00    0.00    1.00    0.00    0.00
+        </param>
+
+    </group>
+
+    <group name="JOINTSET_CFG">
+        <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>
+        </group>
+    </group>
+
+</params>
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb24-j4_7-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb24-j4_7-mc.xml
deleted file mode 100644
index b1bf317339..0000000000
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb24-j4_7-mc.xml
+++ /dev/null
@@ -1,72 +0,0 @@
-<?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="left_arm-eb24-j4_7-mc" type="embObjMotionControl">
-
-    <xi:include href="../../general.xml" />
-    <xi:include href="../../hardware/electronics/left_arm-eb24-j4_7-eln.xml" />
-    <xi:include href="../../hardware/mechanicals/left_arm-eb24-j4_7-mec.xml" />
-    <xi:include href="./left_arm-eb24-j4_7-mc_service.xml" />
-
-    <!-- joint number                           0                   1                   2                   3                   -->
-    <!-- joint name                             "l_wrist_prosup"    "l_wrist_pitch"     "l_wrist_yaw"       "l_hand_finger"     -->
-    <group name="LIMITS">
-        <param name="jntPosMax">                 88                 25                  35                  60                  </param>
-        <param name="jntPosMin">                -88                 -70                 -15                 10                  </param>
-        <param name="jntVelMax">                1000                1000                1000                1000                </param>
-        <param name="motorOverloadCurrents">    10000               2000                2000                2000                </param>
-        <param name="motorNominalCurrents">     5000                1000                1000                600                 </param>
-        <param name="motorPeakCurrents">        5000                2000                2000                1000                </param>
-        <param name="motorPwmLimit">            3360                3360                3360                3360                </param>
-    </group>
-
-    <group name="TIMEOUTS">
-        <param name="velocity">                 100                 100                 100                 100                 </param>
-    </group>
-
-    <group name="IMPEDANCE">
-        <param name="stiffness">                0.0                 0.0                 0.0                 0.0                 </param>
-        <param name="damping">                  0.0                 0.0                 0.0                 0.0                 </param>
-    </group>
-
-    <group name="CONTROLS">
-       <param name="positionControl">           POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-       <param name="velocityControl">           POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-       <param name="mixedControl">              POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-       <param name="torqueControl">             none                none                none                none                </param>
-       <param name="currentPid">                none                none                none                none                </param>
-       <param name="speedPid">                  none                none                none                none                </param>
-    </group>
-
-
-    <!-- default position PID: begin -->
-
-    <group name="POS_PID_DEFAULT">
-        <param name="controlLaw">           minjerk                 </param>
-        <param name="outputType">           pwm                     </param>
-        <param name="fbkControlUnits">      metric_units            </param>
-        <param name="outputControlUnits">   machine_units           </param>
-        <param name="kp">                   -200.0      -500.0     -500.0     -500.0      </param>
-        <param name="kd">                   0           0           0           0           </param>
-        <param name="ki">                   -200.0      -50.0       -50.0       -50.0       </param>
-        <param name="maxOutput">            3360        3360        3360        3360        </param>
-        <param name="maxInt">               3360        3360        3360        3360        </param>
-        <param name="stictionUp">           0           0           0           0           </param>
-        <param name="stictionDown">         0           0           0           0           </param>
-        <param name="kff">                  0           0           0           0           </param>
-    </group>
-
-    <!-- default position PID: end -->
-
-
-    <!-- other default PIDs: begin -->
-    <!-- other default PIDs: end -->
-
-
-    <!-- custom PIDs: begin -->
-    <!-- custom PIDs: end -->
-
-</device>
-
-
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb24-j4_7-mc_service.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb24-j4_7-mc_service.xml
deleted file mode 100644
index dfdab9e819..0000000000
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb24-j4_7-mc_service.xml
+++ /dev/null
@@ -1,51 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <group name="SERVICE">
-        
-        <param name="type"> eomn_serv_MC_mc4plus </param> 
-    
-        <group name="PROPERTIES">
-        
-            <group name="ETHBOARD">
-                <param name="type">             mc4plus             </param> 
-            </group>          
-
-            
-            
-
-            <group name="JOINTMAPPING">
-
-                <group name="ACTUATOR">                 
-                    <param name="type">             pwm                 pwm                 pwm                 pwm                     </param>
-                    <param name="port">             CONN:P5             CONN:P2             CONN:P4             CONN:P3                 </param>
-                </group>
-                
-                <group name="ENCODER1">  
-                    <param name="type">             none                aea                 aea                 absanalog               </param>  
-                    <param name="port">             CONN:none           CONN:P10            CONN:P11            CONN:P3                 </param>
-                    <param name="position">         none                atjoint             atjoint             atjoint                 </param>
-                    <param name="resolution">          1                -4096               4096                65535          </param> <!-- 6535: this value will change by fw during calibration (type 6) --> 
-                    <param name="tolerance">           0                0.703               0.703                  0                        </param>  
-
-                </group>        
-                
-                <group name="ENCODER2">
-                    <param name="type">             qenc                qenc                qenc                none                    </param>
-                    <param name="port">             CONN:P5             CONN:P2             CONN:P4             CONN:none               </param>
-                    <param name="position">         atmotor             atmotor             atmotor             none                    </param>
-                    <param name="resolution">       -2048               1600                1600                1                       </param>
-                    <param name="tolerance">         0                   0                   0                  0                        </param>  
-
-                </group>  
-            </group>    
-            
-        </group>
-           
-    </group>
-    
-  
-</params>
-
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
new file mode 100644
index 0000000000..7c60b84210
--- /dev/null
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
@@ -0,0 +1,100 @@
+<?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="left_arm-eb31-j4_6-mc" type="embObjMotionControl">
+
+    <xi:include href="../../general.xml" />
+    <xi:include href="../../hardware/electronics/left_arm-eb31-j4_6-eln.xml" />
+    <xi:include href="../../hardware/mechanicals/left_arm-eb31-j4_6-mec.xml" />
+    <xi:include href="./left_arm-eb31-j4_6-mc_service.xml" />
+
+    <!-- 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="jntVelMax">                90                  90                  90                  </param>
+        <param name="motorNominalCurrents">     1000                1000                1000                </param>
+        <param name="motorPeakCurrents">        1500                1500                1500                </param>
+        <param name="motorOverloadCurrents">    2000                2000                2000                </param>
+        <param name="motorPwmLimit">            16000               16000               16000               </param>
+    </group>
+
+    <group name="TIMEOUTS">
+        <param name="velocity">                 100                 100                 100                 </param>
+    </group>
+
+    <group name="IMPEDANCE">
+        <param name="stiffness">                0                   0                   0                   </param>
+        <param name="damping">                  0                   0                   0                   </param>
+    </group>
+
+    <group name="CONTROLS">
+        <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="torqueControl">            none                none                none                </param>
+        <param name="currentPid">               2FOC_CUR_CONTROL    2FOC_CUR_CONTROL    2FOC_CUR_CONTROL    </param>
+        <param name="speedPid">                 2FOC_VEL_CONTROL    2FOC_VEL_CONTROL    2FOC_VEL_CONTROL    </param>
+    </group>
+
+
+    <!-- default position PID: begin -->
+
+    <group name="POS_PID_DEFAULT">
+        <param name="controlLaw">             minjerk               </param>
+        <param name="outputType">             current               </param>
+        <param name="fbkControlUnits">        metric_units          </param>
+        <param name="outputControlUnits">     machine_units         </param>
+        <param name="kp">          -67         -67         -67      </param>
+        <param name="kd">          -11         -11         -11      </param>
+        <param name="ki">          -294        -294        -294     </param>
+        <param name="maxOutput">    500         500         500     </param>
+        <param name="maxInt">       200         200         200     </param>
+        <param name="stictionUp">   0           0           0       </param>
+        <param name="stictionDown"> 0           0           0       </param>
+        <param name="kff">          0           0           0       </param>
+    </group>
+
+    <!-- default position PID: end -->
+
+
+    <!-- other default PIDs: begin -->
+
+    <group name="2FOC_CUR_CONTROL">
+        <param name="controlLaw">           low_lev_current     </param>
+        <param name="fbkControlUnits">      machine_units       </param>
+        <param name="outputControlUnits">   machine_units       </param>
+        <param name="kp">                   2           2           2       </param>
+        <param name="kd">                   0           0           0       </param>
+        <param name="ki">                   500         500         500     </param>
+        <param name="shift">                0           0           0       </param>
+        <param name="maxOutput">            32000       32000       32000   </param>
+        <param name="maxInt">               32000       32000       32000   </param>
+        <param name="kff">                  0            0          0       </param>
+    </group>
+
+    <group name="2FOC_VEL_CONTROL">
+        <param name="controlLaw">           low_lev_speed       </param>
+        <param name="fbkControlUnits">      machine_units       </param>
+        <param name="outputControlUnits">   machine_units       </param>
+        <param name="kff">                  0           0           0       </param>
+        <param name="kp">                   12          12          12      </param>
+        <param name="kd">                   0           0           0       </param>
+        <param name="ki">                   16          16          16      </param>
+        <param name="shift">                10          10          10      </param>
+        <param name="maxOutput">            32000       32000       32000   </param>
+        <param name="maxInt">               32000       32000       32000   </param>
+    </group>
+
+    <!-- other default PIDs: end -->
+
+
+    <!-- custom PIDs: begin -->
+
+    <!-- custom PIDs: end -->
+
+</device>
+
+
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
new file mode 100644
index 0000000000..f9eb6a0968
--- /dev/null
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
@@ -0,0 +1,58 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
+
+    <group name="SERVICE">
+
+        <param name="type"> eomn_serv_MC_foc </param>
+
+        <group name="PROPERTIES">
+
+            <group name="ETHBOARD">
+                <param name="type">                 amc        </param>
+            </group>
+
+            <group name="CANBOARDS">
+                <param name="type">                 amcbldc     </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">            0           </param>
+                    <param name="build">            7           </param>
+                </group>
+            </group>
+
+            <group name="JOINTMAPPING">
+
+                <group name="ACTUATOR">
+                    <param name="type">             eomc_act_foc        foc                 foc         </param>
+                    <param name="port">             CAN1:1:0            CAN1:2:0            CAN1:3:0    </param>
+                </group>
+
+                <group name="ENCODER1">
+                    <param name="type">             eomc_enc_aea        aea                 aea         </param>
+                    <param name="port">             CONN:J5_X1          CONN:J5_X2          CONN:J5_X3     </param>
+                    <param name="position">         eomc_pos_atjoint    atjoint             atjoint     </param>
+                    <param name="resolution">       4096                4096                4096        </param>
+                    <param name="tolerance">        0.4                 0.4                 0.4         </param>
+                </group>
+
+                <group name="ENCODER2">
+                    <param name="type">             none                none                none       </param>
+                    <param name="port">             CAN1:1:0            CAN1:2:0            CAN1:3:0   </param>
+                    <param name="position">         atmotor             atmotor             atmotor    </param>
+                    <param name="resolution">        0                   0                   0         </param>
+                    <param name="tolerance">         0                   0                   0         </param>
+                </group>
+
+            </group>
+
+        </group>
+
+    </group>
+
+</params>
\ No newline at end of file
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb27-j4_7-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb27-j4_7-mc.xml
deleted file mode 100644
index ab8753c1c6..0000000000
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb27-j4_7-mc.xml
+++ /dev/null
@@ -1,72 +0,0 @@
-<?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="right_arm-eb27-j4_7-mc" type="embObjMotionControl">
-
-    <xi:include href="../../general.xml" />
-    <xi:include href="../../hardware/electronics/right_arm-eb27-j4_7-eln.xml" />
-    <xi:include href="../../hardware/mechanicals/right_arm-eb27-j4_7-mec.xml" />
-    <xi:include href="./right_arm-eb27-j4_7-mc_service.xml" />
-
-    <!-- joint number                           0                   1                   2                   3                   -->
-    <!-- joint name                             "r_wrist_prosup"    "r_wrist_pitch"     "r_wrist_yaw"       "r_hand_finger"     -->
-    <group name="LIMITS">
-        <param name="jntPosMax">                 88                 25                  35                  60                  </param>
-        <param name="jntPosMin">                -88                 -70                 -15                 10                  </param>
-        <param name="jntVelMax">                1000                1000                1000                1000                </param>
-        <param name="motorOverloadCurrents">    10000               2000                2000                2000                </param>
-        <param name="motorNominalCurrents">     5000                1000                1000                600                 </param>
-        <param name="motorPeakCurrents">        5000                2000                2000                1000                </param>
-        <param name="motorPwmLimit">            3360                3360                3360                3360                </param>
-    </group>
-
-    <group name="TIMEOUTS">
-        <param name="velocity">                 100                 100                 100                 100                 </param>
-    </group>
-
-    <group name="IMPEDANCE">
-        <param name="stiffness">                0.0                 0.0                 0.0                 0.0                 </param>
-        <param name="damping">                  0.0                 0.0                 0.0                 0.0                 </param>
-    </group>
-
-    <group name="CONTROLS">
-       <param name="positionControl">           POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-       <param name="velocityControl">           POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-       <param name="mixedControl">              POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-       <param name="torqueControl">             none                none                none                none                </param>
-       <param name="currentPid">                none                none                none                none                </param>
-       <param name="speedPid">                  none                none                none                none                </param>
-    </group>
-
-
-    <!-- default position PID: begin -->
-
-    <group name="POS_PID_DEFAULT">
-        <param name="controlLaw">           minjerk                 </param>
-        <param name="outputType">           pwm                     </param>
-        <param name="fbkControlUnits">      metric_units            </param>
-        <param name="outputControlUnits">   machine_units           </param>
-        <param name="kp">                   200.0       500.0       500.0        500.0      </param>
-        <param name="kd">                   0.0         0.0         0.0         0.0         </param>
-        <param name="ki">                   200.0       50.0        50.0        50.0        </param>
-        <param name="maxOutput">            3360        3360        3360        3360        </param>
-        <param name="maxInt">               3360        3360        3360        3360        </param>
-        <param name="stictionUp">           0           0           0           0           </param>
-        <param name="stictionDown">         0           0           0           0           </param>
-        <param name="kff">                  0           0           0           0           </param>
-    </group>
-
-    <!-- default position PID: end -->
-
-
-    <!-- other default PIDs: begin -->
-    <!-- other default PIDs: end -->
-
-
-    <!-- custom PIDs: begin -->
-    <!-- custom PIDs: end -->
-
-</device>
-
-
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb27-j4_7-mc_service.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb27-j4_7-mc_service.xml
deleted file mode 100644
index 9121bfbc6e..0000000000
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb27-j4_7-mc_service.xml
+++ /dev/null
@@ -1,47 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <group name="SERVICE">
-        
-        <param name="type"> eomn_serv_MC_mc4plus </param> 
-    
-        <group name="PROPERTIES">
-        
-            <group name="ETHBOARD">
-                <param name="type">             mc4plus             </param> 
-            </group>          
-
-
-            <group name="JOINTMAPPING">
-
-                <group name="ACTUATOR">                 
-                    <param name="type">             pwm                 pwm                 pwm                 pwm                     </param>
-                    <param name="port">             CONN:P5             CONN:P2             CONN:P4             CONN:P3                 </param>
-                </group>
-                
-                <group name="ENCODER1">  
-                    <param name="type">             none                aea                 aea                 absanalog               </param>  
-                    <param name="port">             CONN:none             CONN:P10            CONN:P11            CONN:P3                 </param>
-                    <param name="position">         none             atjoint             atjoint             atjoint                 </param>
-                    <param name="resolution">       1                   4096               -4096                65535                     </param> <!-- 65535:this value will change by fw during calibration (type 6 or 7) -->
-                    <param name="tolerance">      0                     0.703               0.703                  0                        </param>  
-                </group>        
-                
-                <group name="ENCODER2">
-                    <param name="type">             qenc                qenc                qenc                none                    </param>
-                    <param name="port">             CONN:P5             CONN:P2             CONN:P4             CONN:none               </param>
-                    <param name="position">         atmotor             atmotor             atmotor             none                    </param>
-                    <param name="resolution">       -2048               1600                1600                1                       </param>
-                    <param name="tolerance">        0                0                   0                  0                        </param>  
-                </group>  
-            </group>    
-            
-        </group>
-           
-    </group>
-    
-  
-</params>
-
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
new file mode 100644
index 0000000000..16dfdb0a36
--- /dev/null
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
@@ -0,0 +1,100 @@
+<?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="right_arm-eb30-j4_6-mc" type="embObjMotionControl">
+
+    <xi:include href="../../general.xml" />
+    <xi:include href="../../hardware/electronics/right_arm-eb30-j4_6-eln.xml" />
+    <xi:include href="../../hardware/mechanicals/right_arm-eb30-j4_6-mec.xml" />
+    <xi:include href="./right_arm-eb30-j4_6-mc_service.xml" />
+
+    <!-- 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="jntVelMax">                90                  90                  90                  </param>
+        <param name="motorNominalCurrents">     1000                1000                1000                </param>
+        <param name="motorPeakCurrents">        1500                1500                1500                </param>
+        <param name="motorOverloadCurrents">    2000                2000                2000                </param>
+        <param name="motorPwmLimit">            16000               16000               16000               </param>
+    </group>
+
+    <group name="TIMEOUTS">
+        <param name="velocity">                 100                 100                 100                 </param>
+    </group>
+
+    <group name="IMPEDANCE">
+        <param name="stiffness">                0                   0                   0                   </param>
+        <param name="damping">                  0                   0                   0                   </param>
+    </group>
+
+    <group name="CONTROLS">
+        <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="torqueControl">            none                none                none                </param>
+        <param name="currentPid">               2FOC_CUR_CONTROL    2FOC_CUR_CONTROL    2FOC_CUR_CONTROL    </param>
+        <param name="speedPid">                 2FOC_VEL_CONTROL    2FOC_VEL_CONTROL    2FOC_VEL_CONTROL    </param>
+    </group>
+
+
+    <!-- default position PID: begin -->
+
+    <group name="POS_PID_DEFAULT">
+        <param name="controlLaw">             minjerk               </param>
+        <param name="outputType">             current               </param>
+        <param name="fbkControlUnits">        metric_units          </param>
+        <param name="outputControlUnits">     machine_units         </param>
+        <param name="kp">          -67         -67         -67      </param>
+        <param name="kd">          -11         -11         -11      </param>
+        <param name="ki">          -294        -294        -294     </param>
+        <param name="maxOutput">    500         500         500     </param>
+        <param name="maxInt">       200         200         200     </param>
+        <param name="stictionUp">   0           0           0       </param>
+        <param name="stictionDown"> 0           0           0       </param>
+        <param name="kff">          0           0           0       </param>
+    </group>
+
+    <!-- default position PID: end -->
+
+
+    <!-- other default PIDs: begin -->
+
+    <group name="2FOC_CUR_CONTROL">
+        <param name="controlLaw">           low_lev_current     </param>
+        <param name="fbkControlUnits">      machine_units       </param>
+        <param name="outputControlUnits">   machine_units       </param>
+        <param name="kp">                   2           2           2       </param>
+        <param name="kd">                   0           0           0       </param>
+        <param name="ki">                   500         500         500     </param>
+        <param name="shift">                0           0           0       </param>
+        <param name="maxOutput">            32000       32000       32000   </param>
+        <param name="maxInt">               32000       32000       32000   </param>
+        <param name="kff">                  0            0          0       </param>
+    </group>
+
+    <group name="2FOC_VEL_CONTROL">
+        <param name="controlLaw">           low_lev_speed       </param>
+        <param name="fbkControlUnits">      machine_units       </param>
+        <param name="outputControlUnits">   machine_units       </param>
+        <param name="kff">                  0           0           0       </param>
+        <param name="kp">                   12          12          12      </param>
+        <param name="kd">                   0           0           0       </param>
+        <param name="ki">                   16          16          16      </param>
+        <param name="shift">                10          10          10      </param>
+        <param name="maxOutput">            32000       32000       32000   </param>
+        <param name="maxInt">               32000       32000       32000   </param>
+    </group>
+
+    <!-- other default PIDs: end -->
+
+
+    <!-- custom PIDs: begin -->
+
+    <!-- custom PIDs: end -->
+
+</device>
+
+
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml
new file mode 100644
index 0000000000..f9eb6a0968
--- /dev/null
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml
@@ -0,0 +1,58 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
+
+    <group name="SERVICE">
+
+        <param name="type"> eomn_serv_MC_foc </param>
+
+        <group name="PROPERTIES">
+
+            <group name="ETHBOARD">
+                <param name="type">                 amc        </param>
+            </group>
+
+            <group name="CANBOARDS">
+                <param name="type">                 amcbldc     </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">            0           </param>
+                    <param name="build">            7           </param>
+                </group>
+            </group>
+
+            <group name="JOINTMAPPING">
+
+                <group name="ACTUATOR">
+                    <param name="type">             eomc_act_foc        foc                 foc         </param>
+                    <param name="port">             CAN1:1:0            CAN1:2:0            CAN1:3:0    </param>
+                </group>
+
+                <group name="ENCODER1">
+                    <param name="type">             eomc_enc_aea        aea                 aea         </param>
+                    <param name="port">             CONN:J5_X1          CONN:J5_X2          CONN:J5_X3     </param>
+                    <param name="position">         eomc_pos_atjoint    atjoint             atjoint     </param>
+                    <param name="resolution">       4096                4096                4096        </param>
+                    <param name="tolerance">        0.4                 0.4                 0.4         </param>
+                </group>
+
+                <group name="ENCODER2">
+                    <param name="type">             none                none                none       </param>
+                    <param name="port">             CAN1:1:0            CAN1:2:0            CAN1:3:0   </param>
+                    <param name="position">         atmotor             atmotor             atmotor    </param>
+                    <param name="resolution">        0                   0                   0         </param>
+                    <param name="tolerance">         0                   0                   0         </param>
+                </group>
+
+            </group>
+
+        </group>
+
+    </group>
+
+</params>
\ No newline at end of file
diff --git a/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
index 6bb36d93fe..53763c6936 100644
--- a/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
@@ -5,16 +5,16 @@
     <paramlist name="networks">
         <elem name="left_arm_joints1">(  0  1 0 1 )</elem>
         <elem name="left_arm_joints2">(  2  3 0 1 )</elem>
-        <elem name="left_arm_joints3">(  4 7 0 3 )</elem>
+        <elem name="left_arm_joints3">(  4  6 0 2 )</elem>
         <elem name="left_arm_joints4">(  8  11 0 3 )</elem>
         <elem name="left_arm_joints5">(  12  15 0 3 )</elem>
     </paramlist>
-    <param name="joints"> 16 </param>
+    <param name="joints"> 14 </param>
     <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
             <elem name="left_arm_joints1"> left_arm-eb1-j0_1-mc </elem>
             <elem name="left_arm_joints2"> left_arm-eb2-j2_3-mc </elem>
-            <elem name="left_arm_joints3"> left_arm-eb24-j4_7-mc </elem>
+            <elem name="left_arm_joints3"> left_arm-eb31-j4_6-mc </elem>
             <elem name="left_arm_joints4"> left_arm-eb25-j8_11-mc </elem>
             <elem name="left_arm_joints5"> left_arm-eb26-j12_15-mc </elem>
         </paramlist>
diff --git a/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
index 604b8dd3ec..9c63d3918f 100644
--- a/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
@@ -5,7 +5,7 @@
     <paramlist name="networks">
         <elem name="right_arm_joints1">(  0  1 0 1 )</elem>
         <elem name="right_arm_joints2">(  2  3 0 1 )</elem>
-        <elem name="right_arm_joints3">(  4 7 0 3 )</elem>
+        <elem name="right_arm_joints3">(  4 6 0 2 )</elem>
         <elem name="right_arm_joints4">(  8  11 0 3 )</elem>
         <elem name="right_arm_joints5">(  12  15 0 3 )</elem>
     </paramlist>
@@ -14,9 +14,9 @@
         <paramlist name="networks">
             <elem name="right_arm_joints1"> right_arm-eb3-j0_1-mc </elem>
             <elem name="right_arm_joints2"> right_arm-eb4-j2_3-mc </elem>
-            <elem name="right_arm_joints3"> right_arm-eb27-j4_7-mc </elem>
+            <elem name="right_arm_joints3"> right_arm-eb30-j4_6-mc </elem>
             <elem name="right_arm_joints4"> right_arm-eb28-j8_11-mc </elem>
-            <elem name="right_arm_joints5"> right_arm-eb29-j12_15-mc </elem>                         
+            <elem name="right_arm_joints5"> right_arm-eb29-j12_15-mc </elem>
         </paramlist>
     </action>
     <action phase="shutdown" level="20" type="detach" />

From bb178605987c310bc5d20e5269966c583f2822a6 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Thu, 17 Nov 2022 11:50:42 +0100
Subject: [PATCH 2/9] ergoCubSN000: add fixed xml files for right wrist

---
 ergoCubSN000/calibrators/right_arm-calib.xml  | 42 +++++++++----------
 .../electronics/right_arm-eb27-j4_7-eln.xml   | 40 ------------------
 .../right_arm-eb30-j4_6-mc_service.xml        | 20 ++++-----
 3 files changed, 30 insertions(+), 72 deletions(-)
 delete mode 100644 ergoCubSN000/hardware/electronics/right_arm-eb27-j4_7-eln.xml

diff --git a/ergoCubSN000/calibrators/right_arm-calib.xml b/ergoCubSN000/calibrators/right_arm-calib.xml
index e2687f9d76..03727af717 100644
--- a/ergoCubSN000/calibrators/right_arm-calib.xml
+++ b/ergoCubSN000/calibrators/right_arm-calib.xml
@@ -6,35 +6,31 @@
                 <xi:include href="../general.xml" />
 
 		<group name="GENERAL">
-		    <param name="joints">16</param> <!-- the number of joints of the robot part -->
+		    <param name="joints">14</param> <!-- the number of joints of the robot part -->
 		    <param name="deviceName"> Right_Arm_Calibrator </param>
 		</group>
-<!-- joint logical number           0         1        2         3      4      5      6     7     8     9    10    11    12     13     14    15 -->
+
 <group name="HOME">
-<param name="positionHome">         5        30         0       10      0      0     0     35    15    65     0     0     0      0      0     0     </param>
-<param name="velocityHome">        10        10        10       10     30     30     30    60    30    30    30    30    30     30     30    30     </param>
+<param name="positionHome">            5         30         0          10         0        0       0       30.00    30.00    30.00    30.00    30.00    30.00    30.00  </param>
+<param name="velocityHome">           10         10         10         10         10       10      10      40.00    40.00    40.00    40.00    40.00    40.00    40.00  </param>
 </group>
-
-
 <group name="CALIBRATION">
-<param name="calibrationType">     12          12          12         12         5       12       12     7       7       6       6       6       6       6       6       6       </param> <!--
-<param name="calibration1">        6576       -12866      -4585      -47529      1500    36903  -12703   0       0       0       0       0       0       0       0       0       </param> -->
-<param name="calibration1">        -11711       -59070      -25844       -11080      1500    36903  -12703   0       0       0       0       0       0       0       0       0       </param>
-<param name="calibration2">	   0           0           0          0	         16384   0       0       0       0       9102    9102    9102    9102    9102    9102    10000    </param>
-<param name="calibration3">        0           0           0          0          0       0       0       0       0       -1      1       -1      1       -1      1       1      </param>
-<param name="calibration4">        0           0           0          0          0       0       0       2510    1470    255     510     255     510     200     510     748     </param>
-<param name="calibration5">        0           0           0          0          0       0       0       2670    2350    22      40      18      30      10       30      120     </param>
-<param name="calibrationZero">     0           0           0          0          0       0       0       0       0       0       0       0       0       0       0       0             </param>
-<param name="calibrationDelta">    0           0          0        0          0      -5.5    -4       10       0      0       0       0       0       0       0       0       </param>
-                                                                              
-<param name="startupPosition">     -35        30           0         45          0       0       15      25      45      10       0       0       0       0       0       0       </param>
-<param name="startupVelocity">     10.0       10.0         10.0      10.0        30      30      30      60      100     100     100     100     100     100     100     100     </param>
-<param name="startupMaxPwm">       8000       8000         8000      8000        1500    0       0       0       0       0       0       0       0       0       0       0       </param>
-<param name="startupPosThreshold"> 2          2            2          2          90      90      90      90      90      90      90      90      90      90      90      90      </param>
+<param name="calibrationType">        12         12         12         12         12       12      12      12       12       12       12       12       12       12      </param>
+<param name="calibration1">           -41179     7782       11224      25717      33556   29569   36363   0        0        0        0        0        0        0      </param>
+<param name="calibration2">	          0           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        0        0     </param>
+<param name="calibration4">           0           0           0          0           0       0       0       0       0       0        0        0        0        0    </param>
+<param name="calibration5">           0           0           0          0           0       0       0       0       0       0        0        0        0          0     </param>
+<param name="calibrationZero">        0           0           0          0           0       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         0      </param>
+
+<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0        30.0        30.0    30.0    30.0    30.0    30.0    30.0   </param>
+<param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0       40.0        40.0    40.0    40.0    40.0    40.0    40.0  </param>
+<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000      0           0       0       0       0       0       0      </param>
+<param name="startupPosThreshold">    2          2            2          2         2       2       2          10          10      10      10      10      10      10    </param>
 </group>
 
-<!--param name="CALIB_ORDER"> (1) (0) (2) (3) (4) (5 6) (7) (8 9) (11 13) (10 12 14 15) </param-->
-<param name="CALIB_ORDER"> (1) (0) (2) (3) (4) (5 6) (7) (8 9) (11 13) (10 12 14 15) </param> 
+<param name="CALIB_ORDER">  (1) (0) (2) (3) (5 6 7) (8 9 10 11 12 13)</param>
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">right_arm-mc_remapper</param>
 		</action>
@@ -47,3 +43,5 @@
 
 	</device>
 
+
+
diff --git a/ergoCubSN000/hardware/electronics/right_arm-eb27-j4_7-eln.xml b/ergoCubSN000/hardware/electronics/right_arm-eb27-j4_7-eln.xml
deleted file mode 100644
index 1965ff1762..0000000000
--- a/ergoCubSN000/hardware/electronics/right_arm-eb27-j4_7-eln.xml
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <xi:include href="./pc104.xml" />
-    
-    <group name="ETH_BOARD">
-
-        <group name="ETH_BOARD_PROPERTIES">
-            <param name="IpAddress">                10.0.1.27               </param>
-            <param name="IpPort">                   12345                   </param>
-            <param name="Type">                     mc4plus                 </param>
-            <param name="maxSizeRXpacket">          768                     </param>
-            <param name="maxSizeROP">               384                     </param>
-        </group>
-
-        <group name="ETH_BOARD_SETTINGS">
-            <param name="Name">                     "right_arm-eb27-j4_7"   </param> 
-            <group name="RUNNINGMODE">
-                <param name="period">                   1000                </param>
-                <param name="maxTimeOfRXactivity">      400                 </param>
-                <param name="maxTimeOfDOactivity">      300                 </param>   
-                <param name="maxTimeOfTXactivity">      300                 </param>                
-                <param name="TXrateOfRegularROPs">      3                   </param> 
-            </group>              
-        </group>                 
-        
-        <group name="ETH_BOARD_ACTIONS">
-            <group name="MONITOR_ITS_PRESENCE">
-                <param name="enabled">                  true                </param> 
-                <param name="timeout">                  0.020               </param> 
-                <param name="periodOfMissingReport">    60.0                </param> 
-            </group>
-        </group>
-
-    </group>      
-    
-</params>
-
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml
index f9eb6a0968..9b2c7be753 100644
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc_service.xml
@@ -22,31 +22,31 @@
                 <group name="FIRMWARE">
                     <param name="major">            1           </param>
                     <param name="minor">            0           </param>
-                    <param name="build">            7           </param>
+                    <param name="build">            8          </param>
                 </group>
             </group>
 
             <group name="JOINTMAPPING">
 
                 <group name="ACTUATOR">
-                    <param name="type">             eomc_act_foc        foc                 foc         </param>
+                    <param name="type">             foc                 foc                 foc         </param>
                     <param name="port">             CAN1:1:0            CAN1:2:0            CAN1:3:0    </param>
                 </group>
 
                 <group name="ENCODER1">
-                    <param name="type">             eomc_enc_aea        aea                 aea         </param>
-                    <param name="port">             CONN:J5_X1          CONN:J5_X2          CONN:J5_X3     </param>
-                    <param name="position">         eomc_pos_atjoint    atjoint             atjoint     </param>
+                    <param name="type">             aea                 aea                 aea         </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="tolerance">        0.4                 0.4                 0.4         </param>
                 </group>
 
                 <group name="ENCODER2">
-                    <param name="type">             none                none                none       </param>
-                    <param name="port">             CAN1:1:0            CAN1:2:0            CAN1:3:0   </param>
-                    <param name="position">         atmotor             atmotor             atmotor    </param>
-                    <param name="resolution">        0                   0                   0         </param>
-                    <param name="tolerance">         0                   0                   0         </param>
+                    <param name="type">             none                none                none        </param>
+                    <param name="port">             CAN1:1:0            CAN1:2:0            CAN1:3:0    </param>
+                    <param name="position">         atmotor             atmotor             atmotor     </param>
+                    <param name="resolution">       0                   0                   0           </param>
+                    <param name="tolerance">        0                   0                   0           </param>
                 </group>
 
             </group>

From efdd56d437cb6dd290d4de87cc73b50349281546 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Thu, 17 Nov 2022 11:55:06 +0100
Subject: [PATCH 3/9] ergoCubSN000: fix left wrist files

---
 ergoCubSN000/calibrators/left_arm-calib.xml            |  4 ++--
 .../motorControl/left_arm-eb31-j4_6-mc_service.xml     | 10 +++++-----
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/ergoCubSN000/calibrators/left_arm-calib.xml b/ergoCubSN000/calibrators/left_arm-calib.xml
index 8476108940..3ee9dd9363 100644
--- a/ergoCubSN000/calibrators/left_arm-calib.xml
+++ b/ergoCubSN000/calibrators/left_arm-calib.xml
@@ -16,7 +16,7 @@
 </group>
 <group name="CALIBRATION">
 <param name="calibrationType">        12         12         12         12         12       12      12      12       12       12       12       12       12       12      </param>
-<param name="calibration1">           -41179     7782       11224      25717      6865     29533   30289   0        0        0        0        0        0        0      </param>
+<param name="calibration1">           -41179     7782       11224      25717      10816   7369    21632   0        0        0        0        0        0        0      </param>
 <param name="calibration2">	          0           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        0        0     </param>
 <param name="calibration4">           0           0           0          0           0       0       0       0       0       0        0        0        0        0    </param>
@@ -25,7 +25,7 @@
 <param name="calibrationDelta">       0           0           0          0           0       0       0       0       0       0        0        0        0         0      </param>
 
 <param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0        30.0        30.0    30.0    30.0    30.0    30.0    30.0   </param>
-<param name="startupVelocity">        10.0       10.0         10.0      10.0       30.0    30.0    30.0       40.0        40.0    40.0    40.0    40.0    40.0    40.0  </param>
+<param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0       40.0        40.0    40.0    40.0    40.0    40.0    40.0  </param>
 <param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000      0           0       0       0       0       0       0      </param>
 <param name="startupPosThreshold">    2          2            2          2         2       2       2          10          10      10      10      10      10      10    </param>
 </group>
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
index f9eb6a0968..fe6f640716 100644
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
@@ -22,21 +22,21 @@
                 <group name="FIRMWARE">
                     <param name="major">            1           </param>
                     <param name="minor">            0           </param>
-                    <param name="build">            7           </param>
+                    <param name="build">            8           </param>
                 </group>
             </group>
 
             <group name="JOINTMAPPING">
 
                 <group name="ACTUATOR">
-                    <param name="type">             eomc_act_foc        foc                 foc         </param>
+                    <param name="type">             foc                 foc                 foc         </param>
                     <param name="port">             CAN1:1:0            CAN1:2:0            CAN1:3:0    </param>
                 </group>
 
                 <group name="ENCODER1">
-                    <param name="type">             eomc_enc_aea        aea                 aea         </param>
-                    <param name="port">             CONN:J5_X1          CONN:J5_X2          CONN:J5_X3     </param>
-                    <param name="position">         eomc_pos_atjoint    atjoint             atjoint     </param>
+                    <param name="type">             aea                 aea                 aea         </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="tolerance">        0.4                 0.4                 0.4         </param>
                 </group>

From abf4b3e7e6623aa1e525855e6383a744452d49a9 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Mon, 21 Nov 2022 09:41:58 +0100
Subject: [PATCH 4/9] ergoCubSN000: fix calibration for right writst

---
 ergoCubSN000/calibrators/right_arm-calib.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ergoCubSN000/calibrators/right_arm-calib.xml b/ergoCubSN000/calibrators/right_arm-calib.xml
index 03727af717..c41c79044a 100644
--- a/ergoCubSN000/calibrators/right_arm-calib.xml
+++ b/ergoCubSN000/calibrators/right_arm-calib.xml
@@ -16,7 +16,7 @@
 </group>
 <group name="CALIBRATION">
 <param name="calibrationType">        12         12         12         12         12       12      12      12       12       12       12       12       12       12      </param>
-<param name="calibration1">           -41179     7782       11224      25717      33556   29569   36363   0        0        0        0        0        0        0      </param>
+<param name="calibration1">           -41179     7782       11224      25717      30262   28993   36030   0        0        0        0        0        0        0      </param>
 <param name="calibration2">	          0           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        0        0     </param>
 <param name="calibration4">           0           0           0          0           0       0       0       0       0       0        0        0        0        0    </param>

From 94713db0acd91945d46b5755c5c82d11fbd44943 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Tue, 22 Nov 2022 09:09:15 +0100
Subject: [PATCH 5/9] ergoCubSN000: fix remappers for the arms

---
 ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml  | 4 ++--
 ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
index 53763c6936..f256cb64fb 100644
--- a/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
@@ -12,8 +12,8 @@
     <param name="joints"> 14 </param>
     <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
-            <elem name="left_arm_joints1"> left_arm-eb1-j0_1-mc </elem>
-            <elem name="left_arm_joints2"> left_arm-eb2-j2_3-mc </elem>
+            <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_joints4"> left_arm-eb25-j8_11-mc </elem>
             <elem name="left_arm_joints5"> left_arm-eb26-j12_15-mc </elem>
diff --git a/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
index 9c63d3918f..c9b19f06cc 100644
--- a/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
@@ -12,8 +12,8 @@
     <param name="joints"> 16 </param>
     <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
-            <elem name="right_arm_joints1"> right_arm-eb3-j0_1-mc </elem>
-            <elem name="right_arm_joints2"> right_arm-eb4-j2_3-mc </elem>
+            <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_joints4"> right_arm-eb28-j8_11-mc </elem>
             <elem name="right_arm_joints5"> right_arm-eb29-j12_15-mc </elem>

From e6cf711ca0902c06f8b359e2265e154d89c28816 Mon Sep 17 00:00:00 2001
From: tumme <davide.tome@iit.it>
Date: Tue, 22 Nov 2022 12:12:07 +0100
Subject: [PATCH 6/9] ergoCubSN000 - adding fingers support (4 joints - no FAP)

---
 ergoCubSN000/calibrators/left_arm-calib.xml   | 33 ++++---
 .../calibrators/left_arm_nohand-calib.xml     | 46 ++++++++++
 .../left_upper_arm_no_wrist_no_hand-calib.xml | 47 ++++++++++
 ergoCubSN000/calibrators/right_arm-calib.xml  | 37 ++++----
 .../calibrators/right_arm_nohand-calib.xml    | 47 ++++++++++
 .../calibrators/right_shoulder-calib.xml      |  2 +-
 .../electronics/left_arm-eb23-j7_10-eln.xml   | 40 +++++++++
 .../electronics/right_arm-eb22-j7_10-eln.xml  | 40 +++++++++
 .../mechanicals/left_arm-eb23-j7_10-mec.xml   | 85 +++++++++++++++++++
 .../mechanicals/right_arm-eb22-j7_10-mec.xml  | 85 +++++++++++++++++++
 .../mechanicals/right_leg-eb6-j0_3-mec.xml    |  6 +-
 .../motorControl/left_arm-eb23-j7_10-mc.xml   | 74 ++++++++++++++++
 .../left_arm-eb23-j7_10-mc_service.xml        | 64 ++++++++++++++
 .../left_arm-eb31-j4_6-mc_service.xml         |  2 +-
 .../motorControl/right_arm-eb22-j7_10-mc.xml  | 74 ++++++++++++++++
 .../right_arm-eb22-j7_10-mc_service.xml       | 64 ++++++++++++++
 ergoCubSN000/la.xml                           | 20 +++--
 ergoCubSN000/la_nohands.xml                   | 32 +++++++
 ergoCubSN000/la_upper_no_wrist_no_hand.xml    | 28 ++++++
 ergoCubSN000/ra.xml                           | 33 +++++++
 ergoCubSN000/ra_nohands.xml                   | 32 +++++++
 .../head-inertials_wrapper-deprecated.xml     |  4 +-
 .../motorControl/left_arm-mc_remapper.xml     |  8 +-
 .../left_upper_arm-mc_remapper.xml            | 21 +++++
 .../motorControl/right_arm-mc_remapper.xml    | 22 +++--
 .../motorControl/right_arm-mc_wrapper.xml     |  6 +-
 26 files changed, 880 insertions(+), 72 deletions(-)
 create mode 100644 ergoCubSN000/calibrators/left_arm_nohand-calib.xml
 create mode 100644 ergoCubSN000/calibrators/left_upper_arm_no_wrist_no_hand-calib.xml
 create mode 100644 ergoCubSN000/calibrators/right_arm_nohand-calib.xml
 create mode 100644 ergoCubSN000/hardware/electronics/left_arm-eb23-j7_10-eln.xml
 create mode 100644 ergoCubSN000/hardware/electronics/right_arm-eb22-j7_10-eln.xml
 create mode 100644 ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
 create mode 100644 ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
 create mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
 create mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc_service.xml
 create mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
 create mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc_service.xml
 create mode 100644 ergoCubSN000/la_nohands.xml
 create mode 100644 ergoCubSN000/la_upper_no_wrist_no_hand.xml
 create mode 100644 ergoCubSN000/ra.xml
 create mode 100644 ergoCubSN000/ra_nohands.xml
 create mode 100644 ergoCubSN000/wrappers/motorControl/left_upper_arm-mc_remapper.xml

diff --git a/ergoCubSN000/calibrators/left_arm-calib.xml b/ergoCubSN000/calibrators/left_arm-calib.xml
index 3ee9dd9363..a2d9465949 100644
--- a/ergoCubSN000/calibrators/left_arm-calib.xml
+++ b/ergoCubSN000/calibrators/left_arm-calib.xml
@@ -6,31 +6,30 @@
                 <xi:include href="../general.xml" />
 
 		<group name="GENERAL">
-		    <param name="joints">14</param> <!-- the number of joints of the robot part -->
+		    <param name="joints">11</param> <!-- the number of joints of the robot part -->
 		    <param name="deviceName"> Left_Arm_Calibrator </param>
 		</group>
 
 <group name="HOME">
-<param name="positionHome">            5         30         0          10         0        0       0       30.00    30.00    30.00    30.00    30.00    30.00    30.00  </param>
-<param name="velocityHome">           10         10         10         10         10       10      10      40.00    40.00    40.00    40.00    40.00    40.00    40.00  </param>
+<param name="positionHome">            5         30         0          10         0        0       0        30.00      30.00   30.00   30.00  </param>
+<param name="velocityHome">           10         10         10         10         10       10      10       40.00      40.00   40.00   40.00  </param>
 </group>
 <group name="CALIBRATION">
-<param name="calibrationType">        12         12         12         12         12       12      12      12       12       12       12       12       12       12      </param>
-<param name="calibration1">           -41179     7782       11224      25717      10816   7369    21632   0        0        0        0        0        0        0      </param>
-<param name="calibration2">	          0           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        0        0     </param>
-<param name="calibration4">           0           0           0          0           0       0       0       0       0       0        0        0        0        0    </param>
-<param name="calibration5">           0           0           0          0           0       0       0       0       0       0        0        0        0          0     </param>
-<param name="calibrationZero">        0           0           0          0           0       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         0      </param>
-
-<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0        30.0        30.0    30.0    30.0    30.0    30.0    30.0   </param>
-<param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0       40.0        40.0    40.0    40.0    40.0    40.0    40.0  </param>
-<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000      0           0       0       0       0       0       0      </param>
-<param name="startupPosThreshold">    2          2            2          2         2       2       2          10          10      10      10      10      10      10    </param>
+<param name="calibrationType">        12         12         12         12         12       12      12        10          10      10      10       </param>
+<param name="calibration1">           -41179     7782       11224      25717      10816   7369    21632     -850        -850    -850    -850      </param>
+<param name="calibration2">	          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        </param>
+<param name="calibration4">           0           0           0          0           0       0       0       0           0       0       0        </param>
+<param name="calibration5">           0           0           0          0           0       0       0       0           0       0       0        </param>
+<param name="calibrationZero">        0           0           0          0           0       0       0       85          85      85      85       </param>
+<param name="calibrationDelta">       0           0           0          0           0       0       0       0           0       0       0        </param>
+<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0       30.0        30.0    30.0    30.0 </param>
+<param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0      70.0        70.0    70.0    70.0 </param>
+<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000     0           0       0       0    </param>
+<param name="startupPosThreshold">    2          2            2          2         2       2       2         10          10      10      10   </param>
 </group>
 
-<param name="CALIB_ORDER">  (1) (0) (2) (3) (5 6 7) (8 9 10 11 12 13)</param>
+<param name="CALIB_ORDER">  (1) (0) (2) (3) (4 5 6) (7 8 9 10)</param>
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">left_arm-mc_remapper</param>
 		</action>
diff --git a/ergoCubSN000/calibrators/left_arm_nohand-calib.xml b/ergoCubSN000/calibrators/left_arm_nohand-calib.xml
new file mode 100644
index 0000000000..c6f1e30bc8
--- /dev/null
+++ b/ergoCubSN000/calibrators/left_arm_nohand-calib.xml
@@ -0,0 +1,46 @@
+<?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="left_arm-calibrator" type="parametricCalibratorEth">
+                <xi:include href="../general.xml" />
+
+		<group name="GENERAL">
+		    <param name="joints">11]</param> <!-- the number of joints of the robot part -->
+		    <param name="deviceName"> Left_Arm_Calibrator </param>
+		</group>
+
+<group name="HOME">
+<param name="positionHome">            5         30         0          10         0        0       0        30.00      30.00   30.00   30.00  </param>
+<param name="velocityHome">           10         10         10         10         10       10      10       40.00      40.00   40.00   40.00  </param>
+</group>
+<group name="CALIBRATION">
+<param name="calibrationType">        12         12         12         12         12       12      12        10          10      10      10       </param>
+<param name="calibration1">           -41179     7782       11224      25717      10816   7369    21632     -850        -850    -850    -850      </param>
+<param name="calibration2">	          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        </param>
+<param name="calibration4">           0           0           0          0           0       0       0       0           0       0       0        </param>
+<param name="calibration5">           0           0           0          0           0       0       0       0           0       0       0        </param>
+<param name="calibrationZero">        0           0           0          0           0       0       0       85          85      85      85       </param>
+<param name="calibrationDelta">       0           0           0          0           0       0       0       0           0       0       0        </param>
+<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0       30.0        30.0    30.0    30.0 </param>
+<param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0      70.0        70.0    70.0    70.0 </param>
+<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000     0           0       0       0    </param>
+<param name="startupPosThreshold">    2          2            2          2         2       2       2         10          10      10      10   </param>
+</group>
+
+<param name="CALIB_ORDER">  (1) (0) (2) (3) (4 5 6) (7 8 9 10)</param>
+		<action phase="startup" level="10" type="calibrate">
+		    <param name="target">left_arm-mc_remapper</param>
+		</action>
+
+		<action phase="interrupt1" level="1" type="park">
+		    <param name="target">left_arm-mc_remapper</param>
+		</action>
+
+		<action phase="interrupt3" level="1" type="abort" />
+
+	</device>
+
+
+
diff --git a/ergoCubSN000/calibrators/left_upper_arm_no_wrist_no_hand-calib.xml b/ergoCubSN000/calibrators/left_upper_arm_no_wrist_no_hand-calib.xml
new file mode 100644
index 0000000000..6a57b8ab6b
--- /dev/null
+++ b/ergoCubSN000/calibrators/left_upper_arm_no_wrist_no_hand-calib.xml
@@ -0,0 +1,47 @@
+<?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="left_arm-calibrator" type="parametricCalibratorEth">
+                <xi:include href="../general.xml" />
+
+		<group name="GENERAL">
+		    <param name="joints">4</param> <!-- the number of joints of the robot part -->
+		    <param name="deviceName"> Left_Arm_Calibrator </param>
+		</group>
+
+<group name="HOME">
+<param name="positionHome">            5         30         0          10     </param>
+<param name="velocityHome">           10         10         10         10      </param>
+</group>
+<group name="CALIBRATION">
+<param name="calibrationType">        12         12         12         12     </param>
+<param name="calibration1">           -41179     7782       11224      25717    </param>
+<param name="calibration2">	      0           0           0          0      </param>
+<param name="calibration3">           0           0           0          0    </param>
+<param name="calibration4">           0           0           0          0    </param>
+<param name="calibration5">           0           0           0          0               </param>
+<param name="calibrationZero">        0           0           0          0               </param>
+<param name="calibrationDelta">       0           0           0          0        </param>
+
+<param name="startupPosition">        -35        30           0         45           </param>
+<param name="startupVelocity">        10.0       10.0         10.0      10.0        </param>
+<param name="startupMaxPwm">          8000       8000         8000      8000     </param>
+<param name="startupPosThreshold">    2          2            2          2            </param>
+</group>
+
+<param name="CALIB_ORDER">  (1) (0) (2) (3)</param>
+		<action phase="startup" level="10" type="calibrate">
+		    <param name="target">left_arm-mc_remapper</param>
+		</action>
+
+		<action phase="interrupt1" level="1" type="park">
+		    <param name="target">left_arm-mc_remapper</param>
+		</action>
+
+		<action phase="interrupt3" level="1" type="abort" />
+
+	</device>
+
+
+
diff --git a/ergoCubSN000/calibrators/right_arm-calib.xml b/ergoCubSN000/calibrators/right_arm-calib.xml
index c41c79044a..89ac5c997c 100644
--- a/ergoCubSN000/calibrators/right_arm-calib.xml
+++ b/ergoCubSN000/calibrators/right_arm-calib.xml
@@ -2,35 +2,34 @@
 <!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-calibrator" type="parametricCalibratorEth">
+	<device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_arm-calibrator" type="parametricCalibratorEth">
                 <xi:include href="../general.xml" />
 
 		<group name="GENERAL">
-		    <param name="joints">14</param> <!-- the number of joints of the robot part -->
-		    <param name="deviceName"> Right_Arm_Calibrator </param>
+		    <param name="joints">11</param> <!-- the number of joints of the robot part -->
+		    <param name="deviceName"> Left_Arm_Calibrator </param>
 		</group>
 
 <group name="HOME">
-<param name="positionHome">            5         30         0          10         0        0       0       30.00    30.00    30.00    30.00    30.00    30.00    30.00  </param>
-<param name="velocityHome">           10         10         10         10         10       10      10      40.00    40.00    40.00    40.00    40.00    40.00    40.00  </param>
+<param name="positionHome">            5         30         0          10         0        0       0     30.00      30.00   30.00   30.00   </param>
+<param name="velocityHome">           10         10         10         10         10       10      10    40.00      40.00   40.00   40.00   </param>
 </group>
 <group name="CALIBRATION">
-<param name="calibrationType">        12         12         12         12         12       12      12      12       12       12       12       12       12       12      </param>
-<param name="calibration1">           -41179     7782       11224      25717      30262   28993   36030   0        0        0        0        0        0        0      </param>
-<param name="calibration2">	          0           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        0        0     </param>
-<param name="calibration4">           0           0           0          0           0       0       0       0       0       0        0        0        0        0    </param>
-<param name="calibration5">           0           0           0          0           0       0       0       0       0       0        0        0        0          0     </param>
-<param name="calibrationZero">        0           0           0          0           0       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         0      </param>
-
-<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0        30.0        30.0    30.0    30.0    30.0    30.0    30.0   </param>
-<param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0       40.0        40.0    40.0    40.0    40.0    40.0    40.0  </param>
-<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000      0           0       0       0       0       0       0      </param>
-<param name="startupPosThreshold">    2          2            2          2         2       2       2          10          10      10      10      10      10      10    </param>
+<param name="calibrationType">        12         12         12         12         12       12      12       10          10      10      10         </param>
+<param name="calibration1">           40139     -4670     -60282      -59034      30262   28993   36030    -850        -850    -850    -850        </param>
+<param name="calibration2">	      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          </param>
+<param name="calibration4">           0           0           0          0           0       0       0      0           0       0       0           </param>
+<param name="calibration5">           0           0           0          0           0       0       0      0           0       0       0           </param>
+<param name="calibrationZero">        0           0           0          0           0       0       0      85          85      85      85          </param>
+<param name="calibrationDelta">       0           0           0          0           0       0       0      0           0       0       0     </param>
+<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0      30.0        30.0    30.0    30.0    </param>
+<param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0     70.0        70.0    70.0    70.0    </param>
+<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000    0           0       0       0     </param>
+<param name="startupPosThreshold">    2          2            2          2         2       2       2        10          10      10      10      </param>
 </group>
 
-<param name="CALIB_ORDER">  (1) (0) (2) (3) (5 6 7) (8 9 10 11 12 13)</param>
+<param name="CALIB_ORDER">  (1) (0) (2) (3) (4 5 6) (7 8 9 10)</param>
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">right_arm-mc_remapper</param>
 		</action>
diff --git a/ergoCubSN000/calibrators/right_arm_nohand-calib.xml b/ergoCubSN000/calibrators/right_arm_nohand-calib.xml
new file mode 100644
index 0000000000..62553a84f1
--- /dev/null
+++ b/ergoCubSN000/calibrators/right_arm_nohand-calib.xml
@@ -0,0 +1,47 @@
+<?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="left_arm-calibrator" type="parametricCalibratorEth">
+                <xi:include href="../general.xml" />
+
+		<group name="GENERAL">
+		    <param name="joints">7</param> <!-- the number of joints of the robot part -->
+		    <param name="deviceName"> Left_Arm_Calibrator </param>
+		</group>
+
+<group name="HOME">
+<param name="positionHome">            5         30         0          10         0        0       0         </param>
+<param name="velocityHome">           10         10         10         10         10       10      10        </param>
+</group>
+<group name="CALIBRATION">
+<param name="calibrationType">        12         12         12         12         12       12      12           </param>
+<param name="calibration1">           40139     -4670     -60282      -59034      30262   28993   36030         </param>
+<param name="calibration2">	      0           0           0          0           0       0       0          </param>
+<param name="calibration3">           0           0           0          0           0       0       0          </param>
+<param name="calibration4">           0           0           0          0           0       0       0           </param>
+<param name="calibration5">           0           0           0          0           0       0       0           </param>
+<param name="calibrationZero">        0           0           0          0           0       0       0           </param>
+<param name="calibrationDelta">       0           0           0          0           0       0       0     </param>
+
+<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0       </param>
+<param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0      </param>
+<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000   </param>
+<param name="startupPosThreshold">    2          2            2          2         2       2       2         </param>
+</group>
+
+<param name="CALIB_ORDER">  (1) (0) (2) (3) (4 5 6)</param>
+		<action phase="startup" level="10" type="calibrate">
+		    <param name="target">right_arm-mc_remapper</param>
+		</action>
+
+		<action phase="interrupt1" level="1" type="park">
+		    <param name="target">right_arm-mc_remapper</param>
+		</action>
+
+		<action phase="interrupt3" level="1" type="abort" />
+
+	</device>
+
+
+
diff --git a/ergoCubSN000/calibrators/right_shoulder-calib.xml b/ergoCubSN000/calibrators/right_shoulder-calib.xml
index 964551adcf..7d071cc644 100644
--- a/ergoCubSN000/calibrators/right_shoulder-calib.xml
+++ b/ergoCubSN000/calibrators/right_shoulder-calib.xml
@@ -19,7 +19,7 @@
 <group name="CALIBRATION">
 <param name="calibrationType">     12          12          12         12        </param> <!--
 <param name="calibration1">        6576       -12866      -4585      -47529     </param> -->
-<param name="calibration1">        40139     -2945     -60282      -59034   </param>
+<param name="calibration1">        40139     -4670     -60282      -59034   </param>
 <param name="calibration2">	       0           0           0          0	         </param>
 <param name="calibration3">        0           0           0          0        </param>
 <param name="calibration4">        0           0           0          0         </param>
diff --git a/ergoCubSN000/hardware/electronics/left_arm-eb23-j7_10-eln.xml b/ergoCubSN000/hardware/electronics/left_arm-eb23-j7_10-eln.xml
new file mode 100644
index 0000000000..b2b7437415
--- /dev/null
+++ b/ergoCubSN000/hardware/electronics/left_arm-eb23-j7_10-eln.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_forearm" build="1">
+
+    <xi:include href="./pc104.xml" />
+
+    <group name="ETH_BOARD">
+
+        <group name="ETH_BOARD_PROPERTIES">
+            <param name="IpAddress">                10.0.1.23                </param>
+            <param name="IpPort">                   12345                   </param>
+            <param name="Type">                     mc4plus                    </param>
+            <param name="maxSizeRXpacket">          768                     </param>
+            <param name="maxSizeROP">               384                     </param>
+        </group>
+
+        <group name="ETH_BOARD_SETTINGS">
+            <param name="Name">                     "left_arm-eb23-j7_10"        </param>
+            <group name="RUNNINGMODE">
+                <param name="period">                   1000                </param>
+                <param name="maxTimeOfRXactivity">      400                 </param>
+                <param name="maxTimeOfDOactivity">      300                 </param>
+                <param name="maxTimeOfTXactivity">      300                 </param>
+                <param name="TXrateOfRegularROPs">      5                   </param>
+            </group>
+        </group>
+
+        <group name="ETH_BOARD_ACTIONS">
+            <group name="MONITOR_ITS_PRESENCE">
+                <param name="enabled">                  true                </param>
+                <param name="timeout">                  0.020               </param>
+                <param name="periodOfMissingReport">    60.0                </param>
+            </group>
+        </group>
+
+    </group>
+
+</params>
+
diff --git a/ergoCubSN000/hardware/electronics/right_arm-eb22-j7_10-eln.xml b/ergoCubSN000/hardware/electronics/right_arm-eb22-j7_10-eln.xml
new file mode 100644
index 0000000000..408bb3ea1c
--- /dev/null
+++ b/ergoCubSN000/hardware/electronics/right_arm-eb22-j7_10-eln.xml
@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_forearm" build="1">
+
+    <xi:include href="./pc104.xml" />
+
+    <group name="ETH_BOARD">
+
+        <group name="ETH_BOARD_PROPERTIES">
+            <param name="IpAddress">                10.0.1.22                </param>
+            <param name="IpPort">                   12345                   </param>
+            <param name="Type">                     mc4plus                    </param>
+            <param name="maxSizeRXpacket">          768                     </param>
+            <param name="maxSizeROP">               384                     </param>
+        </group>
+
+        <group name="ETH_BOARD_SETTINGS">
+            <param name="Name">                     "right_arm-eb22-j7_10"        </param>
+            <group name="RUNNINGMODE">
+                <param name="period">                   1000                </param>
+                <param name="maxTimeOfRXactivity">      400                 </param>
+                <param name="maxTimeOfDOactivity">      300                 </param>
+                <param name="maxTimeOfTXactivity">      300                 </param>
+                <param name="TXrateOfRegularROPs">      5                   </param>
+            </group>
+        </group>
+
+        <group name="ETH_BOARD_ACTIONS">
+            <group name="MONITOR_ITS_PRESENCE">
+                <param name="enabled">                  true                </param>
+                <param name="timeout">                  0.020               </param>
+                <param name="periodOfMissingReport">    60.0                </param>
+            </group>
+        </group>
+
+    </group>
+
+</params>
+
diff --git a/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml b/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
new file mode 100644
index 0000000000..9081f15352
--- /dev/null
+++ b/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
@@ -0,0 +1,85 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_forarm" build="1">
+    <group name="GENERAL">
+        <param name="MotioncontrolVersion"> 6 </param>
+        <param name="Joints"> 4 </param> 
+        
+        <!-- joint number in sub-part       0                       1                       2                           3                       -->
+        <!-- joint name                                                                                                                         -->         
+        <param name="AxisMap">              0                       1                       2                           3                       </param>
+        <param name="AxisName">             "l_hand_thumb_proximal" "l_hand_index_proximal" "l_hand_medium_proximal"    "l_hand_pinky_proximal" </param>
+        <param name="AxisType">             "revolute"              "revolute"              "revolute"                  "revolute"              </param>
+        <param name="Encoder">              182.044                 36400.044                 36400.044                     182.044                 </param>
+        <param name="fullscalePWM">         3360                    3360                    3360                        3360                    </param>
+        <param name="ampsToSensor">         1000.0                  1000.0                  1000.0                      1000.0                  </param>
+        <param name="Gearbox_M2J">          159                     1                       1                         159                     </param>
+        <param name="Gearbox_E2J">          1                       1                       1                           1                       </param>
+        <param name="useMotorSpeedFbk">     0                       0                       0                           0                       </param>
+        <param name="MotorType">            "DC"                    "DC"                    "DC"                        "DC"                    </param>
+                
+        <param name="Verbose"> 0 </param>
+    </group>
+
+    <group name="LIMITS">
+        <param name="hardwareJntPosMax">    100             100             100             100         </param>
+        <param name="hardwareJntPosMin">    0               0               0               0           </param>
+        <param name="rotorPosMin">          0               0               0               0           </param>
+        <param name="rotorPosMax">          0               0               0               0           </param>
+    </group>
+    
+    <group name="COUPLINGS"> 
+
+        <param name ="matrixJ2M"> 
+            1.000   0.000   0.000   0.000
+            0.000   1.000   0.000   0.000
+            0.000   0.000   1.000   0.000
+            0.000   0.000   0.000   1.000   
+        </param>
+               
+        <param name ="matrixM2J"> 
+            1.000   0.000   0.000   0.000
+            0.000   1.000   0.000   0.000
+            0.000   0.000   1.000   0.000
+            0.000   0.000   0.000   1.000   
+        </param>
+       
+        <param name ="matrixE2J">  
+            1.000   0.000   0.000   0.000   0.000   0.000
+            0.000   1.000   0.000   0.000   0.000   0.000
+            0.000   0.000   1.000   0.000   0.000   0.000
+            0.000   0.000   0.000   1.000   0.000   0.000 
+        </param>
+                
+    </group>   
+
+    <group name="JOINTSET_CFG"> 
+        <param name= "numberofsets"> 4 </param>
+        <group name="JOINTSET_0">
+            <param name="listofjoints">  0             </param>
+            <param name="constraint">    none          </param>
+            <param name="param1">        0             </param>
+            <param name="param2">        0             </param>
+        </group> 
+        <group name="JOINTSET_1">
+            <param name="listofjoints">  1             </param>
+            <param name="constraint">    none          </param>
+            <param name="param1">        0             </param>
+            <param name="param2">        0             </param>
+        </group>
+        <group name="JOINTSET_2">
+            <param name="listofjoints">  2             </param>
+            <param name="constraint">    none          </param>
+            <param name="param1">        0             </param>
+            <param name="param2">        0             </param>
+        </group>
+        <group name="JOINTSET_3">
+            <param name="listofjoints">  3             </param>
+            <param name="constraint">    none          </param>
+            <param name="param1">        0             </param>
+            <param name="param2">        0             </param>
+        </group>        
+    </group>                                       
+    
+</params>
diff --git a/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml b/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
new file mode 100644
index 0000000000..d3d3da4096
--- /dev/null
+++ b/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
@@ -0,0 +1,85 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_forarm" build="1">
+    <group name="GENERAL">
+        <param name="MotioncontrolVersion"> 6 </param>
+        <param name="Joints"> 4 </param> 
+        
+        <!-- joint number in sub-part       0                       1                       2                           3                       -->
+        <!-- joint name                                                                                                                         -->         
+        <param name="AxisMap">              0                       1                       2                           3                       </param>
+        <param name="AxisName">             "r_hand_thumb_proximal" "r_hand_index_proximal" "r_hand_medium_proximal"    "r_hand_pinky_proximal" </param>
+        <param name="AxisType">             "revolute"              "revolute"              "revolute"                  "revolute"              </param>
+        <param name="Encoder">              182.044                 36400.044                 36400.044                     182.044                 </param>
+        <param name="fullscalePWM">         3360                    3360                    3360                        3360                    </param>
+        <param name="ampsToSensor">         1000.0                  1000.0                  1000.0                      1000.0                  </param>
+        <param name="Gearbox_M2J">          159                     1                       1                         159                     </param>
+        <param name="Gearbox_E2J">          1                       1                       1                           1                       </param>
+        <param name="useMotorSpeedFbk">     0                       0                       0                           0                       </param>
+        <param name="MotorType">            "DC"                    "DC"                    "DC"                        "DC"                    </param>
+                
+        <param name="Verbose"> 0 </param>
+    </group>
+
+    <group name="LIMITS">
+        <param name="hardwareJntPosMax">    100             100             100             100         </param>
+        <param name="hardwareJntPosMin">    0              -10             -10              0           </param>
+        <param name="rotorPosMin">          0               0               0               0           </param>
+        <param name="rotorPosMax">          0               0               0               0           </param>
+    </group>
+    
+    <group name="COUPLINGS"> 
+
+        <param name ="matrixJ2M"> 
+            1.000   0.000   0.000   0.000
+            0.000   1.000   0.000   0.000
+            0.000   0.000   1.000   0.000
+            0.000   0.000   0.000   1.000   
+        </param>
+               
+        <param name ="matrixM2J"> 
+            1.000   0.000   0.000   0.000
+            0.000   1.000   0.000   0.000
+            0.000   0.000   1.000   0.000
+            0.000   0.000   0.000   1.000   
+        </param>
+       
+        <param name ="matrixE2J">  
+            1.000   0.000   0.000   0.000   0.000   0.000
+            0.000   1.000   0.000   0.000   0.000   0.000
+            0.000   0.000   1.000   0.000   0.000   0.000
+            0.000   0.000   0.000   1.000   0.000   0.000 
+        </param>
+                
+    </group>   
+
+    <group name="JOINTSET_CFG"> 
+        <param name= "numberofsets"> 4 </param>
+        <group name="JOINTSET_0">
+            <param name="listofjoints">  0             </param>
+            <param name="constraint">    none          </param>
+            <param name="param1">        0             </param>
+            <param name="param2">        0             </param>
+        </group> 
+        <group name="JOINTSET_1">
+            <param name="listofjoints">  1             </param>
+            <param name="constraint">    none          </param>
+            <param name="param1">        0             </param>
+            <param name="param2">        0             </param>
+        </group>
+        <group name="JOINTSET_2">
+            <param name="listofjoints">  2             </param>
+            <param name="constraint">    none          </param>
+            <param name="param1">        0             </param>
+            <param name="param2">        0             </param>
+        </group>
+        <group name="JOINTSET_3">
+            <param name="listofjoints">  3             </param>
+            <param name="constraint">    none          </param>
+            <param name="param1">        0             </param>
+            <param name="param2">        0             </param>
+        </group>        
+    </group>                                       
+    
+</params>
diff --git a/ergoCubSN000/hardware/mechanicals/right_leg-eb6-j0_3-mec.xml b/ergoCubSN000/hardware/mechanicals/right_leg-eb6-j0_3-mec.xml
index 39632163df..f28d4a970d 100644
--- a/ergoCubSN000/hardware/mechanicals/right_leg-eb6-j0_3-mec.xml
+++ b/ergoCubSN000/hardware/mechanicals/right_leg-eb6-j0_3-mec.xml
@@ -36,9 +36,9 @@
         <param name="HasRotorEncoder">       1             1        1   1  </param>
         <param name="HasRotorEncoderIndex">  1             1        1   1  </param>
         <param name="HasSpeedEncoder">       0             0        0   0  </param>
-        <param name="RotorIndexOffset">      203           295      49   184   </param>
+        <param name="RotorIndexOffset">      203           286      48  184   </param>
         <param name="MotorPoles">            12            8        8   12  </param>
- </group>
+   </group>
 
     <group name="COUPLINGS"> 
                                 
@@ -92,4 +92,4 @@
             <param name="param2">         0             </param>
         </group> 
     </group>                          
-    </params>
+</params>
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
new file mode 100644
index 0000000000..949dc844fe
--- /dev/null
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
@@ -0,0 +1,74 @@
+<?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="left_arm-eb23-j7_10-mc" type="embObjMotionControl">
+
+    <xi:include href="../../general.xml" />
+    <xi:include href="../electronics/left_arm-eb23-j7_10-eln.xml" />
+    <xi:include href="../mechanicals/left_arm-eb23-j7_10-mec.xml" />
+    <xi:include href="../motorControl/left_arm-eb23-j7_10-mc_service.xml" />
+
+    <!-- joint number                           0                   1                   2                   3                   -->
+    <!-- joint name                             thumb               index               medium              pinky               -->    
+    <group name="LIMITS">
+        <param name="jntPosMin">                +5                  +5                  +5                  +5                  </param>
+        <param name="jntPosMax">                +85                 +85                 +85                 +85                 </param>
+        <param name="jntVelMax">                1000                1000                1000                1000                </param>
+        <param name="motorOverloadCurrents">    2000                2000                2500                2500                </param>
+        <param name="motorNominalCurrents">     700                 700                 700                 700                 </param>
+        <param name="motorPeakCurrents">        1500                1500                1500                1500                </param>    
+        <param name="motorPwmLimit">            3360                3360                3360                3360                </param>
+    </group>
+   
+    <group name="TIMEOUTS">
+        <param name="velocity">                 100                 100                 100                 100                 </param>
+    </group>
+
+    <group name="IMPEDANCE">
+        <param name="stiffness">                0.0                 0.0                 0.0                 0.0                 </param>
+        <param name="damping">                  0.0                 0.0                 0.0                 0.0                 </param>
+    </group>
+    
+    <group name="CONTROLS">
+        <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="torqueControl">            none                none                none                none                </param>
+        <param name="currentPid">               none                none                none                none                </param>
+        <param name="speedPid">                 none                none                none                none                </param>
+    </group>
+
+
+    <!-- default position PID: begin -->
+    
+    <group name="POS_PID_DEFAULT">
+        <param name="controlLaw">               minjerk         </param>
+        <param name="outputType">               pwm             </param>
+        <param name="fbkControlUnits">          metric_units    </param>
+        <param name="outputControlUnits">       machine_units   </param>
+        <param name="kp">                       -150        +300        -300        -200    </param>
+        <param name="kd">                       0           0           0           0       </param>
+        <param name="ki">                       -100        +30         -30         -30     </param>
+        <param name="maxOutput">                3360        3360        3360        3360    </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>
+        <param name="kff">                      0           0           0           0       </param>
+    </group>
+
+    <!-- default position PID: end -->
+
+
+    <!-- other default PIDs: begin -->   
+    <!-- other default PIDs: end -->
+
+
+    <!-- custom PIDs: begin -->    
+    <!-- custom PIDs: end -->
+    
+</device>
+
+
+        
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc_service.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc_service.xml
new file mode 100644
index 0000000000..adf3fa34a3
--- /dev/null
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc_service.xml
@@ -0,0 +1,64 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_forearm" build="1">
+
+    <group name="SERVICE">
+        
+        <param name="type"> eomn_serv_MC_mc4plus </param> 
+    
+        <group name="PROPERTIES">
+        
+            <group name="ETHBOARD">
+                <param name="type">                 mc4plus             </param> 
+            </group>          
+<!--
+            <group name="CANBOARDS"> 
+                <param name="type">                 pmc                 </param> 
+                <group name="PROTOCOL">                    
+                    <param name="major">            0                   </param>    
+                    <param name="minor">            0                   </param>     
+                </group>                                   
+                <group name="FIRMWARE">                    
+                    <param name="major">            0                   </param>    
+                    <param name="minor">            0                   </param> 
+                    <param name="build">            0                   </param>
+                </group>
+            </group>
+            
+            <group name="POS"> 
+                <param name="location">             CAN1:1              </param> 
+            </group>                      
+-->            
+
+            <group name="JOINTMAPPING">
+
+                <group name="ACTUATOR">                 
+                    <param name="type">             pwm                 pwm                 pwm                 pwm                     </param>
+                    <param name="port">             CONN:P5             CONN:P4             CONN:P3             CONN:P2                 </param>
+                </group>
+                
+                <group name="ENCODER1">  
+                    <param name="type">             none                none                none                none             </param>  
+                    <param name="port">             CONN:none           CONN:none           CONN:none           CONN:none        </param>
+                    <param name="position">         none                none                none                none             </param> 
+                    <param name="resolution">       1                   1                   1                   1                </param>
+                    <param name="tolerance">        0                   0                   0                   0                </param>  
+                </group>        
+                
+                <group name="ENCODER2">
+                    <param name="type">             qenc                qenc                qenc                qenc                    </param>
+                    <param name="port">             CONN:P5             CONN:P4             CONN:P3             CONN:P2                 </param>
+                    <param name="position">         atmotor             atmotor             atmotor             atmotor                 </param>
+                    <param name="resolution">       -1600               -1600                -1600               -1600                    </param>
+                    <param name="tolerance">        0                   0                   0                   0                       </param>  
+                </group>  
+            </group>                        
+           
+        </group>
+           
+    </group>
+    
+  
+</params>
+
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
index fe6f640716..6009814603 100644
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc_service.xml
@@ -55,4 +55,4 @@
 
     </group>
 
-</params>
\ No newline at end of file
+</params>
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
new file mode 100644
index 0000000000..b368510b31
--- /dev/null
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
@@ -0,0 +1,74 @@
+<?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="rightt_arm-eb22-j7_10-mc" type="embObjMotionControl">
+
+    <xi:include href="../../general.xml" />
+    <xi:include href="../electronics/rightt_arm-eb22-j7_10-eln.xml" />
+    <xi:include href="../mechanicals/rightt_arm-eb22-j7_10-mec.xml" />
+    <xi:include href="../motorControl/rightt_arm-eb22-j7_10-mc_service.xml" />
+
+    <!-- joint number                           0                   1                   2                   3                   -->
+    <!-- joint name                             thumb               index               medium              pinky               -->    
+    <group name="LIMITS">
+        <param name="jntPosMin">               +5                  +5                  +5                  +5                  </param>
+        <param name="jntPosMax">               +85                 +85                 +85                 +85                 </param>
+        <param name="jntVelMax">                1000                1000                1000                1000                </param>
+        <param name="motorOverloadCurrents">    2000                2000                2500                2500                </param>
+        <param name="motorNominalCurrents">     700                 700                 700                 700                 </param>
+        <param name="motorPeakCurrents">        1500                1500                1500                1500                </param>    
+        <param name="motorPwmLimit">            3360                3360                3360                3360                </param>
+    </group>
+   
+    <group name="TIMEOUTS">
+        <param name="velocity">                 100                 100                 100                 100                 </param>
+    </group>
+
+    <group name="IMPEDANCE">
+        <param name="stiffness">                0.0                 0.0                 0.0                 0.0                 </param>
+        <param name="damping">                  0.0                 0.0                 0.0                 0.0                 </param>
+    </group>
+    
+    <group name="CONTROLS">
+        <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
+        <param name="torqueControl">            none                none                none                none                </param>
+        <param name="currentPid">               none                none                none                none                </param>
+        <param name="speedPid">                 none                none                none                none                </param>
+    </group>
+
+
+    <!-- default position PID: begin -->
+    
+    <group name="POS_PID_DEFAULT">
+        <param name="controlLaw">               minjerk         </param>
+        <param name="outputType">               pwm             </param>
+        <param name="fbkControlUnits">          metric_units    </param>
+        <param name="outputControlUnits">       machine_units   </param>
+        <param name="kp">                       -150        -300        -300        -200    </param>
+        <param name="kd">                       0           0           0           0       </param>
+        <param name="ki">                       -100        -30         -30         -30     </param>
+        <param name="maxOutput">                3360        3360        3360        3360    </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>
+        <param name="kff">                      0           0           0           0       </param>
+    </group>
+
+    <!-- default position PID: end -->
+
+
+    <!-- other default PIDs: begin -->   
+    <!-- other default PIDs: end -->
+
+
+    <!-- custom PIDs: begin -->    
+    <!-- custom PIDs: end -->
+    
+</device>
+
+
+        
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc_service.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc_service.xml
new file mode 100644
index 0000000000..909b72c5ae
--- /dev/null
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc_service.xml
@@ -0,0 +1,64 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_forearm" build="1">
+
+    <group name="SERVICE">
+        
+        <param name="type"> eomn_serv_MC_mc4plus </param> 
+    
+        <group name="PROPERTIES">
+        
+            <group name="ETHBOARD">
+                <param name="type">                 mc4plus             </param> 
+            </group>          
+<!--
+            <group name="CANBOARDS"> 
+                <param name="type">                 pmc                 </param> 
+                <group name="PROTOCOL">                    
+                    <param name="major">            0                   </param>    
+                    <param name="minor">            0                   </param>     
+                </group>                                   
+                <group name="FIRMWARE">                    
+                    <param name="major">            0                   </param>    
+                    <param name="minor">            0                   </param> 
+                    <param name="build">            0                   </param>
+                </group>
+            </group>
+            
+            <group name="POS"> 
+                <param name="location">             CAN1:1              </param> 
+            </group>                      
+-->            
+
+            <group name="JOINTMAPPING">
+
+                <group name="ACTUATOR">                 
+                    <param name="type">             pwm                 pwm                 pwm                 pwm              </param>
+                    <param name="port">             CONN:P5             CONN:P4             CONN:P3             CONN:P2          </param>
+                </group>
+                
+                <group name="ENCODER1">  
+                    <param name="type">             none                none                none                none             </param>  
+                    <param name="port">             CONN:none           CONN:none           CONN:none           CONN:none        </param>
+                    <param name="position">         none                none                none                none             </param> 
+                    <param name="resolution">       1                   1                   1                   1                </param>
+                    <param name="tolerance">        0                   0                   0                   0                </param>  
+                </group>        
+                
+                <group name="ENCODER2">
+                    <param name="type">             qenc                qenc                qenc                qenc             </param>
+                    <param name="port">             CONN:P5             CONN:P4             CONN:P3             CONN:P2          </param>
+                    <param name="position">         atmotor             atmotor             atmotor             atmotor          </param>
+                    <param name="resolution">       -1600               -1600                -1600               -1600           </param>
+                    <param name="tolerance">        0                   0                   0                   0                </param>  
+                </group>  
+            </group>                        
+           
+        </group>
+           
+    </group>
+    
+  
+</params>
+
diff --git a/ergoCubSN000/la.xml b/ergoCubSN000/la.xml
index 7780cdad98..40e63d5b78 100644
--- a/ergoCubSN000/la.xml
+++ b/ergoCubSN000/la.xml
@@ -8,19 +8,21 @@
     </params>
 
     <devices>
- 
+
     <!-- motor controllers wrappers -->
-    <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.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/left_arm-mc_remapper.xml" /> 
 
-  
       <!-- LEFT ARM -->
-    <xi:include href="hardware/motorControl/left_arm-eb1-j0_1-mc.xml" />
-    <xi:include href="hardware/motorControl/left_arm-eb3-j2_3-mc.xml" />
- 
-  
+    <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" />
+
+
 
 
     <!--  CALIBRATORS -->
diff --git a/ergoCubSN000/la_nohands.xml b/ergoCubSN000/la_nohands.xml
new file mode 100644
index 0000000000..2cb45855fb
--- /dev/null
+++ b/ergoCubSN000/la_nohands.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN000" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <params>
+    <xi:include href="hardware/electronics/pc104.xml" />
+    </params>
+
+    <devices>
+
+    <!-- motor controllers wrappers -->
+    <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.xml" />
+
+
+    <xi:include href="wrappers/motorControl/left_arm-mc_remapper.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" />
+
+
+
+
+    <!--  CALIBRATORS -->
+    <xi:include href="calibrators/left_arm_nohand-calib.xml" />
+
+
+  </devices>
+</robot>
diff --git a/ergoCubSN000/la_upper_no_wrist_no_hand.xml b/ergoCubSN000/la_upper_no_wrist_no_hand.xml
new file mode 100644
index 0000000000..4522035da6
--- /dev/null
+++ b/ergoCubSN000/la_upper_no_wrist_no_hand.xml
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN000" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <params>
+    <xi:include href="hardware/electronics/pc104.xml" />
+    </params>
+
+    <devices>
+
+    <!-- motor controllers wrappers -->
+    <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.xml" />
+
+
+    <xi:include href="wrappers/motorControl/left_upper_arm-mc_remapper.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" />
+
+    <!--  CALIBRATORS -->
+    <xi:include href="calibrators/left_arm_nohand-calib.xml" />
+
+
+  </devices>
+</robot>
diff --git a/ergoCubSN000/ra.xml b/ergoCubSN000/ra.xml
new file mode 100644
index 0000000000..740aa934e1
--- /dev/null
+++ b/ergoCubSN000/ra.xml
@@ -0,0 +1,33 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN000" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <params>
+    <xi:include href="hardware/electronics/pc104.xml" />
+    </params>
+
+    <devices>
+
+    <!-- motor controllers wrappers -->
+    <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" />
+
+
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper.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" />
+
+
+
+
+    <!--  CALIBRATORS -->
+    <xi:include href="calibrators/right_arm_nohand-calib.xml" />
+
+
+  </devices>
+</robot>
diff --git a/ergoCubSN000/ra_nohands.xml b/ergoCubSN000/ra_nohands.xml
new file mode 100644
index 0000000000..cee0728362
--- /dev/null
+++ b/ergoCubSN000/ra_nohands.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN000" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <params>
+    <xi:include href="hardware/electronics/pc104.xml" />
+    </params>
+
+    <devices>
+
+    <!-- motor controllers wrappers -->
+    <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" />
+
+
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper.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" />
+
+
+
+
+    <!--  CALIBRATORS -->
+    <xi:include href="calibrators/right_arm_nohand-calib.xml" />
+
+
+  </devices>
+</robot>
diff --git a/ergoCubSN000/wrappers/inertials/head-inertials_wrapper-deprecated.xml b/ergoCubSN000/wrappers/inertials/head-inertials_wrapper-deprecated.xml
index 203689886b..b1d16aa6ac 100644
--- a/ergoCubSN000/wrappers/inertials/head-inertials_wrapper-deprecated.xml
+++ b/ergoCubSN000/wrappers/inertials/head-inertials_wrapper-deprecated.xml
@@ -3,7 +3,7 @@
 
 
     <device xmlns:xi="http://www.w3.org/2001/XInclude" name="head-inertials_wrapper-deprecated" type="inertial">
-        <param name="name">  /ergocub/inertial  </param>
+        <param name="name">  /ergocubcub/inertial  </param>
 
 
         <action phase="startup" level="5" type="attach">
@@ -16,4 +16,4 @@
         </action>
 
         <action phase="shutdown" level="5" type="detach" />
-    </device>
+    </device> 
diff --git a/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
index f256cb64fb..3a8b012c14 100644
--- a/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/left_arm-mc_remapper.xml
@@ -6,17 +6,15 @@
         <elem name="left_arm_joints1">(  0  1 0 1 )</elem>
         <elem name="left_arm_joints2">(  2  3 0 1 )</elem>
         <elem name="left_arm_joints3">(  4  6 0 2 )</elem>
-        <elem name="left_arm_joints4">(  8  11 0 3 )</elem>
-        <elem name="left_arm_joints5">(  12  15 0 3 )</elem>
+        <elem name="left_arm_joints4">(  7  10 0 3 )</elem>
     </paramlist>
-    <param name="joints"> 14 </param>
+    <param name="joints"> 11 </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_joints4"> left_arm-eb25-j8_11-mc </elem>
-            <elem name="left_arm_joints5"> left_arm-eb26-j12_15-mc </elem>
+            <elem name="left_arm_joints4"> left_arm-eb23-j7_10-mc </elem>
         </paramlist>
     </action>
     <action phase="shutdown" level="20" type="detach" />
diff --git a/ergoCubSN000/wrappers/motorControl/left_upper_arm-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/left_upper_arm-mc_remapper.xml
new file mode 100644
index 0000000000..f79e751fbf
--- /dev/null
+++ b/ergoCubSN000/wrappers/motorControl/left_upper_arm-mc_remapper.xml
@@ -0,0 +1,21 @@
+<?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="left_upper_arm-mc_remapper" type="controlboardremapper">
+    <paramlist name="networks">
+        <elem name="left_arm_joints1">(  0  1 0 1 )</elem>
+        <elem name="left_arm_joints2">(  2  3 0 1 )</elem>
+        <!-- <elem name="left_arm_joints4">(  8  11 0 3 )</elem>
+        <elem name="left_arm_joints5">(  12  15 0 3 )</elem> -->
+    </paramlist>
+    <param name="joints"> 7 </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_joints4"> left_arm-eb25-j8_11-mc </elem>
+            <elem name="left_arm_joints5"> left_arm-eb26-j12_15-mc </elem> -->
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
index c9b19f06cc..03507af7ca 100644
--- a/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
@@ -1,22 +1,20 @@
 <?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="right_arm-mc_remapper" type="controlboardremapper">
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="rightt_arm-mc_remapper" type="controlboardremapper">
     <paramlist name="networks">
-        <elem name="right_arm_joints1">(  0  1 0 1 )</elem>
-        <elem name="right_arm_joints2">(  2  3 0 1 )</elem>
-        <elem name="right_arm_joints3">(  4 6 0 2 )</elem>
-        <elem name="right_arm_joints4">(  8  11 0 3 )</elem>
-        <elem name="right_arm_joints5">(  12  15 0 3 )</elem>
+        <elem name="rightt_arm_joints1">(  0  1 0 1 )</elem>
+        <elem name="rightt_arm_joints2">(  2  3 0 1 )</elem>
+        <elem name="rightt_arm_joints3">(  4  6 0 2 )</elem>
+        <elem name="rightt_arm_joints4">(  7  10 0 3 )</elem>
     </paramlist>
-    <param name="joints"> 16 </param>
+    <param name="joints"> 11 </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_joints4"> right_arm-eb28-j8_11-mc </elem>
-            <elem name="right_arm_joints5"> right_arm-eb29-j12_15-mc </elem>
+            <elem name="rightt_arm_joints1"> rightt_arm-eb2-j0_1-mc </elem>
+            <elem name="rightt_arm_joints2"> rightt_arm-eb4-j2_3-mc </elem>
+            <elem name="rightt_arm_joints3"> rightt_arm-eb31-j4_6-mc </elem>
+            <elem name="rightt_arm_joints4"> rightt_arm-eb22-j7_10-mc </elem>
         </paramlist>
     </action>
     <action phase="shutdown" level="20" type="detach" />
diff --git a/ergoCubSN000/wrappers/motorControl/right_arm-mc_wrapper.xml b/ergoCubSN000/wrappers/motorControl/right_arm-mc_wrapper.xml
index f875e68eab..76155af830 100644
--- a/ergoCubSN000/wrappers/motorControl/right_arm-mc_wrapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/right_arm-mc_wrapper.xml
@@ -1,11 +1,11 @@
 <?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="right_arm-mc_nws_yarp" type="controlBoard_nws_yarp">
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_arm-mc_nws_yarp" type="controlBoard_nws_yarp">
     <param name="period"> 0.01  </param>
-    <param name="name"> /ergocub/right_arm </param>
+    <param name="name"> /ergocub/left_arm </param>
     <action phase="startup" level="10" type="attach">
-        <param name="device"> right_arm-mc_remapper </param>
+        <param name="device"> left_arm-mc_remapper </param>
     </action>
     <action phase="shutdown" level="15" type="detach" />
 </device>

From cde0c5076025d1f798954de2ba27ea02388c1389 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Davide=20Tom=C3=A9?= <davide.tome@iit.it>
Date: Tue, 22 Nov 2022 12:14:54 +0100
Subject: [PATCH 7/9] Update ra.xml

---
 ergoCubSN000/ra.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ergoCubSN000/ra.xml b/ergoCubSN000/ra.xml
index 740aa934e1..eac79fa937 100644
--- a/ergoCubSN000/ra.xml
+++ b/ergoCubSN000/ra.xml
@@ -26,7 +26,7 @@
 
 
     <!--  CALIBRATORS -->
-    <xi:include href="calibrators/right_arm_nohand-calib.xml" />
+    <xi:include href="calibrators/right_arm-calib.xml" />
 
 
   </devices>

From fbc631119600c1c30d02530973f2d613a0a51ab6 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Tue, 22 Nov 2022 19:47:43 +0100
Subject: [PATCH 8/9] ergoCubSN000: cleanup + make work both hands +
 ergocub_all working

---
 ergoCubSN000/calibrators/left_arm-calib.xml   | 16 ++--
 ergoCubSN000/calibrators/left_leg-calib.xml   |  2 +-
 ergoCubSN000/calibrators/right_arm-calib.xml  | 19 +++--
 ergoCubSN000/calibrators/right_leg-calib.xml  |  2 +-
 ergoCubSN000/calibrators/torso-calib.xml      |  2 +-
 ergoCubSN000/ergocub_all.xml                  | 18 ++--
 .../electronics/left_arm-eb25-j8_11-eln.xml   | 40 ---------
 .../electronics/left_arm-eb26-j12_15-eln.xml  | 40 ---------
 .../electronics/right_arm-eb28-j8_11-eln.xml  | 40 ---------
 .../electronics/right_arm-eb29-j12_15-eln.xml | 40 ---------
 .../mechanicals/left_arm-eb2-j0_1-mec.xml     |  4 +-
 .../mechanicals/left_arm-eb23-j7_10-mec.xml   |  4 +-
 .../mechanicals/left_arm-eb25-j8_11-mec.xml   | 77 -----------------
 .../mechanicals/left_arm-eb26-j12_15-mec.xml  | 82 -------------------
 .../mechanicals/right_arm-eb1-j0_1-mec.xml    |  4 +-
 .../mechanicals/right_arm-eb22-j7_10-mec.xml  |  4 +-
 .../mechanicals/right_arm-eb28-j8_11-mec.xml  | 76 -----------------
 .../mechanicals/right_arm-eb29-j12_15-mec.xml | 82 -------------------
 .../motorControl/left_arm-eb2-j0_1-mc.xml     |  2 +-
 .../motorControl/left_arm-eb23-j7_10-mc.xml   | 14 ++--
 .../motorControl/left_arm-eb25-j8_11-mc.xml   | 75 -----------------
 .../left_arm-eb25-j8_11-mc_service.xml        | 66 ---------------
 .../motorControl/left_arm-eb26-j12_15-mc.xml  | 72 ----------------
 .../left_arm-eb26-j12_15-mc_service.xml       | 64 ---------------
 .../motorControl/right_arm-eb1-j0_1-mc.xml    |  2 +-
 .../motorControl/right_arm-eb22-j7_10-mc.xml  | 14 ++--
 .../motorControl/right_arm-eb28-j8_11-mc.xml  | 75 -----------------
 .../right_arm-eb28-j8_11-mc_service.xml       | 65 ---------------
 .../motorControl/right_arm-eb29-j12_15-mc.xml | 63 --------------
 .../right_arm-eb29-j12_15-mc_service.xml      | 65 ---------------
 .../motorControl/right_arm-mc_remapper.xml    | 18 ++--
 .../motorControl/right_arm-mc_wrapper.xml     |  6 +-
 32 files changed, 68 insertions(+), 1085 deletions(-)
 delete mode 100644 ergoCubSN000/hardware/electronics/left_arm-eb25-j8_11-eln.xml
 delete mode 100644 ergoCubSN000/hardware/electronics/left_arm-eb26-j12_15-eln.xml
 delete mode 100644 ergoCubSN000/hardware/electronics/right_arm-eb28-j8_11-eln.xml
 delete mode 100644 ergoCubSN000/hardware/electronics/right_arm-eb29-j12_15-eln.xml
 delete mode 100644 ergoCubSN000/hardware/mechanicals/left_arm-eb25-j8_11-mec.xml
 delete mode 100644 ergoCubSN000/hardware/mechanicals/left_arm-eb26-j12_15-mec.xml
 delete mode 100644 ergoCubSN000/hardware/mechanicals/right_arm-eb28-j8_11-mec.xml
 delete mode 100644 ergoCubSN000/hardware/mechanicals/right_arm-eb29-j12_15-mec.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb25-j8_11-mc.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb25-j8_11-mc_service.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb26-j12_15-mc.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/left_arm-eb26-j12_15-mc_service.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb28-j8_11-mc.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb28-j8_11-mc_service.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb29-j12_15-mc.xml
 delete mode 100644 ergoCubSN000/hardware/motorControl/right_arm-eb29-j12_15-mc_service.xml

diff --git a/ergoCubSN000/calibrators/left_arm-calib.xml b/ergoCubSN000/calibrators/left_arm-calib.xml
index a2d9465949..23937b9db0 100644
--- a/ergoCubSN000/calibrators/left_arm-calib.xml
+++ b/ergoCubSN000/calibrators/left_arm-calib.xml
@@ -15,21 +15,21 @@
 <param name="velocityHome">           10         10         10         10         10       10      10       40.00      40.00   40.00   40.00  </param>
 </group>
 <group name="CALIBRATION">
-<param name="calibrationType">        12         12         12         12         12       12      12        10          10      10      10       </param>
-<param name="calibration1">           -41179     7782       11224      25717      10816   7369    21632     -850        -850    -850    -850      </param>
-<param name="calibration2">	          0           0           0          0           0       0       0       0           0       0       0        </param>
+<param name="calibrationType">        12         12         12         12         12       12      12        5          5      5      5       </param>
+<param name="calibration1">           -41179     7782       11224      25717      10816   7369    21632      0        500    500     500      </param>
+<param name="calibration2">	      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        </param>
 <param name="calibration4">           0           0           0          0           0       0       0       0           0       0       0        </param>
 <param name="calibration5">           0           0           0          0           0       0       0       0           0       0       0        </param>
-<param name="calibrationZero">        0           0           0          0           0       0       0       85          85      85      85       </param>
+<param name="calibrationZero">        0           0           0          0           0       0       0       0          0       0       0       </param>
 <param name="calibrationDelta">       0           0           0          0           0       0       0       0           0       0       0        </param>
-<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0       30.0        30.0    30.0    30.0 </param>
+<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0       10.0        10.0    10.0    10.0 </param>
 <param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0      70.0        70.0    70.0    70.0 </param>
-<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000     0           0       0       0    </param>
-<param name="startupPosThreshold">    2          2            2          2         2       2       2         10          10      10      10   </param>
+<param name="startupMaxPwm">          2000       2000         2000      2000       16000   16000   16000     0           1000    1000       0    </param>
+<param name="startupPosThreshold">    2          2            2          2         2       2       2         50          50      50      50   </param>
 </group>
 
-<param name="CALIB_ORDER">  (1) (0) (2) (3) (4 5 6) (7 8 9 10)</param>
+<param name="CALIB_ORDER">(1) (0) (2) (3) (4 5 6) (7 8 9 10)</param> <!-- joint 7 is not working (demo Rome 22) -->
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">left_arm-mc_remapper</param>
 		</action>
diff --git a/ergoCubSN000/calibrators/left_leg-calib.xml b/ergoCubSN000/calibrators/left_leg-calib.xml
index 44761238c5..738e65b328 100644
--- a/ergoCubSN000/calibrators/left_leg-calib.xml
+++ b/ergoCubSN000/calibrators/left_leg-calib.xml
@@ -27,7 +27,7 @@
     
     <param name="startupPosition">                    0               5           0            -5          0       0        </param>
     <param name="startupVelocity">                    10.0            10.0        10.0          10         10      10       </param>
-    <param name="startupMaxPwm">                      12000           8000        3000          8000       4000    3000     </param>
+    <param name="startupMaxPwm">                      2000           2000        2000          4000       2000    2000     </param>
     <param name="startupPosThreshold">                2               2           2             2          2       2        </param>
   </group>
 
diff --git a/ergoCubSN000/calibrators/right_arm-calib.xml b/ergoCubSN000/calibrators/right_arm-calib.xml
index 89ac5c997c..a3f6e3a284 100644
--- a/ergoCubSN000/calibrators/right_arm-calib.xml
+++ b/ergoCubSN000/calibrators/right_arm-calib.xml
@@ -2,12 +2,12 @@
 <!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-calibrator" type="parametricCalibratorEth">
+	<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-calibrator" type="parametricCalibratorEth">
                 <xi:include href="../general.xml" />
 
 		<group name="GENERAL">
 		    <param name="joints">11</param> <!-- the number of joints of the robot part -->
-		    <param name="deviceName"> Left_Arm_Calibrator </param>
+		    <param name="deviceName"> Right_Arm_Calibrator </param>
 		</group>
 
 <group name="HOME">
@@ -15,21 +15,22 @@
 <param name="velocityHome">           10         10         10         10         10       10      10    40.00      40.00   40.00   40.00   </param>
 </group>
 <group name="CALIBRATION">
-<param name="calibrationType">        12         12         12         12         12       12      12       10          10      10      10         </param>
-<param name="calibration1">           40139     -4670     -60282      -59034      30262   28993   36030    -850        -850    -850    -850        </param>
-<param name="calibration2">	      0           0           0          0           0       0       0          0           0       0       0      </param>
+<param name="calibrationType">        12         12         12         12         12       12      12       5           5       5       5         </param>
+<param name="calibration1">           40139     -3690     -60282      -59034      30262   28993   36030    500          500    500      500        </param>
+<param name="calibration2">	      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          </param>
 <param name="calibration4">           0           0           0          0           0       0       0      0           0       0       0           </param>
 <param name="calibration5">           0           0           0          0           0       0       0      0           0       0       0           </param>
-<param name="calibrationZero">        0           0           0          0           0       0       0      85          85      85      85          </param>
+<param name="calibrationZero">        0           0           0          0           0       0       0      0           0       0       0          </param>
 <param name="calibrationDelta">       0           0           0          0           0       0       0      0           0       0       0     </param>
-<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0      30.0        30.0    30.0    30.0    </param>
+<param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0      10.0        10.0    10.0    10.0    </param>
 <param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0     70.0        70.0    70.0    70.0    </param>
-<param name="startupMaxPwm">          8000       8000         8000      8000       16000   16000   16000    0           0       0       0     </param>
+<param name="startupMaxPwm">          2000       2000         2000      2000       16000   16000   16000    0           0       0       0     </param>
 <param name="startupPosThreshold">    2          2            2          2         2       2       2        10          10      10      10      </param>
 </group>
 
-<param name="CALIB_ORDER">  (1) (0) (2) (3) (4 5 6) (7 8 9 10)</param>
+<!-- <param name="CALIB_ORDER">  (7 8 9 10) </param> -->
+ <param name="CALIB_ORDER">  (1) (0) (2) (3) (4 5 6) (7 8 9 10) </param>
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">right_arm-mc_remapper</param>
 		</action>
diff --git a/ergoCubSN000/calibrators/right_leg-calib.xml b/ergoCubSN000/calibrators/right_leg-calib.xml
index 8f5ceb6f0d..ec68eccd57 100644
--- a/ergoCubSN000/calibrators/right_leg-calib.xml
+++ b/ergoCubSN000/calibrators/right_leg-calib.xml
@@ -27,7 +27,7 @@
     
     <param name="startupPosition">                    0               5           0            -5          0       0        </param>
     <param name="startupVelocity">                    10.0            10.0        10.0          10         10      10       </param>
-    <param name="startupMaxPwm">                      12000           8000       4000          8000       4000    3000     </param>
+    <param name="startupMaxPwm">                      2000           2000       2000          4000       2000    2000     </param>
     <param name="startupPosThreshold">                2               2           2             2          2       2        </param>
   </group>
 
diff --git a/ergoCubSN000/calibrators/torso-calib.xml b/ergoCubSN000/calibrators/torso-calib.xml
index 647d5ebf84..972e4a7344 100644
--- a/ergoCubSN000/calibrators/torso-calib.xml
+++ b/ergoCubSN000/calibrators/torso-calib.xml
@@ -39,7 +39,7 @@
 </group>
 
  <!-- check old calibrator for the correct sequence, then ask to Randazz -->
-		<param name="CALIB_ORDER"> (0) (1) (2) </param>
+		<param name="CALIB_ORDER"> (1) (2) </param> <!-- roll disabled demo Rome 22 -->
 
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">torso-mc_remapper</param>
diff --git a/ergoCubSN000/ergocub_all.xml b/ergoCubSN000/ergocub_all.xml
index 9133df33b2..8455e6c6ae 100644
--- a/ergoCubSN000/ergocub_all.xml
+++ b/ergoCubSN000/ergocub_all.xml
@@ -16,13 +16,13 @@
     <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_shoulder-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" />
 
-    <xi:include href="wrappers/motorControl/right_shoulder-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper.xml" />
 
-    <xi:include href="wrappers/motorControl/left_shoulder-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.xml" />
 
-    <xi:include href="wrappers/motorControl/left_shoulder-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/left_arm-mc_remapper.xml" />
 
     <xi:include href="wrappers/motorControl/torso-mc_wrapper.xml" />
 
@@ -41,10 +41,14 @@
     <!-- 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" />
 
     <!-- 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" />
 
     <!-- LEFT LEG -->
     <xi:include href="hardware/motorControl/left_leg-eb8-j0_3-mc.xml" />
@@ -70,11 +74,11 @@
 
 
     <!--  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_shoulder-calib.xml" />
-    <xi:include href="calibrators/right_shoulder-calib.xml" />
-    <xi:include href="calibrators/torso-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" />
   </devices>
 </robot>
diff --git a/ergoCubSN000/hardware/electronics/left_arm-eb25-j8_11-eln.xml b/ergoCubSN000/hardware/electronics/left_arm-eb25-j8_11-eln.xml
deleted file mode 100644
index 111529fe0f..0000000000
--- a/ergoCubSN000/hardware/electronics/left_arm-eb25-j8_11-eln.xml
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <xi:include href="./pc104.xml" />
-    
-    <group name="ETH_BOARD">
-
-        <group name="ETH_BOARD_PROPERTIES">
-            <param name="IpAddress">                10.0.1.25               </param>
-            <param name="IpPort">                   12345                   </param>
-            <param name="Type">                     mc4plus                 </param>
-            <param name="maxSizeRXpacket">          768                     </param>
-            <param name="maxSizeROP">               384                     </param>
-        </group>
-
-        <group name="ETH_BOARD_SETTINGS">
-            <param name="Name">                     "left_arm-eb25-j8_11"   </param> 
-            <group name="RUNNINGMODE">
-                <param name="period">                   1000                </param>
-                <param name="maxTimeOfRXactivity">      400                 </param>
-                <param name="maxTimeOfDOactivity">      300                 </param>   
-                <param name="maxTimeOfTXactivity">      300                 </param>                
-                <param name="TXrateOfRegularROPs">      5                   </param> 
-            </group>              
-        </group>                 
-        
-        <group name="ETH_BOARD_ACTIONS">
-            <group name="MONITOR_ITS_PRESENCE">
-                <param name="enabled">                  true                </param> 
-                <param name="timeout">                  0.020               </param> 
-                <param name="periodOfMissingReport">    60.0                </param> 
-            </group>
-        </group>
-
-    </group>  
-
-</params>
-
diff --git a/ergoCubSN000/hardware/electronics/left_arm-eb26-j12_15-eln.xml b/ergoCubSN000/hardware/electronics/left_arm-eb26-j12_15-eln.xml
deleted file mode 100644
index f450a186e7..0000000000
--- a/ergoCubSN000/hardware/electronics/left_arm-eb26-j12_15-eln.xml
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <xi:include href="./pc104.xml" />
-    
-    <group name="ETH_BOARD">
-
-        <group name="ETH_BOARD_PROPERTIES">
-            <param name="IpAddress">                10.0.1.26               </param>
-            <param name="IpPort">                   12345                   </param>
-            <param name="Type">                     mc4plus                 </param>
-            <param name="maxSizeRXpacket">          768                     </param>
-            <param name="maxSizeROP">               384                     </param>
-        </group>
-
-        <group name="ETH_BOARD_SETTINGS">
-            <param name="Name">                     "left_arm-eb26-j12_15"  </param> 
-            <group name="RUNNINGMODE">
-                <param name="period">                   1000                </param>
-                <param name="maxTimeOfRXactivity">      400                 </param>
-                <param name="maxTimeOfDOactivity">      300                 </param>   
-                <param name="maxTimeOfTXactivity">      300                 </param>                
-                <param name="TXrateOfRegularROPs">      5                   </param> 
-            </group>              
-        </group>                 
-        
-        <group name="ETH_BOARD_ACTIONS">
-            <group name="MONITOR_ITS_PRESENCE">
-                <param name="enabled">                  true                </param> 
-                <param name="timeout">                  0.020               </param> 
-                <param name="periodOfMissingReport">    60.0                </param> 
-            </group>
-        </group>
-
-    </group>   
-
-</params>
-
diff --git a/ergoCubSN000/hardware/electronics/right_arm-eb28-j8_11-eln.xml b/ergoCubSN000/hardware/electronics/right_arm-eb28-j8_11-eln.xml
deleted file mode 100644
index a8296cf768..0000000000
--- a/ergoCubSN000/hardware/electronics/right_arm-eb28-j8_11-eln.xml
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <xi:include href="./pc104.xml" />
-    
-    <group name="ETH_BOARD">
-
-        <group name="ETH_BOARD_PROPERTIES">
-            <param name="IpAddress">                10.0.1.28               </param>
-            <param name="IpPort">                   12345                   </param>
-            <param name="Type">                     mc4plus                 </param>
-            <param name="maxSizeRXpacket">          768                     </param>
-            <param name="maxSizeROP">               384                     </param>
-        </group>
-
-        <group name="ETH_BOARD_SETTINGS">
-            <param name="Name">                     "right_arm-eb28-j8_11"  </param> 
-            <group name="RUNNINGMODE">
-                <param name="period">                   1000                </param>
-                <param name="maxTimeOfRXactivity">      400                 </param>
-                <param name="maxTimeOfDOactivity">      300                 </param>   
-                <param name="maxTimeOfTXactivity">      300                 </param>                
-                <param name="TXrateOfRegularROPs">      5                   </param> 
-            </group>              
-        </group>                 
-        
-        <group name="ETH_BOARD_ACTIONS">
-            <group name="MONITOR_ITS_PRESENCE">
-                <param name="enabled">                  true                </param> 
-                <param name="timeout">                  0.020               </param> 
-                <param name="periodOfMissingReport">    60.0                </param> 
-            </group>
-        </group>
-
-    </group>  
-
-</params>
-
diff --git a/ergoCubSN000/hardware/electronics/right_arm-eb29-j12_15-eln.xml b/ergoCubSN000/hardware/electronics/right_arm-eb29-j12_15-eln.xml
deleted file mode 100644
index f23cbce599..0000000000
--- a/ergoCubSN000/hardware/electronics/right_arm-eb29-j12_15-eln.xml
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <xi:include href="./pc104.xml" />
-    
-    <group name="ETH_BOARD">
-
-        <group name="ETH_BOARD_PROPERTIES">
-            <param name="IpAddress">                10.0.1.29               </param>
-            <param name="IpPort">                   12345                   </param>
-            <param name="Type">                     mc4plus                 </param>
-            <param name="maxSizeRXpacket">          768                     </param>
-            <param name="maxSizeROP">               384                     </param>
-        </group>
-
-        <group name="ETH_BOARD_SETTINGS">
-            <param name="Name">                     "right_arm-eb29-j12_15" </param> 
-            <group name="RUNNINGMODE">
-                <param name="period">                   1000                </param>
-                <param name="maxTimeOfRXactivity">      400                 </param>
-                <param name="maxTimeOfDOactivity">      300                 </param>   
-                <param name="maxTimeOfTXactivity">      300                 </param>                
-                <param name="TXrateOfRegularROPs">      5                   </param> 
-            </group>              
-        </group>                 
-        
-        <group name="ETH_BOARD_ACTIONS">
-            <group name="MONITOR_ITS_PRESENCE">
-                <param name="enabled">                  true                </param> 
-                <param name="timeout">                  0.020               </param> 
-                <param name="periodOfMissingReport">    60.0                </param> 
-            </group>
-        </group>
-
-    </group>   
-
-</params>
-
diff --git a/ergoCubSN000/hardware/mechanicals/left_arm-eb2-j0_1-mec.xml b/ergoCubSN000/hardware/mechanicals/left_arm-eb2-j0_1-mec.xml
index 883d56b46e..222bce3589 100644
--- a/ergoCubSN000/hardware/mechanicals/left_arm-eb2-j0_1-mec.xml
+++ b/ergoCubSN000/hardware/mechanicals/left_arm-eb2-j0_1-mec.xml
@@ -20,8 +20,8 @@
     
     <group name="LIMITS">
         <!--                                    0                   1              -->
-        <param name="hardwareJntPosMax">        16                165                 </param>  
-        <param name="hardwareJntPosMin">       -90                  0                 </param>
+        <param name="hardwareJntPosMax">        16                140                 </param>  
+        <param name="hardwareJntPosMin">       -90                -20                 </param>
         <param name="rotorPosMin">              0                   0                 </param> 
         <param name="rotorPosMax">              0                   0                 </param>
     </group>
diff --git a/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml b/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
index 9081f15352..09e63e4788 100644
--- a/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
+++ b/ergoCubSN000/hardware/mechanicals/left_arm-eb23-j7_10-mec.xml
@@ -23,8 +23,8 @@
     </group>
 
     <group name="LIMITS">
-        <param name="hardwareJntPosMax">    100             100             100             100         </param>
-        <param name="hardwareJntPosMin">    0               0               0               0           </param>
+        <param name="hardwareJntPosMax">    160             160             160             160         </param>
+        <param name="hardwareJntPosMin">    -10             -10            -10               -10           </param>
         <param name="rotorPosMin">          0               0               0               0           </param>
         <param name="rotorPosMax">          0               0               0               0           </param>
     </group>
diff --git a/ergoCubSN000/hardware/mechanicals/left_arm-eb25-j8_11-mec.xml b/ergoCubSN000/hardware/mechanicals/left_arm-eb25-j8_11-mec.xml
deleted file mode 100644
index b7730f5fc5..0000000000
--- a/ergoCubSN000/hardware/mechanicals/left_arm-eb25-j8_11-mec.xml
+++ /dev/null
@@ -1,77 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
- 
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-    <group name="GENERAL">
-        <param name="MotioncontrolVersion"> 6 </param>
-        <param name="Joints"> 4 </param> <!-- the number of joints of the robot part -->
-
-        <!-- joint number in sub-part           0                 1                 2                3           -->
-        <param name="AxisName">                 "l_thumb_oppose" "l_thumb_proximal" "l_thumb_distal" "l_index_proximal"  </param> 
-        <param name="AxisType">                 "revolute"       "revolute"         "revolute"       "revolute"      </param>
-        <param name="AxisMap">                  0                1                  2                3               </param>
-        <param name="Encoder">                  182.044          182.044            182.044          182.044         </param>    
-        <param name="fullscalePWM">            3360            3360             3360            3360            </param>
-        <param name="ampsToSensor">            1000.0          1000.0           1000.0          1000.0          </param>
-        <param name="Gearbox_M2J">              256                256                256           256              </param>
-        <param name="Gearbox_E2J">   	        1                 1                   1              1               </param>
-        <param name="useMotorSpeedFbk">         0                 0                   0              0               </param>
-        <param name="MotorType">               "DC"              "DC"                "DC"           "DC"               </param>
-        <param name="Verbose">                  0                                                                    </param>
-    </group>
-    
-    <group name="LIMITS">
-        <param name="hardwareJntPosMax">       110          90             180              90           </param>
-        <param name="hardwareJntPosMin">       10          0              0                0            </param>
-        <param name="rotorPosMin">              0           0           -70000            -5120         </param> 
-        <param name="rotorPosMax">              0           0           5120            32000           </param>
-    </group>
-
-       <group name="COUPLINGS"> 
-                            
-        <param name ="matrixJ2M"> 
-            1.000   0.000   0.000   0.000
-           -1.000  +1.000   0.000   0.000
-            0.000   0.000   1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-
-         <param name ="matrixM2J"> 
-            1.000   0.000   0.000   0.000
-           +1.000  +1.000   0.000   0.000
-            0.000   0.000   1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-       
-         <param name ="matrixE2J">  
-            1.000   0.000   0.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000   0.000   0.000
-            0.000   0.000   1.000   0.000   0.000   0.000
-            0.000   0.000   0.000   1.000   0.000   0.000   
-        </param>
-  
-    </group>  
-    <group name="JOINTSET_CFG"> 
-        <param name= "numberofsets"> 3 </param>
-         <group name="JOINTSET_0">
-                    <param name="listofjoints">  0  1            </param>
-                    <param name="constraint">    none          </param>
-                    <param name="param1">        0             </param>
-                    <param name="param2">        0            </param>
-            </group> 
-            <group name="JOINTSET_1">
-                    <param name="listofjoints">   2             </param>
-                    <param name="constraint">     none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-            <group name="JOINTSET_2">
-                    <param name="listofjoints">   3             </param>
-                    <param name="constraint">     none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-    </group>              
-    
-    
-</params>
diff --git a/ergoCubSN000/hardware/mechanicals/left_arm-eb26-j12_15-mec.xml b/ergoCubSN000/hardware/mechanicals/left_arm-eb26-j12_15-mec.xml
deleted file mode 100644
index 73fa0c18bf..0000000000
--- a/ergoCubSN000/hardware/mechanicals/left_arm-eb26-j12_15-mec.xml
+++ /dev/null
@@ -1,82 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
- 
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-    <group name="GENERAL">
-        <param name="MotioncontrolVersion"> 6 </param>
-        <param name="Joints"> 4 </param> <!-- the number of joints of the robot part -->
-
-        <!-- joint number in sub-part           0                 1                  2                 3           -->
-        <param name="AxisName">                 "l_index_distal" "l_middle_proximal" "l_middle_distal" "l_pinky"  </param> 
-        <param name="AxisType">                 "revolute"       "revolute"           "revolute"        "revolute"      </param>
-        <param name="AxisMap">                  0                1                   2                  3               </param>   
-        <param name="Encoder">                  182.044          182.044             182.044            182.044         </param>    
-        <param name="fullscalePWM">             3360             3360               3360                3360            </param>
-        <param name="ampsToSensor">             1000.0           1000.0             1000.0              1000.0          </param>
-        <param name="Gearbox_M2J">              256              256                 256                256             </param>
-        <param name="Gearbox_E2J">                1               1                  1                  1             </param>
-        <param name="useMotorSpeedFbk">           0               0                  0                  0             </param>
-        <param name="MotorType">               "DC"              "DC"                "DC"           "DC"               </param>
-        <param name="Verbose">                  0                                                                       </param>
-    </group>
-    
-    <group name="LIMITS">
-        <param name="hardwareJntPosMax">     180          90              180             270        </param>
-        <param name="hardwareJntPosMin">       0           0                0               0        </param> 
-        <param name="rotorPosMin">         -5120       -5120              -65000          -5120     </param> 
-        <param name="rotorPosMax">          65000       32000              5120            65000      </param>
-    </group>
-
-    <group name="COUPLINGS"> 
-                            
-        <param name ="matrixJ2M"> 
-            1.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000
-            0.000   0.000   1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-
-        <param name ="matrixM2J"> 
-            1.000   0.000   0.000   0.000
-            1.000   1.000   0.000   0.000
-            0.000   0.000   1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-       
-         <param name ="matrixE2J">  
-            1.000   0.000   0.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000   0.000   0.000
-            0.000   0.000   1.000   0.000   0.000   0.000
-            0.000   0.000   0.000   1.000   0.000   0.000   
-        </param>
-  
-    </group>                
-
-    <group name="JOINTSET_CFG"> 
-        <param name= "numberofsets"> 4</param>
-         <group name="JOINTSET_0">
-                    <param name="listofjoints">  0             </param>
-                    <param name="constraint">    none          </param>
-                    <param name="param1">        0             </param>
-                    <param name="param2">        0             </param>
-            </group> 
-            <group name="JOINTSET_1">
-                    <param name="listofjoints">   1             </param>
-                    <param name="constraint">      none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-            <group name="JOINTSET_2">
-                    <param name="listofjoints">   2             </param>
-                    <param name="constraint">      none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-            <group name="JOINTSET_3">
-                    <param name="listofjoints">   3             </param>
-                    <param name="constraint">     none         </param>
-                    <param name="param1">         0            </param>
-                    <param name="param2">         0            </param>
-            </group> 
-    </group>                            
-</params>
diff --git a/ergoCubSN000/hardware/mechanicals/right_arm-eb1-j0_1-mec.xml b/ergoCubSN000/hardware/mechanicals/right_arm-eb1-j0_1-mec.xml
index 5d1a9b7554..9a5629e9ac 100644
--- a/ergoCubSN000/hardware/mechanicals/right_arm-eb1-j0_1-mec.xml
+++ b/ergoCubSN000/hardware/mechanicals/right_arm-eb1-j0_1-mec.xml
@@ -20,8 +20,8 @@
     
     <group name="LIMITS">
         <!--                                    0                   1                   -->
-        <param name="hardwareJntPosMax">        16                165                  </param>  
-        <param name="hardwareJntPosMin">       -90                  0                   </param>
+        <param name="hardwareJntPosMax">        16                140                  </param>  
+        <param name="hardwareJntPosMin">       -90                -20                   </param>
         <param name="rotorPosMin">              0                   0                   </param> 
         <param name="rotorPosMax">              0                   0                   </param>
     </group>
diff --git a/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml b/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
index d3d3da4096..84a98c970c 100644
--- a/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
+++ b/ergoCubSN000/hardware/mechanicals/right_arm-eb22-j7_10-mec.xml
@@ -23,8 +23,8 @@
     </group>
 
     <group name="LIMITS">
-        <param name="hardwareJntPosMax">    100             100             100             100         </param>
-        <param name="hardwareJntPosMin">    0              -10             -10              0           </param>
+        <param name="hardwareJntPosMax">    160             160             160             160         </param>
+        <param name="hardwareJntPosMin">    -10              -10             -10              -10           </param>
         <param name="rotorPosMin">          0               0               0               0           </param>
         <param name="rotorPosMax">          0               0               0               0           </param>
     </group>
diff --git a/ergoCubSN000/hardware/mechanicals/right_arm-eb28-j8_11-mec.xml b/ergoCubSN000/hardware/mechanicals/right_arm-eb28-j8_11-mec.xml
deleted file mode 100644
index 8dc158053f..0000000000
--- a/ergoCubSN000/hardware/mechanicals/right_arm-eb28-j8_11-mec.xml
+++ /dev/null
@@ -1,76 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
- 
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-    <group name="GENERAL">
-        <param name="MotioncontrolVersion"> 6 </param>
-        <param name="Joints"> 4 </param> <!-- the number of joints of the robot part -->
-
-        <!-- joint number in sub-part           0                 1                 2                3           -->
-        <param name="AxisName">                 "r_thumb_oppose" "r_thumb_proximal" "r_thumb_distal" "r_index_proximal"  </param> 
-        <param name="AxisType">                 "revolute"       "revolute"         "revolute"       "revolute"      </param>
-        <param name="AxisMap">                  0                1                  2                3               </param>  
-        <param name="Encoder">                  182.044          182.044            182.044          182.044         </param>    
-        <param name="fullscalePWM">             3360             3360               3360            3360             </param>
-        <param name="ampsToSensor">             1000.0           1000.0             1000.0          1000.0           </param>
-        <param name="Gearbox_M2J">              256                256                256           256              </param>
-        <param name="Gearbox_E2J">   	        1                 1                   1              1               </param>
-        <param name="useMotorSpeedFbk">         0                 0                   0              0               </param>
-        <param name="MotorType">                "DC"             "DC"                "DC"           "DC"             </param>
-        <param name="Verbose">                  0                                                                    </param>
-    </group>
-    
-    <group name="LIMITS">
-        <param name="hardwareJntPosMax">       110          90             180              90            </param>
-        <param name="hardwareJntPosMin">       10           0              0                0            </param>
-        <param name="rotorPosMin">              0           0         -5120             -5120            </param> 
-        <param name="rotorPosMax">              0           0         70000            32000             </param>
-    </group>
-
-    <group name="COUPLINGS"> 
-                            
-        <param name ="matrixJ2M"> 
-            1.000   0.000   0.000   0.000
-            1.000  +1.000   0.000   0.000
-            0.000   0.000   1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-
-         <param name ="matrixM2J"> 
-            1.000   0.000   0.000   0.000
-           -1.000  +1.000   0.000   0.000
-            0.000   0.000   1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-       
-         <param name ="matrixE2J">  
-            1.000   0.000   0.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000   0.000   0.000
-            0.000   0.000   1.000   0.000   0.000   0.000
-            0.000   0.000   0.000   1.000   0.000   0.000   
-        </param>
-  
-    </group>  
-
-    <group name="JOINTSET_CFG"> 
-        <param name= "numberofsets"> 3 </param>
-         <group name="JOINTSET_0">
-                    <param name="listofjoints">  0  1            </param>
-                    <param name="constraint">    none          </param>
-                    <param name="param1">        0             </param>
-                    <param name="param2">        0            </param>
-            </group> 
-            <group name="JOINTSET_1">
-                    <param name="listofjoints">   2             </param>
-                    <param name="constraint">     none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-            <group name="JOINTSET_2">
-                    <param name="listofjoints">   3             </param>
-                    <param name="constraint">     none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-    </group>              
-</params>
diff --git a/ergoCubSN000/hardware/mechanicals/right_arm-eb29-j12_15-mec.xml b/ergoCubSN000/hardware/mechanicals/right_arm-eb29-j12_15-mec.xml
deleted file mode 100644
index 5b2afc90b7..0000000000
--- a/ergoCubSN000/hardware/mechanicals/right_arm-eb29-j12_15-mec.xml
+++ /dev/null
@@ -1,82 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
- 
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-    <group name="GENERAL">
-        <param name="MotioncontrolVersion"> 6 </param>
-        <param name="Joints"> 4 </param> <!-- the number of joints of the robot part -->
-
-        <!-- joint number in sub-part           0                 1                  2                 3           -->
-        <param name="AxisName">                 "r_index_distal" "r_middle_proximal" "r_middle_distal" "r_pinky"  </param> 
-        <param name="AxisType">                 "revolute"       "revolute"           "revolute"        "revolute"       </param>
-        <param name="AxisMap">                  0                1                   2                  3                </param>   
-         <param name="Encoder">                  182.044          182.044             182.044            182.044         </param>   
-        <param name="fullscalePWM">             3360              3360                3360             3360              </param>
-        <param name="ampsToSensor">             1000.0            1000.0              1000.0           1000.0            </param>
-        <param name="Gearbox_M2J">               256              256                 256                256             </param>
-        <param name="Gearbox_E2J">   	           1                1                   1                  1             </param>
-        <param name="useMotorSpeedFbk">            0                0                   0                  0             </param>
-        <param name="MotorType">                  "DC"             "DC"                "DC"               "DC"           </param>
-        <param name="Verbose">                  0                                                                        </param>
-    </group>
-    
-    <group name="LIMITS">
-        <param name="hardwareJntPosMax">        180                 90                180                 270          </param>
-        <param name="hardwareJntPosMin">          0                  0                  0                   0          </param> 
-        <param name="rotorPosMin">           -65000              -5120              -5120               -65000          </param> 
-        <param name="rotorPosMax">             5120              32000              65000                 5120          </param>
-    </group>
-
-    <group name="COUPLINGS"> 
-                            
-        <param name ="matrixJ2M"> 
-            1.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000
-            0.000   0.000   1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-
-        <param name ="matrixM2J"> 
-            1.000   0.000   0.000   0.000
-            1.000   1.000   0.000   0.000
-            0.000   0.000   1.000   0.000
-            0.000   0.000   0.000   1.000   
-        </param>
-       
-         <param name ="matrixE2J">  
-            1.000   0.000   0.000   0.000   0.000   0.000
-            0.000   1.000   0.000   0.000   0.000   0.000
-            0.000   0.000   1.000   0.000   0.000   0.000
-            0.000   0.000   0.000   1.000   0.000   0.000   
-        </param>
-  
-    </group>                
-
-    <group name="JOINTSET_CFG"> 
-        <param name= "numberofsets"> 4</param>
-         <group name="JOINTSET_0">
-                    <param name="listofjoints">  0             </param>
-                    <param name="constraint">    none          </param>
-                    <param name="param1">        0             </param>
-                    <param name="param2">        0             </param>
-            </group> 
-            <group name="JOINTSET_1">
-                    <param name="listofjoints">   1             </param>
-                    <param name="constraint">      none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-            <group name="JOINTSET_2">
-                    <param name="listofjoints">   2             </param>
-                    <param name="constraint">      none          </param>
-                    <param name="param1">         0             </param>
-                    <param name="param2">         0             </param>
-            </group> 
-            <group name="JOINTSET_3">
-                    <param name="listofjoints">   3             </param>
-                    <param name="constraint">     none         </param>
-                    <param name="param1">         0            </param>
-                    <param name="param2">         0            </param>
-            </group> 
-    </group>                            
-</params>
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb2-j0_1-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb2-j0_1-mc.xml
index 8c2d1fc267..99e3b66f17 100644
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb2-j0_1-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb2-j0_1-mc.xml
@@ -11,7 +11,7 @@
     <!-- joint number                           0                   1                   -->
     <!-- joint name                          shoulder_pitch        shoulder_roll        -->
     <group name="LIMITS">
-        <param name="jntPosMax">                14                  163                 </param>
+        <param name="jntPosMax">                14                  130                 </param>
         <param name="jntPosMin">                -88                 17                  </param>
         <param name="jntVelMax">                120                 120                 </param>
         <param name="motorNominalCurrents">     5000                5000                </param>
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
index 949dc844fe..2dd34892b3 100644
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb23-j7_10-mc.xml
@@ -13,12 +13,12 @@
     <!-- joint number                           0                   1                   2                   3                   -->
     <!-- joint name                             thumb               index               medium              pinky               -->    
     <group name="LIMITS">
-        <param name="jntPosMin">                +5                  +5                  +5                  +5                  </param>
-        <param name="jntPosMax">                +85                 +85                 +85                 +85                 </param>
+        <param name="jntPosMin">                0                    0                    0                  0                  </param>
+        <param name="jntPosMax">                150                 150                 150                 150                 </param>
         <param name="jntVelMax">                1000                1000                1000                1000                </param>
-        <param name="motorOverloadCurrents">    2000                2000                2500                2500                </param>
-        <param name="motorNominalCurrents">     700                 700                 700                 700                 </param>
-        <param name="motorPeakCurrents">        1500                1500                1500                1500                </param>    
+        <param name="motorOverloadCurrents">    4000                4000                4000                4000                </param>
+        <param name="motorNominalCurrents">     2000                2000                2000                2000                 </param>
+        <param name="motorPeakCurrents">        3500                3500                3500                3500                </param>    
         <param name="motorPwmLimit">            3360                3360                3360                3360                </param>
     </group>
    
@@ -48,9 +48,9 @@
         <param name="outputType">               pwm             </param>
         <param name="fbkControlUnits">          metric_units    </param>
         <param name="outputControlUnits">       machine_units   </param>
-        <param name="kp">                       -150        +300        -300        -200    </param>
+        <param name="kp">                       -150        -300        -300        -200    </param>
         <param name="kd">                       0           0           0           0       </param>
-        <param name="ki">                       -100        +30         -30         -30     </param>
+        <param name="ki">                       -100        -30         -30         -30     </param>
         <param name="maxOutput">                3360        3360        3360        3360    </param>
         <param name="maxInt">                   1000        1000        1000        1000    </param>
         <param name="stictionUp">               0           0           0           0       </param>
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb25-j8_11-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb25-j8_11-mc.xml
deleted file mode 100644
index 90be44be6e..0000000000
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb25-j8_11-mc.xml
+++ /dev/null
@@ -1,75 +0,0 @@
-<?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="left_arm-eb25-j8_11-mc" type="embObjMotionControl">
-
-    <xi:include href="../../general.xml" />
-    <xi:include href="../../hardware/electronics/left_arm-eb25-j8_11-eln.xml" />
-    <xi:include href="../../hardware/mechanicals/left_arm-eb25-j8_11-mec.xml" />   
-    <xi:include href="./left_arm-eb25-j8_11-mc_service.xml" /> 
-
-    <!-- joint number                           0                   1                   2                   3                   -->
-    <!-- joint name                             "l_thumb_oppose"    "l_thumb_proximal"  "l_thumb_distal"    "l_index_proximal"  -->
-    <group name="LIMITS">
-        <param name="jntPosMax">                110                 90                  180                 90                  </param>
-        <param name="jntPosMin">                10                  0                   0                   0                   </param>
-        <param name="jntVelMax">                1000                1000                1000                1000                </param>
-        <param name="motorOverloadCurrents">    2000                2000                2000                2000                </param>
-        <param name="motorNominalCurrents">     600                 600                 600                 600                 </param>
-        <param name="motorPeakCurrents">        1000                1000                1000                1000                </param>
-   <!-- to fix problem in https://github.com/robotology/icub-tech-support/issues/1048 the values are changed-->
-        <param name="motorPwmLimit">            1800                1800                1800                1800                </param>
-    </group>
-
-    <group name="TIMEOUTS">
-        <param name="velocity">                 100                 100                 100                 100	                </param>
-    </group>
-
-    <group name="IMPEDANCE">
-        <param name="stiffness">                0.0                 0.0                 0.0                 0.0                 </param>
-        <param name="damping">                  0.0                 0.0                 0.0                 0.0                 </param>
-    </group>
-    
-    <group name="CONTROLS">
-       <param name="positionControl">           POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-       <param name="velocityControl">           POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-       <param name="mixedControl">              POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-       <param name="torqueControl">             none                none                none                none                </param> 
-       <param name="currentPid">                none                none                none                none                </param>
-       <param name="speedPid">                  none                none                none                none                </param> 
-    </group>
-
-
-    <!-- default position PID: begin -->
-
-    <group name="POS_PID_DEFAULT">
-        <param name="controlLaw">           minjerk                 </param>
-        <param name="outputType">           pwm                     </param>
-        <param name="fbkControlUnits">      metric_units            </param> 
-        <param name="outputControlUnits">   machine_units           </param>
-        <param name="kp">                   -500        500         500         500     </param>
-        <param name="kd">                   0           0           0           0       </param>
-        <param name="ki">                   -50         50          50          50      </param>
-   <!-- to fix problem in https://github.com/robotology/icub-tech-support/issues/1048 the values are changed-->
-        <param name="maxOutput">            1800       1800        1800        1800    </param>
-        <param name="maxInt">               1800       1800        1800        1800    </param>
-        <param name="stictionUp">           0           0           0           0       </param>
-        <param name="stictionDown">         0           0           0           0       </param>
-        <param name="kff">                  0           0           0           0       </param>
-    </group>
-
-    <!-- default position PID: end -->
-
-
-    <!-- other default PIDs: begin -->   
-    <!-- other default PIDs: end -->
-
-
-    <!-- custom PIDs: begin -->    
-    <!-- custom PIDs: end -->
-    
-</device> 
- 
-    
-
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb25-j8_11-mc_service.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb25-j8_11-mc_service.xml
deleted file mode 100644
index da13b06cd9..0000000000
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb25-j8_11-mc_service.xml
+++ /dev/null
@@ -1,66 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <group name="SERVICE">
-        
-        <param name="type"> eomn_serv_MC_mc4plusmais </param> 
-    
-        <group name="PROPERTIES">
-        
-            <group name="ETHBOARD">
-                <param name="type">             mc4plus             </param> 
-            </group>          
-
-            <group name="CANBOARDS"> 
-                <param name="type">                 mais                </param> 
-                <group name="PROTOCOL">
-                    <param name="major">            0                   </param>    
-                    <param name="minor">            0                   </param>     
-                </group>                    
-                <group name="FIRMWARE">
-                    <param name="major">            0                   </param>    
-                    <param name="minor">            0                   </param> 
-                    <param name="build">            0                   </param>
-                </group>
-            </group>
-            
-            <group name="MAIS"> 
-                <param name="location">             CAN1:14             </param> 
-            </group>                      
-            
-           
-
-            <group name="JOINTMAPPING">
-
-                <group name="ACTUATOR">                 
-                    <param name="type">             pwm                 pwm                 pwm                 pwm                     </param>
-                    <param name="port">             CONN:P3             CONN:P2             CONN:P4             CONN:P5                 </param>
-                </group>
-                
-                <group name="ENCODER1">  
-                    <param name="type">             absanalog           mais                mais                mais                    </param>  
-                    <param name="port">             CONN:P3             MAIS:thumbproximal  MAIS:thumbdistal    MAIS:indexproximal      </param>
-                    <param name="position">         atjoint             atjoint             atjoint             atjoint                 </param>
-                    <param name="resolution">        65535              65535               65535               65535           </param> <!-- 65535:this value will change by fw during calibration (type 6 or 7) --> 
-                    <param name="tolerance">         0                0                   0                  0                        </param>  
-                </group>        
-                
-                <group name="ENCODER2">
-                    <param name="type">             none                qenc                qenc                qenc                    </param>
-                    <param name="port">             CONN:none           CONN:P2             CONN:P4             CONN:P5                 </param>
-                    <param name="position">         none                atmotor             atmotor             atmotor                 </param>
-                    <param name="resolution">       1                   40                   40                 40                      </param>
-                    <param name="tolerance">        0                    0                   0                  0                        </param>  
-                </group>  
-            </group>    
-            
-           
-       </group>
-             
-    </group>
-    
-  
-</params>
-
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb26-j12_15-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb26-j12_15-mc.xml
deleted file mode 100644
index 4ffe461c80..0000000000
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb26-j12_15-mc.xml
+++ /dev/null
@@ -1,72 +0,0 @@
-<?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="left_arm-eb26-j12_15-mc" type="embObjMotionControl">
-
-    <xi:include href="../../general.xml" />
-    <xi:include href="../../hardware/electronics/left_arm-eb26-j12_15-eln.xml" />
-    <xi:include href="../../hardware/mechanicals/left_arm-eb26-j12_15-mec.xml" />   
-    <xi:include href="./left_arm-eb26-j12_15-mc_service.xml" />
-
-    <!-- joint number                           0                   1                   2                   3                   -->
-    <!-- joint name                             index_distal        middle-proximal     middle-distal       little-fingers      -->     
-    <group name="LIMITS">
-        <param name="jntPosMax">                180                 90                  180                 270                 </param>
-        <param name="jntPosMin">                0                   0                   0                   0                   </param> 
-        <param name="jntVelMax">                1000                1000                1000                1000                </param>
-        <param name="motorOverloadCurrents">    2000                2000                2000                2000                </param>
-        <param name="motorNominalCurrents">     600                 600                 600                 600                 </param>
-        <param name="motorPeakCurrents">        1000                1000                1000                1000                </param>
-        <param name="motorPwmLimit">            3360                3360                3360                3360                </param>
-    </group>
-
-    <group name="TIMEOUTS">
-        <param name="velocity">                 100                 100                 100                 100	                </param>
-    </group>
-
-    <group name="IMPEDANCE">
-        <param name="stiffness">                0.0                 0.0                 0.0                 0.0                 </param>
-        <param name="damping">                  0.0                 0.0                 0.0                 0.0                 </param>
-    </group>
-
-    <group name="CONTROLS">
-        <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-        <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-        <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-        <param name="torqueControl">            none                none                none                none                </param> 
-        <param name="currentPid">               none                none                none                none                </param>
-        <param name="speedPid">                 none                none                none                none                </param> 
-    </group>
-
-
-    <!-- default position PID: begin -->
-    
-    <group name="POS_PID_DEFAULT">
-        <param name="controlLaw">             minjerk                   </param>
-        <param name="outputType">             pwm                       </param>
-        <param name="fbkControlUnits">        metric_units              </param> 
-        <param name="outputControlUnits">     machine_units             </param>
-        <param name="kp">                   -500        300         500        -200         </param>
-        <param name="kd">                   0           0           0           0           </param>
-        <param name="ki">                   -50         50          50         -30          </param>
-        <param name="maxOutput">            3360        3360        3360        3360        </param>
-        <param name="maxInt">               3360        3360        3360        3360        </param>
-        <param name="stictionUp">           0           0           0           0           </param>
-        <param name="stictionDown">         0           0           0           0           </param>
-        <param name="kff">                  0           0           0           0           </param>
-    </group>
-
-    <!-- default position PID: end -->
-
-
-    <!-- other default PIDs: begin -->   
-    <!-- other default PIDs: end -->
-
-
-    <!-- custom PIDs: begin -->    
-    <!-- custom PIDs: end -->
-    
-</device> 
-    
-
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb26-j12_15-mc_service.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb26-j12_15-mc_service.xml
deleted file mode 100644
index 4611334cd9..0000000000
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb26-j12_15-mc_service.xml
+++ /dev/null
@@ -1,64 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <group name="SERVICE">
-        
-        <param name="type"> eomn_serv_MC_mc4plusmais </param> 
-    
-        <group name="PROPERTIES">
-        
-            <group name="ETHBOARD">
-                <param name="type">             mc4plus             </param> 
-            </group>          
-
-            <group name="CANBOARDS"> 
-                <param name="type">                 mais                </param> 
-                <group name="PROTOCOL">
-                    <param name="major">            0                   </param>    
-                    <param name="minor">            0                   </param>     
-                </group>                    
-                <group name="FIRMWARE">
-                    <param name="major">            0                   </param>    
-                    <param name="minor">            0                   </param> 
-                    <param name="build">            0                   </param>
-                </group>
-            </group>
-            
-            <group name="MAIS"> 
-                <param name="location">             CAN1:14             </param> 
-            </group>                      
-            
- 
-            <group name="JOINTMAPPING">
-
-                <group name="ACTUATOR">                 
-                    <param name="type">             pwm                 pwm                 pwm                 pwm                     </param>
-                    <param name="port">             CONN:P3             CONN:P2             CONN:P4             CONN:P5                 </param>
-                </group>
-                
-                <group name="ENCODER1">  
-                    <param name="type">             mais                mais                mais                mais                    </param>  
-                    <param name="port">             MAIS:indexdistal    MAIS:mediumproximal MAIS:mediumdistal   MAIS:littlefingers      </param>
-                    <param name="position">         atjoint             atjoint             atjoint             atjoint                 </param> 
-                    <param name="resolution">          65535            65535               65535              65535                    </param> <!-- 65535:this value will change by fw during calibration (type 6) -->
-                    <param name="tolerance">          0                  0                   0                  0                        </param>  
-                </group>        
-                
-                <group name="ENCODER2">
-                    <param name="type">             qenc                qenc                qenc                qenc                    </param>
-                    <param name="port">             CONN:P3             CONN:P2             CONN:P4             CONN:P5                 </param>
-                    <param name="position">         atmotor             atmotor             atmotor             atmotor                 </param>
-                    <param name="resolution">          40               40                  40                  40                      </param>
-                    <param name="tolerance">         0                   0                   0                  0                        </param>  
-                </group>  
-            </group>    
-            
-        </group>
-           
-    </group>
-    
-  
-</params>
-
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb1-j0_1-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb1-j0_1-mc.xml
index b966197ff8..514c55bb81 100644
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb1-j0_1-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb1-j0_1-mc.xml
@@ -12,7 +12,7 @@
     <!-- joint number                           0                   1                   -->
     <!-- joint name                          shoulder_pitch            shoulder_roll    -->
     <group name="LIMITS">
-        <param name="jntPosMax">                14                  163                </param>
+        <param name="jntPosMax">                14                  130                </param>
         <param name="jntPosMin">                -88                 17                  </param>
         <param name="jntVelMax">                120                 120                 </param>
         <param name="motorNominalCurrents">     5000                5000                </param>
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
index b368510b31..7405a558b7 100644
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb22-j7_10-mc.xml
@@ -3,20 +3,20 @@
 
 
 
-<device xmlns:xi="http://www.w3.org/2001/XInclude" name="rightt_arm-eb22-j7_10-mc" type="embObjMotionControl">
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-eb22-j7_10-mc" type="embObjMotionControl">
 
     <xi:include href="../../general.xml" />
-    <xi:include href="../electronics/rightt_arm-eb22-j7_10-eln.xml" />
-    <xi:include href="../mechanicals/rightt_arm-eb22-j7_10-mec.xml" />
-    <xi:include href="../motorControl/rightt_arm-eb22-j7_10-mc_service.xml" />
+    <xi:include href="../electronics/right_arm-eb22-j7_10-eln.xml" />
+    <xi:include href="../mechanicals/right_arm-eb22-j7_10-mec.xml" />
+    <xi:include href="../motorControl/right_arm-eb22-j7_10-mc_service.xml" />
 
     <!-- joint number                           0                   1                   2                   3                   -->
     <!-- joint name                             thumb               index               medium              pinky               -->    
     <group name="LIMITS">
-        <param name="jntPosMin">               +5                  +5                  +5                  +5                  </param>
-        <param name="jntPosMax">               +85                 +85                 +85                 +85                 </param>
+        <param name="jntPosMin">                0                   0                   0                   0                  </param>
+        <param name="jntPosMax">                150                 150                 150                 150                 </param>
         <param name="jntVelMax">                1000                1000                1000                1000                </param>
-        <param name="motorOverloadCurrents">    2000                2000                2500                2500                </param>
+        <param name="motorOverloadCurrents">    4000                4000                4000                4000                </param>
         <param name="motorNominalCurrents">     700                 700                 700                 700                 </param>
         <param name="motorPeakCurrents">        1500                1500                1500                1500                </param>    
         <param name="motorPwmLimit">            3360                3360                3360                3360                </param>
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb28-j8_11-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb28-j8_11-mc.xml
deleted file mode 100644
index 064d3cb61c..0000000000
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb28-j8_11-mc.xml
+++ /dev/null
@@ -1,75 +0,0 @@
-<?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="right_arm-eb28-j8_11-mc" type="embObjMotionControl">
-
-    <xi:include href="../../general.xml" />
-    <xi:include href="../../hardware/electronics/right_arm-eb28-j8_11-eln.xml" />
-    <xi:include href="../../hardware/mechanicals/right_arm-eb28-j8_11-mec.xml" />    
-    <xi:include href="./right_arm-eb28-j8_11-mc_service.xml" />
-
-    <!-- joint number                           0                   1                   2                   3                   -->
-    <!-- joint name                             "r_thumb_oppose"    "r_thumb_proximal"  "r_thumb_distal"    "r_index_proximal"  -->
-    <group name="LIMITS">
-        <param name="jntPosMax">                110                 90                  180                 90                  </param>
-        <param name="jntPosMin">                10                  0                   0                   0                   </param>
-        <param name="jntVelMax">                1000                1000                1000                1000                </param>
-        <param name="motorOverloadCurrents">    2000                2000                2000                2000                </param>
-        <param name="motorNominalCurrents">      600                600                 600                 600                 </param>
-        <param name="motorPeakCurrents">        1000                1000                1000                1000                </param>
-   <!-- to fix problem in https://github.com/robotology/icub-tech-support/issues/1048 the values are changed-->
-        <param name="motorPwmLimit">            3360                3360                3360                3360                </param>
-    </group>
-
-    <group name="TIMEOUTS">
-        <param name="velocity">                 100                 100                 100                 100	                </param>
-    </group>
-
-    <group name="IMPEDANCE">
-        <param name="stiffness">                0.0                 0.0                 0.0                 0.0                 </param>
-        <param name="damping">                  0.0                 0.0                 0.0                 0.0                 </param>
-    </group>
-
-    <group name="CONTROLS">
-       <param name="positionControl">           POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-       <param name="velocityControl">           POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-       <param name="mixedControl">              POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-       <param name="torqueControl">             none                none                none                none                </param> 
-       <param name="currentPid">                none                none                none                none                </param>
-       <param name="speedPid">                  none                none                none                none                </param> 
-    </group>
-
-
-    <!-- default position PID: begin -->
-
-    <group name="POS_PID_DEFAULT">
-        <param name="controlLaw">           minjerk                   </param>
-        <param name="outputType">           pwm                       </param>
-        <param name="fbkControlUnits">      metric_units              </param> 
-        <param name="outputControlUnits">   machine_units             </param>
-        <param name="kp">                   -150        150         -500        500         </param>
-        <param name="kd">                   0           0           0           0           </param>
-        <param name="ki">                   -50         50          -50         50          </param>
-   <!-- to fix problem in https://github.com/robotology/icub-tech-support/issues/1048 the values are changed-->
-        <param name="maxOutput">            3360        3360        3360        3360        </param>
-        <param name="maxInt">               3360        3360        3360        3360        </param>
-        <param name="stictionUp">           0           0           0           0           </param>
-        <param name="stictionDown">         0           0           0           0           </param>
-        <param name="kff">                  0           0           0           0           </param>
-    </group>
-
-    <!-- default position PID: end -->
-
-
-    <!-- other default PIDs: begin -->   
-    <!-- other default PIDs: end -->
-
-
-    <!-- custom PIDs: begin -->    
-    <!-- custom PIDs: end -->
-    
-</device>  
-
-    
-
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb28-j8_11-mc_service.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb28-j8_11-mc_service.xml
deleted file mode 100644
index d9a2041eeb..0000000000
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb28-j8_11-mc_service.xml
+++ /dev/null
@@ -1,65 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <group name="SERVICE">
-        
-        <param name="type"> eomn_serv_MC_mc4plusmais </param> 
-    
-        <group name="PROPERTIES">
-        
-            <group name="ETHBOARD">
-                <param name="type">             mc4plus             </param> 
-            </group>          
-
-            <group name="CANBOARDS"> 
-                <param name="type">                 mais                </param> 
-                <group name="PROTOCOL">
-                    <param name="major">            0                   </param>    
-                    <param name="minor">            0                   </param>     
-                </group>                    
-                <group name="FIRMWARE">
-                    <param name="major">            0                   </param>    
-                    <param name="minor">            0                   </param> 
-                    <param name="build">            0                   </param>
-                </group>
-            </group>
-            
-            <group name="MAIS"> 
-                <param name="location">             CAN1:14             </param> 
-            </group>                      
-            
-
-            <group name="JOINTMAPPING">
-
-                <group name="ACTUATOR">                 
-                    <param name="type">             pwm                 pwm                 pwm                 pwm                     </param>
-                    <param name="port">             CONN:P3             CONN:P2             CONN:P4             CONN:P5                 </param>
-                </group>
-                
-                <group name="ENCODER1">  
-                    <param name="type">             absanalog           mais                mais                mais                    </param>  
-                    <param name="port">             CONN:P3             MAIS:thumbproximal  MAIS:thumbdistal    MAIS:indexproximal      </param>
-                    <param name="position">         atjoint             atjoint             atjoint             atjoint                 </param> 
-                    <param name="resolution">        65535              65535               65535               65535           </param> <!-- 65535:this value will change by fw during calibration (type 6 or 7) --> 
-                    <param name="tolerance">         0                0                   0                  0                        </param>  
-                </group>        
-                
-                <group name="ENCODER2">
-                    <param name="type">             none                qenc                qenc                qenc                    </param>
-                    <param name="port">             CONN:none           CONN:P2             CONN:P4             CONN:P5                 </param>
-                    <param name="position">         none                atmotor             atmotor             atmotor                 </param>
-                    <param name="resolution">          1                40                  40                  40                      </param>
-                    <param name="tolerance">           0                0                   0                  0                        </param>  
-                </group>  
-    
-            </group>    
-            
-        </group>
-           
-    </group>
-    
-  
-</params>
-
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb29-j12_15-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb29-j12_15-mc.xml
deleted file mode 100644
index 690ec8b1c1..0000000000
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb29-j12_15-mc.xml
+++ /dev/null
@@ -1,63 +0,0 @@
-<?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="right_arm-eb29-j12_15-mc" type="embObjMotionControl">
-
-    <xi:include href="../../general.xml" />
-    <xi:include href="../../hardware/electronics/right_arm-eb29-j12_15-eln.xml" />
-    <xi:include href="../../hardware/mechanicals/right_arm-eb29-j12_15-mec.xml" />   
-    <xi:include href="./right_arm-eb29-j12_15-mc_service.xml" />
-
-    <!-- joint number                           0                   1                   2                   3               -->
-    <!-- joint name                             index_distal        middle-proximal     middle-distal       little-fingers  -->     
-    <group name="LIMITS">
-        <param name="jntPosMax">                180                 90                  180                 270             </param>
-        <param name="jntPosMin">                0                   0                   0                   0               </param> 
-        <param name="jntVelMax">                1000                1000                1000                1000            </param>
-        <param name="motorOverloadCurrents">    2000                2000                2000                2000            </param>
-        <param name="motorNominalCurrents">     600                 600                 600                 600             </param>
-        <param name="motorPeakCurrents">        1000                1000                1000                1000            </param>
-        <param name="motorPwmLimit">            3360                3360                3360                3360            </param>
-    </group>
-
-    <group name="TIMEOUTS">
-        <param name="velocity">                 100                 100                 100                 100	                </param>
-    </group>
-
-    <group name="IMPEDANCE">
-        <param name="stiffness">                0.0                 0.0                 0.0                 0.0                 </param>
-        <param name="damping">                  0.0                 0.0                 0.0                 0.0                 </param>
-    </group>
-
-    <group name="CONTROLS">
-        <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-        <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-        <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     POS_PID_DEFAULT     </param> 
-        <param name="torqueControl">            none                none                none                none                </param> 
-        <param name="currentPid">               none                none                none                none                </param>
-        <param name="speedPid">                 none                none                none                none                </param> 
-    </group>
-
-    <group name="POS_PID_DEFAULT">
-        <param name="controlLaw">             minjerk                   </param>
-        <param name="outputType">             pwm                       </param>
-        <param name="fbkControlUnits">        metric_units              </param> 
-        <param name="outputControlUnits">     machine_units             </param>
-        <param name="kp">                500         500         -500        200         </param>
-        <param name="kd">                0           0           0           0           </param>
-        <param name="ki">                50          50          -50         20          </param>
-        <param name="maxOutput">         3360        3360        3360        3360        </param>
-        <param name="maxInt">            3360        3360        3360        3360        </param>
-        <param name="stictionUp">        0           0           0           0           </param>
-        <param name="stictionDown">      0           0           0           0           </param>
-        <param name="kff">               0           0           0           0           </param>
-    </group>
-
-    <!-- begin of reserved section. this section is reserved for custom PIDs -->
-    
-    <!-- end of reserved section. this section is reserved for custom PIDs -->
-    
-</device> 
-    
-
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb29-j12_15-mc_service.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb29-j12_15-mc_service.xml
deleted file mode 100644
index 55870a78c3..0000000000
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb29-j12_15-mc_service.xml
+++ /dev/null
@@ -1,65 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" ?>
-<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
-
-<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="ergoCubSN000" build="1">
-
-    <group name="SERVICE">
-        
-        <param name="type"> eomn_serv_MC_mc4plusmais </param> 
-    
-        <group name="PROPERTIES">
-        
-            <group name="ETHBOARD">
-                <param name="type">             mc4plus             </param> 
-            </group>          
-
-            <group name="CANBOARDS"> 
-                <param name="type">                 mais                </param> 
-                <group name="PROTOCOL">
-                    <param name="major">            0                   </param>    
-                    <param name="minor">            0                   </param>     
-                </group>                    
-                <group name="FIRMWARE">
-                    <param name="major">            0                   </param>    
-                    <param name="minor">            0                   </param> 
-                    <param name="build">            0                   </param>
-                </group>
-            </group>
-            
-            <group name="MAIS"> 
-                <param name="location">             CAN1:14             </param> 
-            </group>                      
-            
-           
-
-            <group name="JOINTMAPPING">
-
-                <group name="ACTUATOR">                 
-                    <param name="type">             pwm                 pwm                 pwm                 pwm                     </param>
-                    <param name="port">             CONN:P3             CONN:P2             CONN:P4             CONN:P5                 </param>
-                </group>
-                
-                <group name="ENCODER1">  
-                    <param name="type">             mais                mais                mais                mais                    </param>  
-                    <param name="port">             MAIS:indexdistal    MAIS:mediumproximal MAIS:mediumdistal   MAIS:littlefingers      </param>
-                    <param name="position">         atjoint             atjoint             atjoint             atjoint                 </param>
-                    <param name="resolution">       65535               65535               65535               65535                   </param> <!-- 65535:this value will change by fw during calibration (type 6) --> 
-                    <param name="tolerance">        0                   0                   0                   0                       </param>  
-                </group>        
-                
-                <group name="ENCODER2">
-                    <param name="type">             qenc                qenc                qenc                qenc                    </param>
-                    <param name="port">             CONN:P3             CONN:P2             CONN:P4             CONN:P5                 </param>
-                    <param name="position">         atmotor             atmotor             atmotor             atmotor                 </param>
-                    <param name="resolution">       40                  40                  40                  40                      </param>
-                    <param name="tolerance">        0                   0                   0                   0                       </param>  
-                </group>  
-            </group>    
-            
-        </group>
-           
-    </group>
-    
-  
-</params>
-
diff --git a/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml b/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
index 03507af7ca..8697503acc 100644
--- a/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/right_arm-mc_remapper.xml
@@ -1,20 +1,20 @@
 <?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="rightt_arm-mc_remapper" type="controlboardremapper">
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-mc_remapper" type="controlboardremapper">
     <paramlist name="networks">
-        <elem name="rightt_arm_joints1">(  0  1 0 1 )</elem>
-        <elem name="rightt_arm_joints2">(  2  3 0 1 )</elem>
-        <elem name="rightt_arm_joints3">(  4  6 0 2 )</elem>
-        <elem name="rightt_arm_joints4">(  7  10 0 3 )</elem>
+        <elem name="right_arm_joints1">(  0  1 0 1 )</elem>
+        <elem name="right_arm_joints2">(  2  3 0 1 )</elem>
+        <elem name="right_arm_joints3">(  4  6 0 2 )</elem>
+        <elem name="right_arm_joints4">(  7  10 0 3 )</elem>
     </paramlist>
     <param name="joints"> 11 </param>
     <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
-            <elem name="rightt_arm_joints1"> rightt_arm-eb2-j0_1-mc </elem>
-            <elem name="rightt_arm_joints2"> rightt_arm-eb4-j2_3-mc </elem>
-            <elem name="rightt_arm_joints3"> rightt_arm-eb31-j4_6-mc </elem>
-            <elem name="rightt_arm_joints4"> rightt_arm-eb22-j7_10-mc </elem>
+            <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_joints4"> right_arm-eb22-j7_10-mc </elem>
         </paramlist>
     </action>
     <action phase="shutdown" level="20" type="detach" />
diff --git a/ergoCubSN000/wrappers/motorControl/right_arm-mc_wrapper.xml b/ergoCubSN000/wrappers/motorControl/right_arm-mc_wrapper.xml
index 76155af830..f875e68eab 100644
--- a/ergoCubSN000/wrappers/motorControl/right_arm-mc_wrapper.xml
+++ b/ergoCubSN000/wrappers/motorControl/right_arm-mc_wrapper.xml
@@ -1,11 +1,11 @@
 <?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="left_arm-mc_nws_yarp" type="controlBoard_nws_yarp">
+<device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-mc_nws_yarp" type="controlBoard_nws_yarp">
     <param name="period"> 0.01  </param>
-    <param name="name"> /ergocub/left_arm </param>
+    <param name="name"> /ergocub/right_arm </param>
     <action phase="startup" level="10" type="attach">
-        <param name="device"> left_arm-mc_remapper </param>
+        <param name="device"> right_arm-mc_remapper </param>
     </action>
     <action phase="shutdown" level="15" type="detach" />
 </device>

From 2ab5fa09d57f59b1b37d0cfa8866f1769af63890 Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Wed, 23 Nov 2022 14:29:04 +0100
Subject: [PATCH 9/9] ergoCubSN000: latest fixes before departing for Rome

---
 ergoCubSN000/calibrators/left_arm-calib.xml  | 6 +++---
 ergoCubSN000/calibrators/left_leg-calib.xml  | 4 ++--
 ergoCubSN000/calibrators/right_arm-calib.xml | 2 +-
 ergoCubSN000/calibrators/right_leg-calib.xml | 4 ++--
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/ergoCubSN000/calibrators/left_arm-calib.xml b/ergoCubSN000/calibrators/left_arm-calib.xml
index 23937b9db0..18e78c89df 100644
--- a/ergoCubSN000/calibrators/left_arm-calib.xml
+++ b/ergoCubSN000/calibrators/left_arm-calib.xml
@@ -16,7 +16,7 @@
 </group>
 <group name="CALIBRATION">
 <param name="calibrationType">        12         12         12         12         12       12      12        5          5      5      5       </param>
-<param name="calibration1">           -41179     7782       11224      25717      10816   7369    21632      0        500    500     500      </param>
+<param name="calibration1">           -41179     7782       11224      25717      10816   7369    21632      500        500    500     500      </param>
 <param name="calibration2">	      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        </param>
 <param name="calibration4">           0           0           0          0           0       0       0       0           0       0       0        </param>
@@ -25,11 +25,11 @@
 <param name="calibrationDelta">       0           0           0          0           0       0       0       0           0       0       0        </param>
 <param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0       10.0        10.0    10.0    10.0 </param>
 <param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0      70.0        70.0    70.0    70.0 </param>
-<param name="startupMaxPwm">          2000       2000         2000      2000       16000   16000   16000     0           1000    1000       0    </param>
+<param name="startupMaxPwm">          4000       4000         2000      2000       16000   16000   16000     0           1000    1000       0    </param>
 <param name="startupPosThreshold">    2          2            2          2         2       2       2         50          50      50      50   </param>
 </group>
 
-<param name="CALIB_ORDER">(1) (0) (2) (3) (4 5 6) (7 8 9 10)</param> <!-- joint 7 is not working (demo Rome 22) -->
+<param name="CALIB_ORDER">(1) (0) (2) (3) (4 5 6) (8 9 10)</param> <!-- joint 7 is not working (demo Rome 22) -->
 		<action phase="startup" level="10" type="calibrate">
 		    <param name="target">left_arm-mc_remapper</param>
 		</action>
diff --git a/ergoCubSN000/calibrators/left_leg-calib.xml b/ergoCubSN000/calibrators/left_leg-calib.xml
index 738e65b328..3d55823549 100644
--- a/ergoCubSN000/calibrators/left_leg-calib.xml
+++ b/ergoCubSN000/calibrators/left_leg-calib.xml
@@ -9,7 +9,7 @@
   </group>
   
   <group name="HOME">
-    <param name="positionHome">                       0.00           5.00       0.00        -5.00        0.00      0.00        </param>
+    <param name="positionHome">                       0.00           5.00       0.00          0.00        0.00      0.00        </param>
     <param name="velocityHome">                       10.00          10.00      10.00        10.00       10.00     10.00       </param>
   </group>
 
@@ -25,7 +25,7 @@
    <param name="calibrationZero">                     0              0            0             0          0       0        </param>
     <param name="calibrationDelta">                   0              0            0             0          0       0        </param>
     
-    <param name="startupPosition">                    0               5           0            -5          0       0        </param>
+    <param name="startupPosition">                    0               5           0             0          0       0        </param>
     <param name="startupVelocity">                    10.0            10.0        10.0          10         10      10       </param>
     <param name="startupMaxPwm">                      2000           2000        2000          4000       2000    2000     </param>
     <param name="startupPosThreshold">                2               2           2             2          2       2        </param>
diff --git a/ergoCubSN000/calibrators/right_arm-calib.xml b/ergoCubSN000/calibrators/right_arm-calib.xml
index a3f6e3a284..509ef00392 100644
--- a/ergoCubSN000/calibrators/right_arm-calib.xml
+++ b/ergoCubSN000/calibrators/right_arm-calib.xml
@@ -25,7 +25,7 @@
 <param name="calibrationDelta">       0           0           0          0           0       0       0      0           0       0       0     </param>
 <param name="startupPosition">        -35        30           0         45         0.0     0.0     0.0      10.0        10.0    10.0    10.0    </param>
 <param name="startupVelocity">        10.0       10.0         10.0      10.0       10.0    10.0    10.0     70.0        70.0    70.0    70.0    </param>
-<param name="startupMaxPwm">          2000       2000         2000      2000       16000   16000   16000    0           0       0       0     </param>
+<param name="startupMaxPwm">          4000       4000         2000      2000       16000   16000   16000    0           0       0       0     </param>
 <param name="startupPosThreshold">    2          2            2          2         2       2       2        10          10      10      10      </param>
 </group>
 
diff --git a/ergoCubSN000/calibrators/right_leg-calib.xml b/ergoCubSN000/calibrators/right_leg-calib.xml
index ec68eccd57..aec6c7908d 100644
--- a/ergoCubSN000/calibrators/right_leg-calib.xml
+++ b/ergoCubSN000/calibrators/right_leg-calib.xml
@@ -9,7 +9,7 @@
   </group>
 
   <group name="HOME">
-    <param name="positionHome">                       0.00             5.00        0.00     -5.00           0.00          0.00        </param>
+    <param name="positionHome">                       0.00             5.00        0.00      0.00           0.00          0.00        </param>
     <param name="velocityHome">                       10.00           10.00       10.00     10.00          10.00         10.00        </param>
   </group>
 
@@ -25,7 +25,7 @@
     <param name="calibrationZero">                    0              0            0             0          0       0        </param>
     <param name="calibrationDelta">                   0              0            0             0          0       0        </param>
     
-    <param name="startupPosition">                    0               5           0            -5          0       0        </param>
+    <param name="startupPosition">                    0               5           0             0          0       0        </param>
     <param name="startupVelocity">                    10.0            10.0        10.0          10         10      10       </param>
     <param name="startupMaxPwm">                      2000           2000       2000          4000       2000    2000     </param>
     <param name="startupPosThreshold">                2               2           2             2          2       2        </param>