From 1a0f6fc426eea351d8a4ade33ae36220b1c7e284 Mon Sep 17 00:00:00 2001
From: Marco Randazzo <marco.randazzo@iit.it>
Date: Tue, 14 Jan 2025 11:49:47 +0100
Subject: [PATCH] Fixed all_joints for ros2

---
 R1SN003/R1.xml                                | 17 ++++++------
 .../motorControl/cer_alljoints_remapper.xml   | 26 +++++++++----------
 .../left_arm_no_hand-mc_remapper.xml          | 15 +++++++++++
 .../right_arm_no_hand-mc_remapper.xml         | 15 +++++++++++
 R1SN003/yarprobotinterface.ini                |  2 +-
 5 files changed, 53 insertions(+), 22 deletions(-)
 create mode 100644 R1SN003/wrappers/motorControl/left_arm_no_hand-mc_remapper.xml
 create mode 100644 R1SN003/wrappers/motorControl/right_arm_no_hand-mc_remapper.xml

diff --git a/R1SN003/R1.xml b/R1SN003/R1.xml
index fe4e3976a1..cb46437141 100644
--- a/R1SN003/R1.xml
+++ b/R1SN003/R1.xml
@@ -47,7 +47,8 @@
 
     <!-- LEFT ARM -->
     <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.xml" disabled_by="disable_left_arm" />
-    <xi:include href="wrappers/motorControl/left_arm-mc_remapper.xml" disabled_by="disable_left_arm" />
+    <xi:include href="wrappers/motorControl/left_arm-mc_remapper.xml" disabled_by="disable_left_arm disable_left_hand" />
+    <xi:include href="wrappers/motorControl/left_arm_no_hand-mc_remapper.xml" disabled_by="disable_left_arm" />
 
     <xi:include href="hardware/motorControl/left_arm-eb2-j0_1-mc.xml" disabled_by="disable_left_arm" />
     <xi:include href="hardware/motorControl/left_arm-eb4-j2_3-mc.xml" disabled_by="disable_left_arm" />
@@ -57,7 +58,8 @@
 
     <!-- RIGHT ARM -->
     <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" disabled_by="disable_right_arm" />
-    <xi:include href="wrappers/motorControl/right_arm-mc_remapper.xml" disabled_by="disable_right_arm" />
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper.xml" disabled_by="disable_right_arm disable_right_hand" />
+    <xi:include href="wrappers/motorControl/right_arm_no_hand-mc_remapper.xml" disabled_by="disable_right_arm" />
 
     <xi:include href="hardware/motorControl/right_arm-eb5-j0_1-mc.xml" disabled_by="disable_right_arm" />
     <xi:include href="hardware/motorControl/right_arm-eb7-j2_3-mc.xml" disabled_by="disable_right_arm" />
@@ -107,14 +109,13 @@
     <xi:include href="wrappers/FT/right_arm-FT_nws_ros2.xml" enabled_by="enable_ros2" disabled_by="disable_ros2 disable_right_arm" /> -->
 
     <!--  ROS -->
-   <!-- <xi:include href="wrappers/motorControl/cer_alljoints_remapper.xml" enabled_by="enable_ros enable_ros2" disabled_by="disable_base disable_head disable_left_arm disable_left_hand disable_right_arm disable_right_hand disable_torso"/>
-    <xi:include href="wrappers/motorControl/cer_alljoints_ros_wrapper.xml" enabled_by="enable_ros" disabled_by="disable_base disable_head disable_left_arm disable_left_hand disable_right_arm disable_right_hand disable_torso"/>
-    <xi:include href="wrappers/motorControl/cer_alljoints_ros2_wrapper.xml" enabled_by="enable_ros2" disabled_by="disable_base disable_head disable_left_arm disable_left_hand disable_right_arm disable_right_hand disable_torso"/> -->
+    <xi:include href="wrappers/motorControl/cer_alljoints_remapper.xml" enabled_by="enable_ros2" disabled_by="disable_base disable_head disable_left_arm disable_left_hand disable_right_arm disable_right_hand disable_torso"/>
+    <xi:include href="wrappers/motorControl/cer_alljoints_ros2_wrapper.xml" enabled_by="enable_ros2" disabled_by="disable_base disable_head disable_left_arm disable_left_hand disable_right_arm disable_right_hand disable_torso"/>
 
     <!-- BATTERY -->
-    <xi:include href="wrappers/battery/r1battery.xml" />
-    <xi:include href="wrappers/battery/r1battery_nws_ros2.xml" />
-    <xi:include href="hardware/battery/r1battery.xml" /> 
+    <xi:include href="wrappers/battery/r1battery.xml" enabled_by="enable_battery" disabled_by="disable_battery" />
+    <xi:include href="wrappers/battery/r1battery_nws_ros2.xml" enabled_by="enable_battery enable_ros2" disabled_by="disable_battery disable_ros2" />
+    <xi:include href="hardware/battery/r1battery.xml" enabled_by="enable_battery" disabled_by="disable_battery" />
 
 </devices>
 </robot>
