icub3: add ROS2 nws

it requires robotology/simmechanics-to-urdf#53
robotology · Dec 19, 2022 · 4b7f077 · 4b7f077
1 parent be3969f
commit 4b7f077
Show file tree

Hide file tree

Showing 16 changed files with 166 additions and 26 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
@@ -931,6 +947,6 @@ XMLBlobs:
     - |
             <gazebo>
               <plugin name="robotinterface" filename="libgazebo_yarp_robotinterface.so">
-                <yarpRobotInterfaceConfigurationFile>model://iCub/conf_icub3/icub.xml</yarpRobotInterfaceConfigurationFile>
+                <yarpRobotInterfaceConfigurationFile>model://iCub/conf_icub3/icub_ROS2.xml</yarpRobotInterfaceConfigurationFile>
               </plugin>
             </gazebo>
diff --git a/simmechanics/data/icub3/conf/icub_ROS2.xml b/simmechanics/data/icub3/conf/icub_ROS2.xml
@@ -35,5 +35,13 @@
     <!-- 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>
diff --git a/simmechanics/data/icub3/conf/wrappers/FT/right_arm-FT_wrapper.xml b/simmechanics/data/icub3/conf/wrappers/FT/right_arm-FT_wrapper.xml
@@ -5,7 +5,7 @@
     <device xmlns:xi="http://www.w3.org/2001/XInclude" name="right_arm-FT_wrapper" type="multipleanalogsensorsserver">
         <param name="period">      10                           </param>
         <param name="name">       ${portprefix}/right_arm      </param>
-        
+
         <action phase="startup" level="10" type="attach">
             <paramlist name="networks">
                 <elem name="FirstStrain">  icub_right_arm_ft </elem>

diff --git a/simmechanics/data/icub3/conf/wrappers/FT/right_foot-FT_nws_ros2.xml b/simmechanics/data/icub3/conf/wrappers/FT/right_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="right_foot-FT_wrapper_ros2" type="wrenchStamped_nws_ros2">
+        <param name="topic_name">      /right_foot_ft                      </param>
+        <param name="node_name">      icub_right_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">  right_foot-FT_remapper </elem>
+            </paramlist>
+        </action>
+
+        <action phase="shutdown" level="15" type="detach" />
+    </device>