From 17165c5067b323683f90240d7510f9a991e1843f Mon Sep 17 00:00:00 2001
From: Lorenzo <lor.rapetti@gmail.com>
Date: Thu, 16 Feb 2023 18:53:12 +0100
Subject: [PATCH 1/8] [ergoCubSN000] Update leg torque control gains

---
 .../motorControl/left_leg-eb8-j0_3-mc.xml     |  8 +++----
 .../motorControl/left_leg-eb9-j4_5-mc.xml     | 14 +++++------
 .../motorControl/right_leg-eb6-j0_3-mc.xml    |  8 +++----
 .../motorControl/right_leg-eb7-j4_5-mc.xml    | 24 +++++++++----------
 4 files changed, 27 insertions(+), 27 deletions(-)

diff --git a/ergoCubSN000/hardware/motorControl/left_leg-eb8-j0_3-mc.xml b/ergoCubSN000/hardware/motorControl/left_leg-eb8-j0_3-mc.xml
index aa547f2ef7..3181802275 100644
--- a/ergoCubSN000/hardware/motorControl/left_leg-eb8-j0_3-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/left_leg-eb8-j0_3-mc.xml
@@ -117,18 +117,18 @@
         <param name="outputType">          current                      </param>
         <param name="fbkControlUnits">     metric_units                 </param>
         <param name="outputControlUnits">  machine_units                </param>
-        <param name="kp">            -0.0        0.0      -0.0      -0.0       </param>
+        <param name="kp">            -100        150      -0.0      -350       </param>
         <param name="kd">             0          0         0         0         </param>
-        <param name="ki">            -0          0        -0        -0         </param>
+        <param name="ki">            -10         25        -0       -100       </param>
         <param name="maxOutput">      2500       2500      2500      2500      </param>
         <param name="maxInt">         200        200       200       200       </param>
         <param name="ko">             0          0         0         0         </param>
         <param name="stictionUp">     0          0         0         0         </param>
         <param name="stictionDown">   0          0         0         0         </param>
         <param name="kff">            1          1         1         1         </param>
-        <param name="kbemf">         -0.0        0.0      -0.0      -0.0       </param>
+        <param name="kbemf">         -0.0001     0.0001   -0.0005   -0.0003    </param>
         <param name="filterType">     0          0         0         0         </param>
-        <param name="ktau">          -50         50       -50       -50        </param>
+        <param name="ktau">          -40         50       -150      -110       </param>
     </group>
 
     <!-- custom PIDs: end -->
diff --git a/ergoCubSN000/hardware/motorControl/left_leg-eb9-j4_5-mc.xml b/ergoCubSN000/hardware/motorControl/left_leg-eb9-j4_5-mc.xml
index 37976f7877..c297db0e3f 100644
--- a/ergoCubSN000/hardware/motorControl/left_leg-eb9-j4_5-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/left_leg-eb9-j4_5-mc.xml
@@ -116,18 +116,18 @@
         <param name="outputType">          current                      </param>
         <param name="fbkControlUnits">     metric_units                 </param>
         <param name="outputControlUnits">  machine_units                </param>
-        <param name="kp">     -1000       -1000         </param>
-        <param name="kd">         0            0            </param>
-        <param name="ki">      -500         -1000            </param>
-        <param name="maxOutput">           2500        2500            </param>
-        <param name="maxInt">              200         200            </param>
+        <param name="kp">                 -0          -50          </param>
+        <param name="kd">                  0           0            </param>
+        <param name="ki">                 -0           0            </param>
+        <param name="maxOutput">           2500        2500         </param>
+        <param name="maxInt">              200         200          </param>
         <param name="ko">                  0           0            </param>
         <param name="stictionUp">          0           0            </param>
         <param name="stictionDown">        0           0            </param>
         <param name="kff">                 1           1            </param>
-        <param name="kbemf">           -0.0009     -0.0003       </param>
+        <param name="kbemf">              -0.0009     -0.0003       </param>
         <param name="filterType">          0           0            </param>
-        <param name="ktau">           -128.81    -227.739         </param>
+        <param name="ktau">               -128.81     -160          </param>
     </group>
     <!-- custom PIDs: end -->
 
