From afb4e41e81b0e8ed5ba74f6454c719445ceaabcf Mon Sep 17 00:00:00 2001
From: Nicogene <nicolo.genesio@iit.it>
Date: Wed, 13 Mar 2024 14:16:21 +0100
Subject: [PATCH] Add configuration files for using the sw layer that handles
 the coupling

See:

- #178
- https://github.com/robotology/yarp/pull/3026
- https://github.com/robotology/yarp/pull/3085
---
 urdf/ergoCub/conf/ergocub_coupling_ros2.xml   | 47 +++++++++++++++++++
 .../motorControl/all-mc_remapper-coupling.xml | 20 ++++++++
 .../all-mc_remapper_ros2-coupling.xml         | 12 +++++
 .../motorControl/all-mc_remapper_ros2.xml     |  4 +-
 .../left_arm-mc_remapper-coupling.xml         | 14 ++++++
 .../left_hand-coupling_handler.xml            | 31 ++++++++++++
 .../motorControl/left_hand-mc_wrapper.xml     | 11 +++++
 .../right_arm-mc_remapper-coupling.xml        | 14 ++++++
 .../right_hand-coupling_handler.xml           | 31 ++++++++++++
 9 files changed, 182 insertions(+), 2 deletions(-)
 create mode 100644 urdf/ergoCub/conf/ergocub_coupling_ros2.xml
 create mode 100644 urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper-coupling.xml
 create mode 100644 urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper_ros2-coupling.xml
 create mode 100644 urdf/ergoCub/conf/wrappers/motorControl/left_arm-mc_remapper-coupling.xml
 create mode 100644 urdf/ergoCub/conf/wrappers/motorControl/left_hand-coupling_handler.xml
 create mode 100644 urdf/ergoCub/conf/wrappers/motorControl/left_hand-mc_wrapper.xml
 create mode 100644 urdf/ergoCub/conf/wrappers/motorControl/right_arm-mc_remapper-coupling.xml
 create mode 100644 urdf/ergoCub/conf/wrappers/motorControl/right_hand-coupling_handler.xml