diff --git a/R1SN003/wrappers/motorControl/cer_alljoints_remapper.xml b/R1SN003/wrappers/motorControl/cer_alljoints_remapper.xml
index 0be244a55d..d7e752b4c0 100644
--- a/R1SN003/wrappers/motorControl/cer_alljoints_remapper.xml
+++ b/R1SN003/wrappers/motorControl/cer_alljoints_remapper.xml
@@ -2,26 +2,26 @@
 <!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="cer_all_joints_mc_remapper" type="controlboardremapper">
     <paramlist name="networks">
-        <elem name="head">(            0  1   0  1 )</elem>
-        <elem name="torso">(           2  4   0  2 )</elem>
-        <elem name="torso_yaw">(       5  5   3  3 )</elem>
-        <elem name="right_arm">(       6  13  0  7 )</elem>
-        <elem name="left_arm">(        14 21  0  7 )</elem>
-        <elem name="right_hand">(      22 23  0  1 )</elem>
-        <elem name="left_hand">(       24 25  0  1 )</elem>
-        <elem name="base">(            26 27  0  1 )</elem>
+        <elem name="base">(            0  1   0  1 )</elem>
+        <elem name="head">(            2  3   0  1 )</elem>
+        <elem name="torso">(           4  6   0  2 )</elem>
+        <elem name="torso_yaw">(       7  7   3  3 )</elem>
+        <elem name="right_arm">(       8  14  0  6 )</elem>
+        <elem name="left_arm">(        15 21  0  6 )</elem>
+    <!--    <elem name="right_hand">(      22 23  0  1 )</elem>  -->
+    <!--    <elem name="left_hand">(       24 25  0  1 )</elem>  -->
     </paramlist>
-    <param name="joints"> 28 </param>
+    <param name="joints"> 22 </param>
     <action phase="startup" level="6" type="attach">
         <paramlist name="networks">
             <elem name="head">            cer_head_mc               </elem>
             <elem name="torso">           cer_torso_equivalent_mc   </elem>
             <elem name="torso_yaw">       cer_torso_mc              </elem>
             <elem name="base">            cer_mobile_base_mc        </elem>
-            <elem name="right_arm">       cer_right_arm_no_hand_mc_remapper  </elem>
-            <elem name="right_hand">      cer_right_hand_mc         </elem>
-            <elem name="left_arm">        cer_left_arm_no_hand_mc_remapper   </elem>
-            <elem name="left_hand">       cer_left_hand_mc          </elem>
+            <elem name="right_arm">       right_arm_no_hand_mc_remapper  </elem>
+     <!--       <elem name="right_hand">      right_hand_mc         </elem>  -->
+            <elem name="left_arm">        left_arm_no_hand_mc_remapper   </elem>
+     <!--       <elem name="left_hand">       left_hand_mc          </elem>  -->
         </paramlist>
     </action>
     <action phase="shutdown" level="19" type="detach" />
diff --git a/R1SN003/wrappers/motorControl/left_arm_no_hand-mc_remapper.xml b/R1SN003/wrappers/motorControl/left_arm_no_hand-mc_remapper.xml
new file mode 100644
index 0000000000..9a8d0f106d
--- /dev/null
+++ b/R1SN003/wrappers/motorControl/left_arm_no_hand-mc_remapper.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="left_arm_no_hand_mc_remapper" type="controlboardremapper">
+    <param name="axesNames">(l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch)</param>
+    <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_joints3"> left_arm-eb31-j4_6-mc  </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/R1SN003/wrappers/motorControl/right_arm_no_hand-mc_remapper.xml b/R1SN003/wrappers/motorControl/right_arm_no_hand-mc_remapper.xml
new file mode 100644
index 0000000000..e17b38113c
--- /dev/null
+++ b/R1SN003/wrappers/motorControl/right_arm_no_hand-mc_remapper.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="right_arm_no_hand_mc_remapper" type="controlboardremapper">
+    <param name="axesNames">(r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch)</param>
+    <param name="joints"> 7 </param>
+    <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+            <elem name="right_arm_joints1"> right_arm-eb5-j0_1-mc    </elem>
+            <elem name="right_arm_joints2"> right_arm-eb7-j2_3-mc    </elem>        
+            <elem name="right_arm_joints3"> right_arm-eb30-j4_6-mc   </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/R1SN003/yarprobotinterface.ini b/R1SN003/yarprobotinterface.ini
index 0357739314..7a426778e3 100644
--- a/R1SN003/yarprobotinterface.ini
+++ b/R1SN003/yarprobotinterface.ini
@@ -1,3 +1,3 @@
 config ./R1.xml
 enable_tags (enable_ros2)
-disable_tags ()
+disable_tags (disable_battery)