diff --git a/ergoCubSN000/hardware/motorControl/right_leg-eb6-j0_3-mc.xml b/ergoCubSN000/hardware/motorControl/right_leg-eb6-j0_3-mc.xml
index d3dc685e7a..73d8678881 100644
--- a/ergoCubSN000/hardware/motorControl/right_leg-eb6-j0_3-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/right_leg-eb6-j0_3-mc.xml
@@ -117,18 +117,18 @@
         <param name="outputType">          current                      </param>
         <param name="fbkControlUnits">     metric_units                 </param>
         <param name="outputControlUnits">  machine_units                </param>
-        <param name="kp">             0.0       -0.0       0.0       0.0       </param>
+        <param name="kp">             100       -150       0.0       350       </param>
         <param name="kd">             0          0         0         0         </param>
-        <param name="ki">             0         -0         0         0         </param>
+        <param name="ki">             10        -25        0         100       </param>
         <param name="maxOutput">      2500       2500      2500      2500      </param>
         <param name="maxInt">         200        200       200       200       </param>
         <param name="ko">             0          0         0         0         </param>
         <param name="stictionUp">     0          0         0         0         </param>
         <param name="stictionDown">   0          0         0         0         </param>
         <param name="kff">            1          1         1         1         </param>
-        <param name="kbemf">          0.0       -0.0       0.0       0.0       </param>
+        <param name="kbemf">          0.0001    -0.0001    0.0005    0.0003    </param>
         <param name="filterType">     0          0         0         0         </param>
-        <param name="ktau">           50        -50        50        50        </param>
+        <param name="ktau">           40        -50        150       110       </param>
     </group>
 
     <!-- custom PIDs: end -->
diff --git a/ergoCubSN000/hardware/motorControl/right_leg-eb7-j4_5-mc.xml b/ergoCubSN000/hardware/motorControl/right_leg-eb7-j4_5-mc.xml
index 6be2ea8ce5..763263dfd3 100644
--- a/ergoCubSN000/hardware/motorControl/right_leg-eb7-j4_5-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/right_leg-eb7-j4_5-mc.xml
@@ -116,18 +116,18 @@
         <param name="outputType">          current                      </param>
         <param name="fbkControlUnits">     metric_units                 </param>
         <param name="outputControlUnits">  machine_units                </param>
-        <param name="kp">              1000       1000      </param>
-        <param name="kd">              0          0      </param>
-        <param name="ki">              500        1000    </param>
-        <param name="maxOutput">        2500       2500        </param>
-        <param name="maxInt">            200        200       </param>
-        <param name="ko">                  0          0     </param>
-        <param name="stictionUp">          0          0     </param>
-        <param name="stictionDown">           0          0     </param>
-        <param name="kff">                    1          1     </param>
-        <param name="kbemf">               0.0009     0.0003    </param>
-        <param name="filterType">            0          0     </param>
-        <param name="ktau">             128.81     227.739     </param>
+        <param name="kp">                 0          50            </param>
+        <param name="kd">                 0          0            </param>
+        <param name="ki">                 0          0            </param>
+        <param name="maxOutput">          2500       2500         </param>
+        <param name="maxInt">             200        200          </param>
+        <param name="ko">                 0          0            </param>
+        <param name="stictionUp">         0          0            </param>
+        <param name="stictionDown">       0          0            </param>
+        <param name="kff">                1          1            </param>
+        <param name="kbemf">              0.0009     0.0003       </param>
+        <param name="filterType">         0          0            </param>
+        <param name="ktau">               128.81     160          </param>
     </group>
     <!-- custom PIDs: end -->
 

From 83981bb857c5f166c7b1a701a00b226640ba77e5 Mon Sep 17 00:00:00 2001
From: Lorenzo <lor.rapetti@gmail.com>
Date: Fri, 24 Feb 2023 14:48:17 +0100
Subject: [PATCH 2/8] [ergoCubSN000] Enable torque control via current on arms

---
 .../motorControl/left_arm-eb2-j0_1-mc.xml     | 33 ++++++++++++++++++-
 .../motorControl/left_arm-eb4-j2_3-mc.xml     | 33 ++++++++++++++++++-
 .../motorControl/right_arm-eb1-j0_1-mc.xml    | 33 ++++++++++++++++++-
 .../motorControl/right_arm-eb3-j2_3-mc.xml    | 33 ++++++++++++++++++-
 4 files changed, 128 insertions(+), 4 deletions(-)

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 72ca590815..3f16974819 100644
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb2-j0_1-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb2-j0_1-mc.xml
@@ -33,7 +33,7 @@
         <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
         <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
         <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-        <param name="torqueControl">            TRQ_PID_DEFAULT     TRQ_PID_DEFAULT     </param>
