From 87ceb5ac3326751f47fe4bb4830a2926628a5741 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Tue, 13 Feb 2024 21:49:11 +0100
Subject: [PATCH 1/2] ergoCubSN00*: Fix name of waist_imu_0 sensor and
 imuFrameName in wbd files

---
 ergoCubSN000/estimators/wholebodydynamics.xml               | 4 ++--
 .../estimators/wholebodydynamics_no_right_forearm.xml       | 4 ++--
 ergoCubSN000/hardware/inertials/waist-inertial.xml          | 1 +
 ergoCubSN001/estimators/wholebodydynamics.xml               | 6 +++---
 ergoCubSN001/hardware/inertials/waist-inertial.xml          | 1 +
 5 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/ergoCubSN000/estimators/wholebodydynamics.xml b/ergoCubSN000/estimators/wholebodydynamics.xml
index 85038621db..e3c37c8c1b 100644
--- a/ergoCubSN000/estimators/wholebodydynamics.xml
+++ b/ergoCubSN000/estimators/wholebodydynamics.xml
@@ -22,8 +22,8 @@
         <param name="publishNetExternalWrenches">true</param>
 	<!--<param name="assume_fixed">root_link</param>-->
 	<group name="HW_USE_MAS_IMU">
-	    <param name="accelerometer">sensor_imu_xsensmt</param>
-	    <param name="gyroscope">sensor_imu_xsensmt</param>
+	    <param name="accelerometer">waist_imu_0</param>
+	    <param name="gyroscope">waist_imu_0</param>
 	</group>
 
         <group name="multipleAnalogSensorsNames">
diff --git a/ergoCubSN000/estimators/wholebodydynamics_no_right_forearm.xml b/ergoCubSN000/estimators/wholebodydynamics_no_right_forearm.xml
index 2cb7bc4e55..b1db666098 100644
--- a/ergoCubSN000/estimators/wholebodydynamics_no_right_forearm.xml
+++ b/ergoCubSN000/estimators/wholebodydynamics_no_right_forearm.xml
@@ -22,8 +22,8 @@
         <param name="publishNetExternalWrenches">true</param>
 	<!--<param name="assume_fixed">root_link</param>-->
 	<group name="HW_USE_MAS_IMU">
-	    <param name="accelerometer">sensor_imu_xsensmt</param>
-	    <param name="gyroscope">sensor_imu_xsensmt</param>
+	    <param name="accelerometer">waist_imu_0</param>
+	    <param name="gyroscope">waist_imu_0</param>
 	</group>
 
         <group name="multipleAnalogSensorsNames">
diff --git a/ergoCubSN000/hardware/inertials/waist-inertial.xml b/ergoCubSN000/hardware/inertials/waist-inertial.xml
index ad82a697ca..a620c48868 100644
--- a/ergoCubSN000/hardware/inertials/waist-inertial.xml
+++ b/ergoCubSN000/hardware/inertials/waist-inertial.xml
@@ -7,4 +7,5 @@
     <param name="xsensmt_gyro_period">0.005</param>
     <param name="xsensmt_acc_period">0.005</param>
     <param name="xsensmt_mag_period">0.01</param>
+    <param name="sensor_name">waist_imu_0</param>
   </device>
\ No newline at end of file
diff --git a/ergoCubSN001/estimators/wholebodydynamics.xml b/ergoCubSN001/estimators/wholebodydynamics.xml
index 33f1172a4d..08f24e9432 100644
--- a/ergoCubSN001/estimators/wholebodydynamics.xml
+++ b/ergoCubSN001/estimators/wholebodydynamics.xml
@@ -7,7 +7,7 @@
   <param name="modelFile">model.urdf</param>
   <param name="fixedFrameGravity">(0,0,-9.81)</param>
   <param name="defaultContactFrames">(l_hand_palm,r_hand_palm,root_link,l_foot_front,l_foot_rear,r_foot_front,r_foot_rear,l_upper_leg,r_upper_leg)</param>
-  <param name="imuFrameName">head_imu_0</param>
+  <param name="imuFrameName">waist_imu_0</param>
   <param name="publishOnROS">true</param>
   <param name="forceTorqueEstimateConfidence">2</param>
   <param name="useJointVelocity">true</param>
@@ -22,8 +22,8 @@
   <param name="publishNetExternalWrenches">true</param>
   <!--<param name="assume_fixed">root_link</param>-->
   <group name="HW_USE_MAS_IMU">
-    <param name="accelerometer">sensor_imu_xsensmt</param>
-    <param name="gyroscope">sensor_imu_xsensmt</param>
+    <param name="accelerometer">waist_imu_0</param>
+    <param name="gyroscope">waist_imu_0</param>
   </group>
 
   <group name="multipleAnalogSensorsNames">
diff --git a/ergoCubSN001/hardware/inertials/waist-inertial.xml b/ergoCubSN001/hardware/inertials/waist-inertial.xml
index ad82a697ca..a620c48868 100644
--- a/ergoCubSN001/hardware/inertials/waist-inertial.xml
+++ b/ergoCubSN001/hardware/inertials/waist-inertial.xml
@@ -7,4 +7,5 @@
     <param name="xsensmt_gyro_period">0.005</param>
     <param name="xsensmt_acc_period">0.005</param>
     <param name="xsensmt_mag_period">0.01</param>
+    <param name="sensor_name">waist_imu_0</param>
   </device>
\ No newline at end of file

From d2f159b82a5d9596ce7a597f65b40ee1ded05c7d Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio@traversaro.it>
Date: Wed, 14 Feb 2024 09:08:15 +0100
Subject: [PATCH 2/2] Fix missing empty line at the end of file

---
 ergoCubSN000/hardware/inertials/waist-inertial.xml | 2 +-
 ergoCubSN001/hardware/inertials/waist-inertial.xml | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/ergoCubSN000/hardware/inertials/waist-inertial.xml b/ergoCubSN000/hardware/inertials/waist-inertial.xml
index a620c48868..2547c0da69 100644
--- a/ergoCubSN000/hardware/inertials/waist-inertial.xml
+++ b/ergoCubSN000/hardware/inertials/waist-inertial.xml
@@ -8,4 +8,4 @@
     <param name="xsensmt_acc_period">0.005</param>
     <param name="xsensmt_mag_period">0.01</param>
     <param name="sensor_name">waist_imu_0</param>
-  </device>
\ No newline at end of file
+  </device>
diff --git a/ergoCubSN001/hardware/inertials/waist-inertial.xml b/ergoCubSN001/hardware/inertials/waist-inertial.xml
index a620c48868..2547c0da69 100644
--- a/ergoCubSN001/hardware/inertials/waist-inertial.xml
+++ b/ergoCubSN001/hardware/inertials/waist-inertial.xml
@@ -8,4 +8,4 @@
     <param name="xsensmt_acc_period">0.005</param>
     <param name="xsensmt_mag_period">0.01</param>
     <param name="sensor_name">waist_imu_0</param>
-  </device>
\ No newline at end of file
+  </device>