Merge pull request #228 from robotology/feat/addRos2NWS

icub3: add xml files for running mc and ft ros2 nws
robotology · Dec 23, 2022 · 275ebe3 · 275ebe3
2 parents 3e86dd0 + 810044a
commit 275ebe3
Show file tree

Hide file tree

Showing 18 changed files with 240 additions and 25 deletions.
diff --git a/simmechanics/data/icub2_5/ICUB_2-5_BB_simmechanics_options.yaml.in b/simmechanics/data/icub2_5/ICUB_2-5_BB_simmechanics_options.yaml.in
@@ -595,47 +595,41 @@ linkFrames:
 forceTorqueSensors:
   - jointName: l_leg_ft_sensor
     directionChildToParent: Yes
-    exportFrameInURDF: Yes
     sensorBlobs:
     - |
         <plugin name="left_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
           <yarpConfigurationFile>model://iCub/conf/FT/gazebo_icub_left_leg_ft.ini</yarpConfigurationFile>
         </plugin>
   - jointName: r_leg_ft_sensor
     directionChildToParent: Yes
-    exportFrameInURDF: Yes
     sensorBlobs:
     - |
         <plugin name="right_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
           <yarpConfigurationFile>model://iCub/conf/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
         </plugin>
   - jointName: l_foot_ft_sensor
     directionChildToParent: Yes
-    exportFrameInURDF: Yes
     sensorBlobs:
     - |
         <plugin name="left_foot_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
           <yarpConfigurationFile>model://iCub/conf/FT/gazebo_icub_left_foot_ft.ini</yarpConfigurationFile>
         </plugin>
   - jointName: r_foot_ft_sensor
     directionChildToParent: Yes
-    exportFrameInURDF: Yes
     sensorBlobs:
     - |
         <plugin name="right_foot_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
           <yarpConfigurationFile>model://iCub/conf/FT/gazebo_icub_right_foot_ft.ini</yarpConfigurationFile>
         </plugin>
   - jointName: l_arm_ft_sensor
     directionChildToParent: Yes
-    exportFrameInURDF: Yes
     sensorBlobs:
     - |
         <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
           <yarpConfigurationFile>model://iCub/conf/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
         </plugin>
   - jointName: r_arm_ft_sensor
     directionChildToParent: Yes
-    exportFrameInURDF: Yes
     sensorBlobs:
     - |
         <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">

diff --git a/simmechanics/data/icub3/ICUB_3_all_options.yaml b/simmechanics/data/icub3/ICUB_3_all_options.yaml
@@ -120,8 +120,6 @@ linkFrames:
     frameName: SCSYS_L_SHOULDER_3
   - linkName: l_upper_arm
     frameName: SCSYS_L_UPPERARM
-  - linkName: r_shoulder_2
-    frameName: SCSYS_R_SHOULDER_2_FT
   - linkName: r_shoulder_1
     frameName: SCSYS_R_SHOULDER_1
   - linkName: r_shoulder_2
@@ -225,6 +223,8 @@ forceTorqueSensors:
       exportFrameInURDF: Yes
       frame: sensor
       frameName: SCSYS_L_SHOULDER_2_FT
+      linkName: l_shoulder_2
+      sensorName: l_arm_ft
       sensorBlobs:
       - |
           <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -235,6 +235,8 @@ forceTorqueSensors:
       exportFrameInURDF: Yes
       frame: sensor
       frameName: SCSYS_R_SHOULDER_2_FT
+      linkName: r_shoulder_2
+      sensorName: r_arm_ft
       sensorBlobs:
       - |
           <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -245,7 +247,9 @@ forceTorqueSensors:
       directionChildToParent: Yes
       exportFrameInURDF: Yes
       frame: sensor
+      linkName: l_hip_2
       frameName: SCSYS_L_HIP_2_FT
+      sensorName: l_leg_ft
       sensorBlobs:
       - |
           <plugin name="left_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -255,7 +259,9 @@ forceTorqueSensors:
       directionChildToParent: No
       exportFrameInURDF: Yes
       frame: sensor
+      linkName: l_ankle_2
       frameName: SCSYS_L_ANKLE_2_FT_FRONT
+      sensorName: l_foot_front_ft
       sensorBlobs:
       - |
           <plugin name="left_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -265,7 +271,9 @@ forceTorqueSensors:
       directionChildToParent: No
       exportFrameInURDF: Yes
       frame: sensor
+      linkName: l_ankle_2
       frameName: SCSYS_L_ANKLE_2_FT_REAR
+      sensorName: l_foot_rear_ft
       sensorBlobs:
       - |
           <plugin name="left_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -276,7 +284,9 @@ forceTorqueSensors:
       directionChildToParent: Yes
       exportFrameInURDF: Yes
       frame: sensor
+      linkName: r_hip_2
       frameName: SCSYS_R_HIP_2_FT