+        <param name="torqueControl">            TRQ_PID_OUTPUT_CURR     TRQ_PID_OUTPUT_CURR     </param>
         <param name="currentPid">               2FOC_CUR_CONTROL    2FOC_CUR_CONTROL    </param>
         <param name="speedPid">                 2FOC_VEL_CONTROL    2FOC_VEL_CONTROL    </param>
     </group>
@@ -112,12 +112,43 @@
 
 
     <!-- custom PIDs: begin -->
+    <group name="TRQ_PID_OUTPUT_CURR">
+        <param name="controlLaw">          torque                       </param>
+        <param name="outputType">          current                      </param>
+        <param name="fbkControlUnits">     metric_units                 </param>
+        <param name="outputControlUnits">  machine_units                </param>
+        <param name="kp">             150        100       </param>
+        <param name="kd">             0          0         </param>
+        <param name="ki">             0          0         </param>
+        <param name="maxOutput">      2500       2500      </param>
+        <param name="maxInt">         200        200       </param>
+        <param name="ko">             0          0         </param>
+        <param name="stictionUp">     0          0         </param>
+        <param name="stictionDown">   0          0         </param>
+        <param name="kff">            1          1         </param>
+        <param name="kbemf">          0.0001     0.0001    </param>
+        <param name="filterType">     0          0         </param>
+        <param name="ktau">           250        200       </param>
+    </group>
     <!-- custom PIDs: end -->
 
     <group name="OTHER_CONTROL_PARAMETERS">
         <param name="deadZone">                 0.0049              0.0049              </param>
     </group>
 
+    <!-- kalman filter parameters - vel & acc estimation -->
+    <group name="KALMAN_FILTER">
+        <param name="kalmanFilterEnabled">          1              1            </param>
+        <param name="x0">                           0              0            </param>
+        <param name="x1">                           0              0            </param>
+        <param name="x2">                           0              0            </param>
+        <param name="Q0">                           0.0001         0.0001       </param>
+        <param name="Q1">                           0.01           0.01         </param>
+        <param name="Q2">                           10             10           </param>
+        <param name="R">                            0.001          0.001        </param>
+        <param name="P0">                           0.000099       0.000099     </param>
+    </group>
+
 </device>
 
 
diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb4-j2_3-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb4-j2_3-mc.xml
index c23c29335d..f44dc56212 100644
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb4-j2_3-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb4-j2_3-mc.xml
@@ -33,7 +33,7 @@
         <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
         <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
         <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-        <param name="torqueControl">            TRQ_PID_DEFAULT     TRQ_PID_DEFAULT     </param>
+        <param name="torqueControl">            TRQ_PID_OUTPUT_CURR     TRQ_PID_OUTPUT_CURR     </param>
         <param name="currentPid">               2FOC_CUR_CONTROL    2FOC_CUR_CONTROL    </param>
         <param name="speedPid">                 2FOC_VEL_CONTROL    2FOC_VEL_CONTROL    </param>
     </group>
@@ -112,12 +112,43 @@
 
 
     <!-- custom PIDs: begin -->
+    <group name="TRQ_PID_OUTPUT_CURR">
+        <param name="controlLaw">          torque                       </param>
+        <param name="outputType">          current                      </param>
+        <param name="fbkControlUnits">     metric_units                 </param>
+        <param name="outputControlUnits">  machine_units                </param>
+        <param name="kp">             0         -200       </param>
+        <param name="kd">             0          0         </param>
+        <param name="ki">             0          0         </param>
+        <param name="maxOutput">      2500       2500      </param>
+        <param name="maxInt">         200        200       </param>
+        <param name="ko">             0          0         </param>
+        <param name="stictionUp">     0          0         </param>
+        <param name="stictionDown">   0          0         </param>
+        <param name="kff">            1          1         </param>
+        <param name="kbemf">          0.0001    -0.0002    </param>
+        <param name="filterType">     0          0         </param>
+        <param name="ktau">           50        -300       </param>
+    </group>
     <!-- custom PIDs: end -->
 
     <group name="OTHER_CONTROL_PARAMETERS">
         <param name="deadZone">                 0.0049              0.0049              </param>
     </group>
 
