From 352272c21f12d84094dff1a5de0241c253186896 Mon Sep 17 00:00:00 2001
From: icub <icub@iit.it>
Date: Mon, 15 May 2023 14:03:38 +0000
Subject: [PATCH] Add imu of ergocub legs

---
 ergoCubSN000/ergocub_wbd.xml                  |  4 ++
 ergoCubSN000/ft.xml                           |  2 +-
 .../inertials/left_leg-eb8-j0_3-inertial.xml  | 46 +++++++++++++++++++
 .../inertials/right_leg-eb6-j0_3-inertial.xml | 46 +++++++++++++++++++
 .../wrappers/FT/right_arm-FT_wrapper.xml      |  4 +-
 .../inertials/left_foot-imu_wrapper.xml       |  2 +-
 .../inertials/left_leg-imu_wrapper.xml        | 20 ++++++++
 .../inertials/right_foot-imu_wrapper.xml      |  2 +-
 .../inertials/right_leg-imu_wrapper.xml       | 20 ++++++++
 9 files changed, 141 insertions(+), 5 deletions(-)
 create mode 100644 ergoCubSN000/hardware/inertials/left_leg-eb8-j0_3-inertial.xml
 create mode 100644 ergoCubSN000/hardware/inertials/right_leg-eb6-j0_3-inertial.xml
 create mode 100644 ergoCubSN000/wrappers/inertials/left_leg-imu_wrapper.xml
 create mode 100644 ergoCubSN000/wrappers/inertials/right_leg-imu_wrapper.xml

diff --git a/ergoCubSN000/ergocub_wbd.xml b/ergoCubSN000/ergocub_wbd.xml
index daa91e2eb4..d58a7e815d 100644
--- a/ergoCubSN000/ergocub_wbd.xml
+++ b/ergoCubSN000/ergocub_wbd.xml
@@ -95,12 +95,16 @@
     <!-- FT SENSORS - IMU -->
     <xi:include href="hardware/inertials/left_arm-eb2-j0_1-inertial.xml" />
     <xi:include href="hardware/inertials/right_arm-eb1-j0_1-inertial.xml" />
+    <xi:include href="hardware/inertials/left_leg-eb8-j0_3-inertial.xml" />
+    <xi:include href="hardware/inertials/right_leg-eb6-j0_3-inertial.xml" />
     <xi:include href="hardware/inertials/left_leg-eb9-j4_5-inertial.xml" />
     <xi:include href="hardware/inertials/right_leg-eb7-j4_5-inertial.xml" />
 
     <!-- FT SENSORS - IMU - MULTIPLE ANALOG SENSOR SERVERS -->
     <xi:include href="wrappers/inertials/left_arm-imu_wrapper.xml" />
     <xi:include href="wrappers/inertials/right_arm-imu_wrapper.xml" />
+    <xi:include href="wrappers/inertials/left_leg-imu_wrapper.xml" />
+    <xi:include href="wrappers/inertials/right_leg-imu_wrapper.xml" />
     <xi:include href="wrappers/inertials/left_foot-imu_wrapper.xml" />
     <xi:include href="wrappers/inertials/right_foot-imu_wrapper.xml" />
 
diff --git a/ergoCubSN000/ft.xml b/ergoCubSN000/ft.xml
index 7c1381daa7..0a14b6e703 100644
--- a/ergoCubSN000/ft.xml
+++ b/ergoCubSN000/ft.xml
@@ -12,7 +12,7 @@
     <!-- ANALOG SENSORS FT -->
 
 
-   <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
+   <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" /> 
    <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" />
 
    <xi:include href="hardware/FT/left_arm-eb2-j0_1-strain.xml" />
diff --git a/ergoCubSN000/hardware/inertials/left_leg-eb8-j0_3-inertial.xml b/ergoCubSN000/hardware/inertials/left_leg-eb8-j0_3-inertial.xml
new file mode 100644
index 0000000000..18af34bcfd
--- /dev/null
+++ b/ergoCubSN000/hardware/inertials/left_leg-eb8-j0_3-inertial.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_leg-eb8-j0_3-inertial" type="embObjIMU">
+
+        <xi:include href="../../general.xml" />
+
+        <xi:include href="../../hardware/electronics/left_leg-eb8-j0_3-eln.xml" />
+
+        <group name="SERVICE">
+
+            <param name="type"> eomn_serv_AS_inertials3 </param>
+
+            <group name="PROPERTIES">
+
+                <group name="CANBOARDS">
+                    <param name="type">             eobrd_strain2           </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="SENSORS">
+                    <param name="id">       l_leg_ft_acc    l_leg_ft_gyro    l_leg_ft_eul    l_leg_ft_mag    l_leg_ft_stat</param>
+                    <param name="type">     eoas_imu_acc    eoas_imu_gyr     eoas_imu_eul    eoas_imu_mag    eoas_imu_status</param>
+                    <param name="boardType">  strain2       strain2      strain2            strain2           strain2</param>
+                    <param name="location">  CAN2:13       CAN2:13      CAN2:13       CAN2:13        CAN2:13    </param>
+                </group>
+
+            </group>
+
+            <group name="SETTINGS">
+                <param name="acquisitionRate">      10      </param>
+                <param name="enabledSensors">       l_leg_ft_acc    l_leg_ft_gyro    l_leg_ft_eul    l_leg_ft_mag    l_leg_ft_stat </param>
+            </group>
+        </group>
+
+    </device>
diff --git a/ergoCubSN000/hardware/inertials/right_leg-eb6-j0_3-inertial.xml b/ergoCubSN000/hardware/inertials/right_leg-eb6-j0_3-inertial.xml
new file mode 100644
index 0000000000..dd537bd125
--- /dev/null
+++ b/ergoCubSN000/hardware/inertials/right_leg-eb6-j0_3-inertial.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="right_leg-eb6-j0_3-inertial" type="embObjIMU">
+
+        <xi:include href="../../general.xml" />
+
+        <xi:include href="../../hardware/electronics/right_leg-eb6-j0_3-eln.xml" />
+
+        <group name="SERVICE">
+
+            <param name="type"> eomn_serv_AS_inertials3 </param>
+
+            <group name="PROPERTIES">
+
+                <group name="CANBOARDS">
+                    <param name="type">             eobrd_strain2           </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="SENSORS">
+                    <param name="id">       r_leg_ft_acc    r_leg_ft_gyro    r_leg_ft_eul    r_leg_ft_mag    r_leg_ft_stat </param>
+                    <param name="type">     eoas_imu_acc   eoas_imu_gyr      eoas_imu_eul    eoas_imu_mag   eoas_imu_status</param>
+                    <param name="boardType">  strain2       strain2           strain2         strain2        strain2  </param>
+                    <param name="location">  CAN2:13       CAN2:13      CAN2:13       CAN2:13        CAN2:13    </param>
+                </group>
+
+            </group>
+
+            <group name="SETTINGS">
+                <param name="acquisitionRate">      10      </param>
+                <param name="enabledSensors">       r_leg_ft_acc    r_leg_ft_gyro    r_leg_ft_eul    r_leg_ft_mag    r_leg_ft_stat</param>
+            </group>
+        </group>
+
+    </device>
diff --git a/ergoCubSN000/wrappers/FT/right_arm-FT_wrapper.xml b/ergoCubSN000/wrappers/FT/right_arm-FT_wrapper.xml
index 1dd10cbfd4..6643d5bdd8 100644
--- a/ergoCubSN000/wrappers/FT/right_arm-FT_wrapper.xml
+++ b/ergoCubSN000/wrappers/FT/right_arm-FT_wrapper.xml
@@ -2,9 +2,9 @@
 <!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-FT_wrapper" type="multipleanalogsensorsserver">