diff --git a/urdf/ergoCub/conf/ergocub_coupling_ros2.xml b/urdf/ergoCub/conf/ergocub_coupling_ros2.xml
new file mode 100644
index 00000000..df826a65
--- /dev/null
+++ b/urdf/ergoCub/conf/ergocub_coupling_ros2.xml
@@ -0,0 +1,47 @@
+<?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="ergocubGazebo" build="1" portprefix="/ergocubSim" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <devices>
+
+    <!-- MOTOR CONTROLLERS -->
+    <xi:include href="wrappers/motorControl/left_arm-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/right_arm-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/left_leg-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/right_leg-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/head-mc_wrapper.xml" />
+    <xi:include href="wrappers/motorControl/torso-mc_wrapper.xml" />
+
+    <xi:include href="wrappers/motorControl/left_hand-coupling_handler.xml" />
+    <xi:include href="wrappers/motorControl/right_hand-coupling_handler.xml" />
+    <xi:include href="wrappers/motorControl/left_arm-mc_remapper-coupling.xml" />
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper-coupling.xml" />
+    <xi:include href="wrappers/motorControl/left_arm-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/right_arm-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/left_leg-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/right_leg-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/head-mc_remapper.xml" />
+    <xi:include href="wrappers/motorControl/torso-mc_remapper.xml" />
+
+    <xi:include href="wrappers/motorControl/all-mc_remapper-coupling.xml" />
+    <xi:include href="wrappers/motorControl/all-mc_remapper_ros2-coupling.xml" />
+    <!--  ANALOG SENSORS FT -->
+    <xi:include href="wrappers/FT/left_leg-FT_remapper.xml" />
+    <xi:include href="wrappers/FT/right_leg-FT_remapper.xml" />
+    <xi:include href="wrappers/FT/left_leg-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/right_leg-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" />
+
+    <!-- INERTIAL SENSOR-->
+    <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
+
+    <!--  HEAD SENSORS -->
+    <xi:include href="sensors/rgbd_camera_wrapper.xml" />
+    <xi:include href="sensors/rgbd_camera_wrapper_ros2.xml" />
+    <xi:include href="sensors/lidar_wrapper.xml" />
+    <xi:include href="sensors/lidar_wrapper_ros2.xml" />
+
+    </devices>
+</robot>
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper-coupling.xml b/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper-coupling.xml
new file mode 100644
index 00000000..171f7c5f
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper-coupling.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="alljoints_mc-coupling" type="controlboardremapper">
+<!-- These are the parameters with torso_pitch removed -->
+    <param name="axesNames">(neck_pitch,neck_roll,neck_yaw,camera_tilt,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,l_wrist_yaw,l_wrist_roll,l_wrist_pitch,l_thumb_add,l_thumb_prox,l_thumb_dist,l_index_add,l_index_prox,l_index_dist,l_middle_prox,l_middle_dist,l_ring_prox,l_ring_dist,l_pinkie_prox,l_pinkie_dist,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,r_wrist_yaw,r_wrist_roll,r_wrist_pitch,r_thumb_add,r_thumb_prox,r_thumb_dist,r_index_add,r_index_prox,r_index_dist,r_middle_prox,r_middle_dist,r_ring_prox,r_ring_dist,r_pinkie_prox,r_pinkie_dist,torso_roll,torso_pitch,torso_yaw,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
+    <param name="period"> 0.010 </param>
+    <param name="joints"> 57 </param>
+    <action phase="startup" level="6" type="attach">
+        <paramlist name="networks">
+            <elem name="head_joints">       head-mc_remapper </elem>
+            <elem name="left_arm_joints">   left_arm-mc_remapper-coupling </elem>
+            <elem name="right_arm_joints">  right_arm-mc_remapper-coupling </elem>
+            <elem name="torso_joints">      torso-mc_remapper </elem>
+            <elem name="left_leg_joints">   left_leg-mc_remapper </elem>
+            <elem name="right_leg_joints">  right_leg-mc_remapper </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
\ No newline at end of file
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper_ros2-coupling.xml b/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper_ros2-coupling.xml
new file mode 100644
index 00000000..91e0f69c
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper_ros2-coupling.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot 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="cbw_ros2" type="controlBoard_nws_ros2">
+        <param name="node_name"> ros2_cb_node </param>
+        <param name="topic_name"> /joint_states </param>
+        <param name="period"> 0.010 </param>
+        <action phase="startup" level="10" type="attach">
+            <param name="device"> alljoints_mc-coupling </param>
+        </action>
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper_ros2.xml b/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper_ros2.xml
index 759872f6..09d61158 100644
--- a/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper_ros2.xml
+++ b/urdf/ergoCub/conf/wrappers/motorControl/all-mc_remapper_ros2.xml
@@ -5,8 +5,8 @@
         <param name="node_name"> ros2_cb_node </param>
         <param name="topic_name"> /joint_states </param>
         <param name="period"> 0.010 </param>
-        <action phase="startup" level="7" type="attach">
+        <action phase="startup" level="10" type="attach">
             <param name="device"> alljoints_mc </param>
         </action>
-        <action phase="shutdown" level="3" type="detach" />
+        <action phase="shutdown" level="15" type="detach" />
     </device>
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/left_arm-mc_remapper-coupling.xml b/urdf/ergoCub/conf/wrappers/motorControl/left_arm-mc_remapper-coupling.xml
new file mode 100644
index 00000000..7f02a471
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/left_arm-mc_remapper-coupling.xml
@@ -0,0 +1,14 @@
+<?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-mc_remapper-coupling" 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,l_thumb_add,l_thumb_prox,l_thumb_dist,l_index_add,l_index_prox,l_index_dist,l_middle_prox,l_middle_dist,l_ring_prox,l_ring_dist,l_pinkie_prox,l_pinkie_dist)</param>
+    <param name="joints"> 19 </param>
+    <action phase="startup" level="6" type="attach">
+        <paramlist name="networks">
+            <elem name="left_arm_joints1"> left_arm_hardware_device </elem>
+            <elem name="left_arm_joints2"> left_hand-coupling_handler </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/left_hand-coupling_handler.xml b/urdf/ergoCub/conf/wrappers/motorControl/left_hand-coupling_handler.xml
new file mode 100644
index 00000000..2596ed30
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/left_hand-coupling_handler.xml
@@ -0,0 +1,31 @@
+<?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_hand-coupling_handler" type="controlBoardCouplingHandler">
+    <param name="coupling_device"> couplingXCubHandMk5 </param>
+    <param name="jointNames"> l_thumb_add l_thumb_prox l_thumb_dist l_index_add l_index_prox l_index_dist l_middle_prox l_middle_dist l_ring_prox l_ring_dist l_pinkie_prox l_pinkie_dist </param>
+    <group name="LIMITS">
+        <param name="jntVelMax"> 100.0   100.0 100.0 100.0 100.0 100.0  100.0 100.0  100.0 100.0   100.0 100.0 </param>
+        <param name="jntPosMax"> 100.0   90.0  101.4 15.0  90.0  103.1  90.0  103.1  90.0  103.1   90.0  98.7  </param>
+        <param name="jntPosMin"> -100.0 -90.0 -101.4 -15.0 -90.0 -103.1 -90.0 -103.1 -90.0  -103.1 -90.0 -98.7 </param>
+    </group>
+    <group name="COUPLING">
+        <param name="actuatedAxesNames"> l_thumb_add l_thumb_oc l_index_add l_index_oc l_middle_oc l_ring_pinky_oc </param>
+        <param name="actuatedAxesPosMin"> 0.0 0.0 0.0 0.0 0.0 0.0 </param>
+        <param name="actuatedAxesPosMax"> 100.0 90.0 15.0 90.0 90.0 90.0 </param>
+    </group>
+    <group name="COUPLING_PARAMS">
+        <param name="L0x"> -0.0050 -0.0050 -0.0050 -0.0050 -0.0050 </param>
+        <param name="L0y"> 0.0040 0.0040 0.0040 0.0040 0.0040 </param>
+        <param name="q2bias"> -173.35 -173.35 -173.35 -173.35 -170.53 </param>
+        <param name="q1off"> 2.45 2.15 2.15 2.15 2.46 </param>
+        <param name="k"> 0.03415 0.03913 0.03913 0.03913 0.03418 </param>
+        <param name="d"> 0.03503 0.04003 0.04003 0.04003 0.03503 </param>
+        <param name="l"> 0.00604 0.00604 0.00604 0.00604 0.00608 </param>
+        <param name="b"> 0.00640 0.00640 0.00640 0.00640 0.00640 </param>
+    </group>
+    <action phase="startup" level="5" type="attach">
+        <param name="device"> left_hand_hardware_device </param>
+    </action>
+    <action phase="shutdown" level="15" type="detach" />
+</device>
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/left_hand-mc_wrapper.xml b/urdf/ergoCub/conf/wrappers/motorControl/left_hand-mc_wrapper.xml
new file mode 100644
index 00000000..4be0eaee
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/left_hand-mc_wrapper.xml
@@ -0,0 +1,11 @@
+<?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_hand-mc_nws_yarp" type="controlBoard_nws_yarp">
+    <param name="name"> ${portprefix}/left_hand </param>
+    <param name="period">0.01</param>
+    <action phase="startup" level="10" type="attach">
+        <param name="device"> left_hand-coupling_handler </param>
+    </action>
+    <action phase="shutdown" level="15" type="detach" />
+</device>
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/right_arm-mc_remapper-coupling.xml b/urdf/ergoCub/conf/wrappers/motorControl/right_arm-mc_remapper-coupling.xml
new file mode 100644
index 00000000..bd44651b
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/right_arm-mc_remapper-coupling.xml
@@ -0,0 +1,14 @@
+<?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-mc_remapper-coupling" 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,r_thumb_add,r_thumb_prox,r_thumb_dist,r_index_add,r_index_prox,r_index_dist,r_middle_prox,r_middle_dist,r_ring_prox,r_ring_dist,r_pinkie_prox,r_pinkie_dist)</param>
+    <param name="joints"> 19 </param>
+    <action phase="startup" level="6" type="attach">
+        <paramlist name="networks">
+            <elem name="right_arm_joints1"> right_arm_hardware_device </elem>
+            <elem name="right_arm_joints2"> right_hand-coupling_handler </elem>
+        </paramlist>
+    </action>
+    <action phase="shutdown" level="20" type="detach" />
+</device>
diff --git a/urdf/ergoCub/conf/wrappers/motorControl/right_hand-coupling_handler.xml b/urdf/ergoCub/conf/wrappers/motorControl/right_hand-coupling_handler.xml
new file mode 100644
index 00000000..2e8868f5
--- /dev/null
+++ b/urdf/ergoCub/conf/wrappers/motorControl/right_hand-coupling_handler.xml
@@ -0,0 +1,31 @@
+<?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_hand-coupling_handler" type="controlBoardCouplingHandler">
+    <param name="coupling_device"> couplingXCubHandMk5 </param>
+    <param name="jointNames"> r_thumb_add r_thumb_prox r_thumb_dist r_index_add r_index_prox r_index_dist r_middle_prox r_middle_dist r_ring_prox r_ring_dist r_pinkie_prox r_pinkie_dist </param>
+    <group name="LIMITS">
+        <param name="jntVelMax"> 100.0   100.0 100.0 100.0 100.0 100.0  100.0 100.0  100.0 100.0   100.0 100.0 </param>
+        <param name="jntPosMax"> 100.0   90.0  101.4 15.0  90.0  103.1  90.0  103.1  90.0  103.1   90.0  98.7  </param>
+        <param name="jntPosMin"> -100.0 -90.0 -101.4 -15.0 -90.0 -103.1 -90.0 -103.1 -90.0  -103.1 -90.0 -98.7 </param>
+    </group>
+    <group name="COUPLING">
+        <param name="actuatedAxesNames"> r_thumb_add r_thumb_oc r_index_add r_index_oc r_middle_oc r_ring_pinky_oc </param>
+        <param name="actuatedAxesPosMin"> 0.0 0.0 0.0 0.0 0.0 0.0 </param>
+        <param name="actuatedAxesPosMax"> 100.0 90.0 15.0 90.0 90.0 90.0 </param>
+    </group>
+    <group name="COUPLING_PARAMS">
+        <param name="L0x"> -0.0050 -0.0050 -0.0050 -0.0050 -0.0050 </param>
+        <param name="L0y"> 0.0040 0.0040 0.0040 0.0040 0.0040 </param>
+        <param name="q2bias"> -173.35 -173.35 -173.35 -173.35 -170.53 </param>
+        <param name="q1off"> 2.45 2.15 2.15 2.15 2.46 </param>
+        <param name="k"> 0.03415 0.03913 0.03913 0.03913 0.03418 </param>
+        <param name="d"> 0.03503 0.04003 0.04003 0.04003 0.03503 </param>
+        <param name="l"> 0.00604 0.00604 0.00604 0.00604 0.00608 </param>
+        <param name="b"> 0.00640 0.00640 0.00640 0.00640 0.00640 </param>
+    </group>
+    <action phase="startup" level="5" type="attach">
+        <param name="device"> right_hand_hardware_device </param>
+    </action>
+    <action phase="shutdown" level="15" type="detach" />
+</device>