+    <!-- kalman filter parameters - vel & acc estimation -->
+    <group name="KALMAN_FILTER">
+        <param name="kalmanFilterEnabled">          1              1            </param>
+        <param name="x0">                           0              0            </param>
+        <param name="x1">                           0              0            </param>
+        <param name="x2">                           0              0            </param>
+        <param name="Q0">                           0.0001         0.0001       </param>
+        <param name="Q1">                           0.01           0.01         </param>
+        <param name="Q2">                           10             10           </param>
+        <param name="R">                            0.001          0.001        </param>
+        <param name="P0">                           0.000099       0.000099     </param>
+    </group>
+
 </device>
 
 
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 6f6b53e96b..e84f207d7b 100644
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb1-j0_1-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb1-j0_1-mc.xml
@@ -34,7 +34,7 @@
         <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
         <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
         <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-        <param name="torqueControl">            TRQ_PID_DEFAULT     TRQ_PID_DEFAULT     </param>
+        <param name="torqueControl">            TRQ_PID_OUTPUT_CURR     TRQ_PID_OUTPUT_CURR     </param>
         <param name="currentPid">               2FOC_CUR_CONTROL    2FOC_CUR_CONTROL    </param>
         <param name="speedPid">                 2FOC_VEL_CONTROL    2FOC_VEL_CONTROL    </param>
     </group>
@@ -114,12 +114,43 @@
 
 
     <!-- custom PIDs: begin -->
+    <group name="TRQ_PID_OUTPUT_CURR">
+        <param name="controlLaw">          torque                       </param>
+        <param name="outputType">          current                      </param>
+        <param name="fbkControlUnits">     metric_units                 </param>
+        <param name="outputControlUnits">  machine_units                </param>
+        <param name="kp">            -150       -100       </param>
+        <param name="kd">             0          0         </param>
+        <param name="ki">             0          0         </param>
+        <param name="maxOutput">      2500       2500      </param>
+        <param name="maxInt">         200        200       </param>
+        <param name="ko">             0          0         </param>
+        <param name="stictionUp">     0          0         </param>
+        <param name="stictionDown">   0          0         </param>
+        <param name="kff">            1          1         </param>
+        <param name="kbemf">         -0.0001    -0.0001    </param>
+        <param name="filterType">     0          0         </param>
+        <param name="ktau">          -250       -200       </param>
+    </group>
     <!-- custom PIDs: end -->
 
     <group name="OTHER_CONTROL_PARAMETERS">
         <param name="deadZone">                 0.0049              0.0049              </param>
     </group>
 
+    <!-- kalman filter parameters - vel & acc estimation -->
+    <group name="KALMAN_FILTER">
+        <param name="kalmanFilterEnabled">          1              1            </param>
+        <param name="x0">                           0              0            </param>
+        <param name="x1">                           0              0            </param>
+        <param name="x2">                           0              0            </param>
+        <param name="Q0">                           0.0001         0.0001       </param>
+        <param name="Q1">                           0.01           0.01         </param>
+        <param name="Q2">                           10             10           </param>
+        <param name="R">                            0.001          0.001        </param>
+        <param name="P0">                           0.000099       0.000099     </param>
+    </group>
+
   </device>
 
 
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb3-j2_3-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb3-j2_3-mc.xml
index 6a1d37cf98..2b511d7bac 100644
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb3-j2_3-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb3-j2_3-mc.xml
@@ -33,7 +33,7 @@
         <param name="positionControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
         <param name="velocityControl">          POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
         <param name="mixedControl">             POS_PID_DEFAULT     POS_PID_DEFAULT     </param>
-        <param name="torqueControl">            TRQ_PID_DEFAULT     TRQ_PID_DEFAULT     </param>
+        <param name="torqueControl">            TRQ_PID_OUTPUT_CURR     TRQ_PID_OUTPUT_CURR     </param>
         <param name="currentPid">               2FOC_CUR_CONTROL    2FOC_CUR_CONTROL    </param>
         <param name="speedPid">                 2FOC_VEL_CONTROL    2FOC_VEL_CONTROL    </param>
     </group>