+      sensorName: r_leg_ft
       sensorBlobs:
       - |
           <plugin name="right_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -286,7 +296,9 @@ forceTorqueSensors:
       directionChildToParent: No
       exportFrameInURDF: Yes
       frame: sensor
+      linkName: r_ankle_2
       frameName: SCSYS_R_ANKLE_2_FT_FRONT
+      sensorName: r_foot_front_ft
       sensorBlobs:
       - |
           <plugin name="right_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -296,7 +308,9 @@ forceTorqueSensors:
       directionChildToParent: No
       exportFrameInURDF: Yes
       frame: sensor
+      linkName: r_ankle_2
       frameName: SCSYS_R_ANKLE_2_FT_REAR
+      sensorName: r_foot_rear_ft
       sensorBlobs:
       - |
           <plugin name="right_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">

diff --git a/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in b/simmechanics/data/icub3/ICUB_3_all_options_gazebo.yaml.in
@@ -219,19 +219,23 @@ forceTorqueSensors:
     # upperbody
     - jointName: l_arm_ft_sensor
       directionChildToParent: Yes
+      exportFrameInURDF: Yes
       frame: sensor
       frameName: SCSYS_L_SHOULDER_2_FT
-      exportFrameInURDF: Yes
+      linkName: l_shoulder_2
+      sensorName: l_arm_ft
       sensorBlobs:
       - |
           <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
             <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
           </plugin>
     - jointName: r_arm_ft_sensor
       directionChildToParent: Yes
+      exportFrameInURDF: Yes
       frame: sensor
       frameName: SCSYS_R_SHOULDER_2_FT
-      exportFrameInURDF: Yes
+      linkName: r_shoulder_2
+      sensorName: r_arm_ft
       sensorBlobs:
       - |
           <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -240,29 +244,35 @@ forceTorqueSensors:
     # left leg
     - jointName: l_leg_ft_sensor
       directionChildToParent: Yes
+      exportFrameInURDF: Yes
       frame: sensor
+      linkName: l_hip_2
       frameName: SCSYS_L_HIP_2_FT
-      exportFrameInURDF: Yes
+      sensorName: l_leg_ft
       sensorBlobs:
       - |
           <plugin name="left_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
             <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_leg_ft.ini</yarpConfigurationFile>
           </plugin>
     - jointName: l_foot_front_ft_sensor
       directionChildToParent: No
+      exportFrameInURDF: Yes
       frame: sensor
+      linkName: l_ankle_2
       frameName: SCSYS_L_ANKLE_2_FT_FRONT
-      exportFrameInURDF: Yes
+      sensorName: l_foot_front_ft
       sensorBlobs:
       - |
           <plugin name="left_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
             <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_front_ft.ini</yarpConfigurationFile>
           </plugin>
     - jointName: l_foot_rear_ft_sensor
       directionChildToParent: No
+      exportFrameInURDF: Yes
       frame: sensor
+      linkName: l_ankle_2
       frameName: SCSYS_L_ANKLE_2_FT_REAR
-      exportFrameInURDF: Yes
+      sensorName: l_foot_rear_ft
       sensorBlobs:
       - |
           <plugin name="left_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -271,29 +281,35 @@ forceTorqueSensors:
     # right leg
     - jointName: r_leg_ft_sensor
       directionChildToParent: Yes
+      exportFrameInURDF: Yes
       frame: sensor
+      linkName: r_hip_2
       frameName: SCSYS_R_HIP_2_FT
-      exportFrameInURDF: Yes
+      sensorName: r_leg_ft
       sensorBlobs:
       - |
           <plugin name="right_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
             <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
           </plugin>
     - jointName: r_foot_front_ft_sensor
       directionChildToParent: No
+      exportFrameInURDF: Yes
       frame: sensor
+      linkName: r_ankle_2
       frameName: SCSYS_R_ANKLE_2_FT_FRONT
-      exportFrameInURDF: Yes
+      sensorName: r_foot_front_ft
       sensorBlobs:
       - |
           <plugin name="right_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
             <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_foot_front_ft.ini</yarpConfigurationFile>
           </plugin>
     - jointName: r_foot_rear_ft_sensor
       directionChildToParent: No
+      exportFrameInURDF: Yes
       frame: sensor
+      linkName: r_ankle_2
       frameName: SCSYS_R_ANKLE_2_FT_REAR
-      exportFrameInURDF: Yes
+      sensorName: r_foot_rear_ft
       sensorBlobs:
       - |
           <plugin name="right_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
@@ -459,7 +475,7 @@ assignedMasses:
   neck_2      : 0.1214
   neck_3      : 0.18386
   head        : 1.8728
-  
+
 assignedInertias:
   # This is due to https://github.com/robotology/icub-models/issues/33
   - linkName: r_shoulder_1