+    <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-FT_wrapper" type="multipleanalogsensorsserver">
         <param name="period">      2                           </param>
-        <param name="name">       /ergocub/right_arm            </param>
+        <param name="name">       /ergocub/right_arm/FT          </param>
 
         <action phase="startup" level="5" type="attach">
             <paramlist name="networks">
diff --git a/ergoCubSN000/wrappers/inertials/left_foot-imu_wrapper.xml b/ergoCubSN000/wrappers/inertials/left_foot-imu_wrapper.xml
index d76a74c2da..3eaca86467 100644
--- a/ergoCubSN000/wrappers/inertials/left_foot-imu_wrapper.xml
+++ b/ergoCubSN000/wrappers/inertials/left_foot-imu_wrapper.xml
@@ -4,7 +4,7 @@
 
     <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_foot-imu_wrapper" type="multipleanalogsensorsserver">
         <param name="period">       10                  </param>
-        <param name="name">     /ergocub/left_foot/imu                </param>
+        <param name="name">     /ergocub/left_foot_heel_tiptoe/imu                </param>
 
 
         <action phase="startup" level="5" type="attach">
diff --git a/ergoCubSN000/wrappers/inertials/left_leg-imu_wrapper.xml b/ergoCubSN000/wrappers/inertials/left_leg-imu_wrapper.xml
new file mode 100644
index 0000000000..107853fd0c
--- /dev/null
+++ b/ergoCubSN000/wrappers/inertials/left_leg-imu_wrapper.xml
@@ -0,0 +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="left_leg-imu_wrapper" type="multipleanalogsensorsserver">
+        <param name="period">       10                  </param>
+        <param name="name">     /ergocub/left_leg/imu                </param>
+
+
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <!-- The name of the element can be any string (we use SetOfInertials to better express its nature).
+                     Its value must match the device name in the corresponding body_part-jx_y-inertials.xml file
+                     or in body_part-ebX-inertials.xml -->
+                <elem name="SetOfInertials">left_leg-eb8-j0_3-inertial</elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="5" type="detach" />
+    </device>
diff --git a/ergoCubSN000/wrappers/inertials/right_foot-imu_wrapper.xml b/ergoCubSN000/wrappers/inertials/right_foot-imu_wrapper.xml
index ca6ad7e799..c2caedc48a 100644
--- a/ergoCubSN000/wrappers/inertials/right_foot-imu_wrapper.xml
+++ b/ergoCubSN000/wrappers/inertials/right_foot-imu_wrapper.xml
@@ -4,7 +4,7 @@
 
     <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_foot-imu_wrapper" type="multipleanalogsensorsserver">
         <param name="period">       10                  </param>
-        <param name="name">     /ergocub/right_foot/imu </param>
+        <param name="name">     /ergocub/right_foot_heel_tiptoe/imu </param>
 
 
         <action phase="startup" level="5" type="attach">
diff --git a/ergoCubSN000/wrappers/inertials/right_leg-imu_wrapper.xml b/ergoCubSN000/wrappers/inertials/right_leg-imu_wrapper.xml
new file mode 100644
index 0000000000..31d094c4d8
--- /dev/null
+++ b/ergoCubSN000/wrappers/inertials/right_leg-imu_wrapper.xml
@@ -0,0 +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_leg-imu_wrapper" type="multipleanalogsensorsserver">
+        <param name="period">       10                  </param>
+        <param name="name">     /ergocub/right_leg/imu </param>
+
+
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <!-- The name of the element can be any string (we use SetOfInertials to better express its nature).
+                     Its value must match the device name in the corresponding body_part-jx_y-inertials.xml file
+                     or in body_part-ebX-inertials.xml -->
+                <elem name="SetOfInertials">right_leg-eb6-j0_3-inertial</elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="5" type="detach" />
+    </device>