@@ -114,12 +114,43 @@
 
 
     <!-- custom PIDs: begin -->
+    <group name="TRQ_PID_OUTPUT_CURR">
+        <param name="controlLaw">          torque                       </param>
+        <param name="outputType">          current                      </param>
+        <param name="fbkControlUnits">     metric_units                 </param>
+        <param name="outputControlUnits">  machine_units                </param>
+        <param name="kp">             0          200       </param>
+        <param name="kd">             0          0         </param>
+        <param name="ki">             0          0         </param>
+        <param name="maxOutput">      2500       2500      </param>
+        <param name="maxInt">         200        200       </param>
+        <param name="ko">             0          0         </param>
+        <param name="stictionUp">     0          0         </param>
+        <param name="stictionDown">   0          0         </param>
+        <param name="kff">            1          1         </param>
+        <param name="kbemf">         -0.0001     0.0002    </param>
+        <param name="filterType">     0          0         </param>
+        <param name="ktau">          -50         300       </param>
+    </group>
     <!-- custom PIDs: end -->
 
     <group name="OTHER_CONTROL_PARAMETERS">
         <param name="deadZone">                 0.0049              0.0049              </param>
     </group>
 
+    <!-- kalman filter parameters - vel & acc estimation -->
+    <group name="KALMAN_FILTER">
+        <param name="kalmanFilterEnabled">          1              1            </param>
+        <param name="x0">                           0              0            </param>
+        <param name="x1">                           0              0            </param>
+        <param name="x2">                           0              0            </param>
+        <param name="Q0">                           0.0001         0.0001       </param>
+        <param name="Q1">                           0.01           0.01         </param>
+        <param name="Q2">                           10             10           </param>
+        <param name="R">                            0.001          0.001        </param>
+        <param name="P0">                           0.000099       0.000099     </param>
+    </group>
+
   </device>
 
 

From d63c343b45542819f91ecf47ce4f355e241e28de Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Wed, 1 Mar 2023 11:48:26 +0100
Subject: [PATCH 3/8] [ergoCubSN000] Update the cluster config file

---
 .../contexts/yarpmanager/cluster-config.xml   | 26 +++++++++----------
 1 file changed, 12 insertions(+), 14 deletions(-)

diff --git a/ergoCubSN000/extra/contexts/yarpmanager/cluster-config.xml b/ergoCubSN000/extra/contexts/yarpmanager/cluster-config.xml
index 40d6e6265a..186363f61b 100644
--- a/ergoCubSN000/extra/contexts/yarpmanager/cluster-config.xml
+++ b/ergoCubSN000/extra/contexts/yarpmanager/cluster-config.xml
@@ -1,17 +1,15 @@
-<cluster name="icub09" user="icub">
-
-<!-- specify type="yarpserver" or type="yarpserver3" to use the old or new nameserver default behavior is to use yarpserver3 -->
-<nameserver namespace="/icub09" node="icub-console" type="yarpserver"> </nameserver>
-
-<!-- specify nodes, if you want to enable display on desktop please specify which desktop to use
-this is usually the value of the DISPLAY variable when logged on -->
-<node display=":0" address="10.0.0.158">icub-console</node>
-<node display=":0" address="10.0.10.164">icub-console-gui</node>
-<node address="10.0.10.115" user="pi">pi-ifeel</node>
-<node display=":1">icub-cuda</node>
-<node address="10.0.0.2">icub-head</node>
-<node>icub-virtualizer</node>
-<node display=":1" address="10.0.10.29">icub29</node>
+<cluster name="ergocub00" user="ergocub">
+
+  <!-- specify type="yarpserver" or type="yarpserver3" to use the old or new nameserver default behavior is to use yarpserver3 -->
+  <nameserver namespace="/ergocub00" node="ergocub-torso" type="yarpserver"> </nameserver>
+
+  <!-- specify nodes, if you want to enable display on desktop please specify which desktop to use
+       this is usually the value of the DISPLAY variable when logged on -->
+  <node display=":0" address="10.0.2.1">ergocub-laptop</node>
+  <node address="10.0.2.2">ergocub-torso</node>
+  <node display=":1" address="10.0.2.3">ergocub-head</node>
+  <node display=":0" address="10.0.10.164" user="icub">icub-console-gui</node>
+  <node>icub-virtualizer</node>
 
 </cluster>
 

