From dd6323a4ec992419e89b454d392f9d43bd30bdf7 Mon Sep 17 00:00:00 2001
From: Martina Gloria <martina.gloria@iit.it>
Date: Fri, 8 Mar 2024 16:02:54 +0100
Subject: [PATCH] icub2_5: Add alljoints-inertials_wrapper/remapper

---
 simmechanics/data/icub2_5/conf/icub.xml         |  2 ++
 .../inertials/alljoints-inertials_remapper.xml  | 17 +++++++++++++++++
 .../inertials/alljoints-inertials_wrapper.xml   | 15 +++++++++++++++
 3 files changed, 34 insertions(+)
 create mode 100644 simmechanics/data/icub2_5/conf/wrappers/inertials/alljoints-inertials_remapper.xml
 create mode 100644 simmechanics/data/icub2_5/conf/wrappers/inertials/alljoints-inertials_wrapper.xml

diff --git a/simmechanics/data/icub2_5/conf/icub.xml b/simmechanics/data/icub2_5/conf/icub.xml
index 3dce396..e43c6bd 100644
--- a/simmechanics/data/icub2_5/conf/icub.xml
+++ b/simmechanics/data/icub2_5/conf/icub.xml
@@ -31,6 +31,8 @@
     <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
     <xi:include href="wrappers/inertials/left_arm-inertials_wrapper.xml" />
     <xi:include href="wrappers/inertials/right_arm-inertials_wrapper.xml" />
+    <xi:include href="wrappers/inertials/alljoints-inertials_remapper.xml" />
+    <xi:include href="wrappers/inertials/alljoints-inertials_wrapper.xml" />
     <!--  NOT USED RIGHT NOW
     <xi:include href="wrappers/inertials/waist-inertials_wrapper.xml" /> -->    
     </devices>
diff --git a/simmechanics/data/icub2_5/conf/wrappers/inertials/alljoints-inertials_remapper.xml b/simmechanics/data/icub2_5/conf/wrappers/inertials/alljoints-inertials_remapper.xml
new file mode 100644
index 0000000..742142a
--- /dev/null
+++ b/simmechanics/data/icub2_5/conf/wrappers/inertials/alljoints-inertials_remapper.xml
@@ -0,0 +1,17 @@
+<?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="alljoints-inertials_remapper" type="multipleanalogsensorsremapper">
+    <param name="OrientationSensorsNames">
+        (l_arm_ft r_arm_ft head_imu_0)
+    </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="head_imu">  head_inertial_hardware_device </elem>
+            <elem name="left_arm_imu">  left_arm_inertial_hardware_device </elem>
+            <elem name="right_arm_imu">  right_arm_inertial_hardware_device </elem>
+        </paramlist>
+    </action>
+
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/simmechanics/data/icub2_5/conf/wrappers/inertials/alljoints-inertials_wrapper.xml b/simmechanics/data/icub2_5/conf/wrappers/inertials/alljoints-inertials_wrapper.xml
new file mode 100644
index 0000000..0370605
--- /dev/null
+++ b/simmechanics/data/icub2_5/conf/wrappers/inertials/alljoints-inertials_wrapper.xml
@@ -0,0 +1,15 @@
+<?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="alljoints-inertials_wrapper" type="multipleanalogsensorsserver">
+    <param name="period">      10                           </param>
+    <param name="name"> ${portprefix}/alljoints/inertials   </param>
+
+    <action phase="startup" level="10" type="attach">
+        <paramlist name="networks">
+            <elem name="FirstStrain"> alljoints-inertials_remapper </elem>
+        </paramlist>
+       </action>
+
+    <action phase="shutdown" level="15" type="detach" />
+</device>