diff --git a/simmechanics/data/icub3/conf/icub_ROS2.xml b/simmechanics/data/icub3/conf/icub_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="iCubGazeboV3" build="1" portprefix="/icubSim" xmlns:xi="http://www.w3.org/2001/XInclude">
+
+    <devices>
+
+    <!-- motor controllers wrappers -->
+    <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_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" />
+    <!--  ANALOG SENSORS FT -->
+    <xi:include href="wrappers/FT/left_foot-FT_remapper.xml" />
+    <xi:include href="wrappers/FT/right_foot-FT_remapper.xml" />
+    <xi:include href="wrappers/FT/left_foot-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/right_foot-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="wrappers/FT/right_leg_hip-FT_wrapper.xml" />
+    <xi:include href="wrappers/FT/left_leg_hip-FT_wrapper.xml" />
+
+    <!-- INERTIAL SENSOR-->
+    <xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
+
+    <!-- all joints remapper -->
+    <xi:include href="./wrappers/motorControl/alljoints-mc_remapper.xml"/>
+    <xi:include href="./wrappers/motorControl/alljoints-mc_nws_ros2.xml"/>
+
+    <!-- ft sensors  ROS2 -->
+    <xi:include href="wrappers/FT/left_foot-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/right_foot-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/left_leg_hip-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/right_leg_hip-FT_nws_ros2.xml" />
+    <xi:include href="wrappers/FT/left_arm-FT_nws_ros2.xml" /> -->
+    <xi:include href="wrappers/FT/right_arm-FT_nws_ros2.xml" />
+    </devices>
+</robot>
diff --git a/simmechanics/data/icub3/conf/wrappers/FT/left_arm-FT_nws_ros2.xml b/simmechanics/data/icub3/conf/wrappers/FT/left_arm-FT_nws_ros2.xml
@@ -0,0 +1,18 @@
+<?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-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /left_arm_ft                      </param>
+        <param name="node_name">      icub_left_arm_ft          </param>
+        <param name="period">      0.1                          </param>
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+                <elem name="FirstStrain">  icub_left_arm_ft </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
diff --git a/simmechanics/data/icub3/conf/wrappers/FT/left_foot-FT_nws_ros2.xml b/simmechanics/data/icub3/conf/wrappers/FT/left_foot-FT_nws_ros2.xml
@@ -0,0 +1,18 @@
+<?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_foot-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /left_foot_ft                      </param>
+        <param name="node_name">      icub_left_foot_ft          </param>
+        <param name="period">      0.1                          </param>
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+                <elem name="FirstStrain">  left_foot-FT_remapper </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
diff --git a/simmechanics/data/icub3/conf/wrappers/FT/left_foot-FT_remapper.xml b/simmechanics/data/icub3/conf/wrappers/FT/left_foot-FT_remapper.xml
@@ -3,8 +3,8 @@
 
 <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_foot-FT_remapper" type="multipleanalogsensorsremapper">
     <param name="SixAxisForceTorqueSensorsNames">
-          (l_foot_rear_ft_sensor
-           l_foot_front_ft_sensor)
+          (l_foot_rear_ft
+           l_foot_front_ft)
     </param>
     <action phase="startup" level="5" type="attach">
          <paramlist name="networks">

diff --git a/simmechanics/data/icub3/conf/wrappers/FT/left_leg_hip-FT_nws_ros2.xml b/simmechanics/data/icub3/conf/wrappers/FT/left_leg_hip-FT_nws_ros2.xml
@@ -0,0 +1,18 @@
+<?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_hip-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /left_leg_hip_ft                      </param>
+        <param name="node_name">      icub_left_leg_hip_ft          </param>
+        <param name="period">      0.1                          </param>
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+                <elem name="FirstStrain">  icub_left_leg_ft </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>
diff --git a/simmechanics/data/icub3/conf/wrappers/FT/left_leg_hip-FT_wrapper.xml b/simmechanics/data/icub3/conf/wrappers/FT/left_leg_hip-FT_wrapper.xml
@@ -5,7 +5,7 @@
     <device xmlns:xi="http://www.w3.org/2001/XInclude" name="left_leg_hip-FT_wrapper" type="multipleanalogsensorsserver">
         <param name="period">      10                          </param>
         <param name="name">       ${portprefix}/left_leg_hip     </param>
-        
+
         <action phase="startup" level="10" type="attach">
             <paramlist name="networks">
                 <elem name="FirstStrain">  icub_left_leg_ft </elem>

diff --git a/simmechanics/data/icub3/conf/wrappers/FT/right_arm-FT_nws_ros2.xml b/simmechanics/data/icub3/conf/wrappers/FT/right_arm-FT_nws_ros2.xml
@@ -0,0 +1,18 @@
+<?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-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /right_arm_ft                      </param>
+        <param name="node_name">      icub_right_arm_ft          </param>
+        <param name="period">      0.1                          </param>
+
+        <action phase="startup" level="10" type="attach">
+            <paramlist name="networks">
+        <!-- The param value must match the device name in the corresponding body_part-ebX-jA_B-strain.xml file -->
+                <elem name="FirstStrain">  icub_right_arm_ft </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>