From 2bb0a96e121ff16ddb2a2e44262382eebb2d4876 Mon Sep 17 00:00:00 2001
From: mebbaid <mohamed.elobaid@iit.it>
Date: Thu, 2 Mar 2023 15:11:46 +0000
Subject: [PATCH 4/8] updated current limits and head PID gains

---
 ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml | 6 +++---
 ergoCubSN000/hardware/motorControl/torso-eb5-j0_2-mc.xml | 8 ++++----
 2 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml b/ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml
index 7f4acc0d5e..7f4edb9973 100644
--- a/ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml
@@ -47,9 +47,9 @@
         <param name="outputType">               pwm             </param>
         <param name="fbkControlUnits">          metric_units    </param>
         <param name="outputControlUnits">       machine_units   </param>
-        <param name="kp">                       -512        +535        </param>
-        <param name="kd">                       -17         +14         </param>
-        <param name="ki">                       -1621       +1670       </param>
+        <param name="kp">                       -200        +200        </param>
+        <param name="kd">                       0           0         </param>
+        <param name="ki">                       -20         +20       </param>
         <param name="maxOutput">                3360        3360        </param>
         <param name="maxInt">                   3360        3360        </param>
         <param name="stictionUp">               0           0           </param>
diff --git a/ergoCubSN000/hardware/motorControl/torso-eb5-j0_2-mc.xml b/ergoCubSN000/hardware/motorControl/torso-eb5-j0_2-mc.xml
index 9aa976cac6..d9c4e6683f 100644
--- a/ergoCubSN000/hardware/motorControl/torso-eb5-j0_2-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/torso-eb5-j0_2-mc.xml
@@ -16,10 +16,10 @@
         <param name="jntPosMax">                23                  45                  43                  </param>
         <param name="jntPosMin">                -23                 -18                 -43                 </param>
         <param name="jntVelMax">                120                 120                 120                 </param>
-        <param name="motorNominalCurrents">     5000                8000                5000                </param>
-        <param name="motorPeakCurrents">        10000               16000               10000                </param>
-        <param name="motorOverloadCurrents">    15000               20000               15000               </param>
-        <param name="motorPwmLimit">            10000               10000               10000               </param>
+        <param name="motorNominalCurrents">     5000                15000               5000                </param>
+        <param name="motorPeakCurrents">        10000               20000               10000                </param>
+        <param name="motorOverloadCurrents">    15000               30000               15000               </param>
+        <param name="motorPwmLimit">            10000               16000               10000               </param>
     </group>
 
     <group name="TIMEOUTS">

From 72579481f145336d79cb80dd13a154bd059dcfa7 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Mon, 6 Mar 2023 16:26:22 +0000
Subject: [PATCH 5/8] [ergoCubSN000] Decrease the PID gains of the head

---
 ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml b/ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml
index 7f4edb9973..1cb1d83ae3 100644
--- a/ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/head-eb20-j0_1-mc.xml
@@ -47,9 +47,9 @@
         <param name="outputType">               pwm             </param>
         <param name="fbkControlUnits">          metric_units    </param>
         <param name="outputControlUnits">       machine_units   </param>
-        <param name="kp">                       -200        +200        </param>
+        <param name="kp">                       -150        +150        </param>
         <param name="kd">                       0           0         </param>
-        <param name="ki">                       -20         +20       </param>
+        <param name="ki">                       -15         +15       </param>
         <param name="maxOutput">                3360        3360        </param>
         <param name="maxInt">                   3360        3360        </param>
         <param name="stictionUp">               0           0           </param>

From affadb08e1399f7a4431ef7d71213ea4c932687e Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Mon, 6 Mar 2023 16:26:47 +0000
Subject: [PATCH 6/8] [ergoCubSN000] Decrease the PID of the wrists

---
 .../hardware/motorControl/left_arm-eb31-j4_6-mc.xml  | 12 ++++++------
 .../hardware/motorControl/right_arm-eb30-j4_6-mc.xml | 12 ++++++------
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc.xml b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
index 7c60b84210..7382b37177 100644
--- a/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/left_arm-eb31-j4_6-mc.xml
@@ -15,9 +15,9 @@
         <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="motorNominalCurrents">     300                 300                 300                </param>
+        <param name="motorPeakCurrents">        360                 360                 360                </param>
+        <param name="motorOverloadCurrents">    450                 450                 450                </param>
         <param name="motorPwmLimit">            16000               16000               16000               </param>
     </group>
 
@@ -47,9 +47,9 @@
         <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="kp">          -50         -50         -50      </param>
+        <param name="kd">          -8          -8          -8      </param>
+        <param name="ki">          -220        -220        -220     </param>
         <param name="maxOutput">    500         500         500     </param>
         <param name="maxInt">       200         200         200     </param>
         <param name="stictionUp">   0           0           0       </param>
diff --git a/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc.xml b/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
index 16dfdb0a36..e7ca2dcc53 100644
--- a/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
+++ b/ergoCubSN000/hardware/motorControl/right_arm-eb30-j4_6-mc.xml
@@ -15,9 +15,9 @@
         <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="motorNominalCurrents">     300                 300                 300                </param>
+        <param name="motorPeakCurrents">        300                 300                 300                </param>
+        <param name="motorOverloadCurrents">    360                 360                 360                </param>
         <param name="motorPwmLimit">            16000               16000               16000               </param>
     </group>
 
@@ -47,9 +47,9 @@
         <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="kp">          -50         -50         -50      </param>
+        <param name="kd">          -8          -8          -8       </param>
+        <param name="ki">          -220        -220        -220     </param>
         <param name="maxOutput">    500         500         500     </param>
         <param name="maxInt">       200         200         200     </param>
         <param name="stictionUp">   0           0           0       </param>

From f555aad3724b974da751faa6d675f55436847e18 Mon Sep 17 00:00:00 2001
From: Giulio Romualdi <giulio.romualdi@gmail.com>
Date: Mon, 6 Mar 2023 16:27:17 +0000
Subject: [PATCH 7/8] [ergoCubSN000] Update the calibrators of the wrists

---
 ergoCubSN000/calibrators/left_arm-calib.xml  | 2 +-
 ergoCubSN000/calibrators/right_arm-calib.xml | 5 +++--
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/ergoCubSN000/calibrators/left_arm-calib.xml b/ergoCubSN000/calibrators/left_arm-calib.xml
index 4b3f5185ab..03624bf540 100644
--- a/ergoCubSN000/calibrators/left_arm-calib.xml
+++ b/ergoCubSN000/calibrators/left_arm-calib.xml
@@ -30,7 +30,7 @@
 	</group>
 
 	<!-- motor's encoder of joint 7 can't read -->
-	<param name="CALIB_ORDER"> (4 5 6) (2) (0) (1) (3) (7 8 9 10) (11 12) </param> 
+	<param name="CALIB_ORDER"> (4 5 6) (3) (2) (0) (1) (7 8 9 10) (11 12) </param> 
 
 	<action phase="startup" level="10" type="calibrate">
 		<param name="target">left_arm-mc_remapper</param>
diff --git a/ergoCubSN000/calibrators/right_arm-calib.xml b/ergoCubSN000/calibrators/right_arm-calib.xml
index b08db65074..4e97a381d4 100644
--- a/ergoCubSN000/calibrators/right_arm-calib.xml
+++ b/ergoCubSN000/calibrators/right_arm-calib.xml
@@ -17,7 +17,7 @@
 	</group>
 	<group name="CALIBRATION">
 		<param name="calibrationType">         10                10             10          10         12            12             12              12      12      12      12      12      12     </param>
-		<param name="calibration1">           -4000              3000           3000       -4000       6109          28993          36030           0       0       0       0       0       0      </param>
+		<param name="calibration1">           -4000              3000           3000       -4000       19023          28993          36030           0       0       0       0       0       0      </param>
 		<param name="calibration2">	           0                 0              0           0          0             0              0               0       0       0       0       0       0      </param>
 		<param name="calibration3">            0                 0              0           0          0             0              0               0       0       0       0       0       0      </param>
 		<param name="calibration4">            0                 0              0           0          0             0              0               0       0       0       0       0       0      </param>
@@ -30,7 +30,8 @@
 		<param name="startupPosThreshold">     2                 2              2           2          2             2              2               90      90      90      90      90      90     </param>
 	</group>
 
-	<param name="CALIB_ORDER">  (4 5 6) (2) (0) (1) (3) (7 8 9 10) (11 12) </param> 
+<!--	<param name="CALIB_ORDER"> (4 5 6)  (2) (0) (1) (3) (7 8) (11 12) </param> --> 
+	<param name="CALIB_ORDER"> (3) (2) (0) (1) (7 8) (11 12) </param>
 
 	<action phase="startup" level="10" type="calibrate">
 		<param name="target">right_arm-mc_remapper</param>

From e2aeec4d94c4fa8f732880c32abe20a2cdd8c7e6 Mon Sep 17 00:00:00 2001
From: ergocub <ergocub@iit.it>
Date: Mon, 6 Mar 2023 17:58:34 +0100
Subject: [PATCH 8/8] Make sure that:

The realsense can be launched wihtout the lidar

The relsense streams using the right port names
---
 ergoCubSN000/realsense2.xml        | 10 ++++++
 ergoCubSN000/sensors/realsense.xml | 56 +++++++++++++++---------------
 2 files changed, 38 insertions(+), 28 deletions(-)
 create mode 100644 ergoCubSN000/realsense2.xml

diff --git a/ergoCubSN000/realsense2.xml b/ergoCubSN000/realsense2.xml
new file mode 100644
index 0000000000..6c7fc82c9f
--- /dev/null
+++ b/ergoCubSN000/realsense2.xml
@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="ergoCubSN000" build="1" portprefix="/ergocub" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+  <devices>
+    <xi:include href="sensors/realsense.xml" />
+  </devices>
+
+</robot>
diff --git a/ergoCubSN000/sensors/realsense.xml b/ergoCubSN000/sensors/realsense.xml
index 18fd952ada..2acc0c6b9c 100644
--- a/ergoCubSN000/sensors/realsense.xml
+++ b/ergoCubSN000/sensors/realsense.xml
@@ -1,35 +1,35 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
 
- <devices>
+<devices>
 
-    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="realsenseD450" type="realsense2">
-        <group name="SETTINGS">
-            <param name="depthResolution"> (640 480) </param>
-            <param name="rgbResolution">  (640 480) </param>
-            <param name="framerate">    30  </param>
-            <param name="enableEmitter"> true </param>
-            <param name="alignmentFrame"> RGB </param>
-        </group>
-        <group name="HW_DESCRIPTION">
-            <param name="clipPlanes"> (0.2 10.0) </param>
-        </group>
-        <group name="QUANT_PARAM">
-            <param name="depth_quant"> 2 </param>
-        </group>
-        <param name="rotateImage180"> false </param>
-    </device>
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="realsenseD450" type="realsense2">
+    <group name="SETTINGS">
+      <param name="depthResolution"> (640 480) </param>
+      <param name="rgbResolution">  (640 480) </param>
+      <param name="framerate">    30  </param>
+      <param name="enableEmitter"> true </param>
+      <param name="alignmentFrame"> RGB </param>
+    </group>
+    <group name="HW_DESCRIPTION">
+      <param name="clipPlanes"> (0.2 10.0) </param>
+    </group>
+    <group name="QUANT_PARAM">
+      <param name="depth_quant"> 2 </param>
+    </group>
+    <param name="rotateImage180"> false </param>
+  </device>
 
-    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="RGBDWrapperyarp" type="rgbdSensor_nws_yarp">
-        <param name="period"> 0.03 </param>
-        <param name="name">    /ergocub/depthCamera </param>
-        <action phase="startup" level="5" type="attach">
-            <paramlist name="networks">
-                <elem name="subdevicergbd"> realsenseD450 </elem>
-            </paramlist>
-        </action>
-        <action phase="shutdown" level="5" type="detach" />
-    </device>
+  <device xmlns:xi="http://www.w3.org/2001/XInclude" name="RGBDWrapperyarp" type="rgbdSensor_nws_yarp">
+    <param name="period"> 0.03 </param>
+    <param name="name">    /depthCamera </param>
+    <action phase="startup" level="5" type="attach">
+      <paramlist name="networks">
+        <elem name="subdevicergbd"> realsenseD450 </elem>
+      </paramlist>
+    </action>
+    <action phase="shutdown" level="5" type="detach" />
+  </device>
 
- </devices>
+</devices>