added a floating link with name prefix (#189)

mhubii · mhubii · commit bb31bcc7f128 · 2024-10-21T10:28:34.000+01:00
diff --git a/lbr_bringup/config/gazebo.rviz b/lbr_bringup/config/gazebo.rviz
@@ -67,6 +67,10 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        lbr_floating_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         lbr_link_0:
           Alpha: 1
           Show Axes: false
@@ -111,10 +115,6 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        world:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
diff --git a/lbr_bringup/config/hardware.rviz b/lbr_bringup/config/hardware.rviz
@@ -68,6 +68,10 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        lbr_floating_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         lbr_link_0:
           Alpha: 1
           Show Axes: false
@@ -112,10 +116,6 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        world:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
diff --git a/lbr_bringup/config/mock.rviz b/lbr_bringup/config/mock.rviz
@@ -67,6 +67,10 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        lbr_floating_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         lbr_link_0:
           Alpha: 1
           Show Axes: false
@@ -111,10 +115,6 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        world:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
diff --git a/lbr_bringup/launch/gazebo.launch.py b/lbr_bringup/launch/gazebo.launch.py
@@ -1,5 +1,5 @@
 from launch import LaunchDescription
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PythonExpression
 from lbr_bringup.description import LBRDescriptionMixin
 from lbr_bringup.gazebo import GazeboMixin
 from lbr_bringup.ros2_control import LBRROS2ControlMixin
@@ -15,13 +15,15 @@ def generate_launch_description() -> LaunchDescription:
         LBRROS2ControlMixin.arg_ctrl()
     )  # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml
 
-    # static transform world -> robot_name/world
+    # static transform world -> <robot_name>_floating_link
     world_robot_tf = [0, 0, 0, 0, 0, 0]  # keep zero
     ld.add_action(
         LBRDescriptionMixin.node_static_tf(
             tf=world_robot_tf,
             parent="world",
-            child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
+            child=PythonExpression(
+                ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"]
+            ),
         )
     )
 
diff --git a/lbr_bringup/launch/hardware.launch.py b/lbr_bringup/launch/hardware.launch.py
@@ -1,7 +1,7 @@
 from launch import LaunchDescription
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessStart
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PythonExpression
 from lbr_bringup.description import LBRDescriptionMixin
 from lbr_bringup.ros2_control import LBRROS2ControlMixin
 
@@ -18,12 +18,14 @@ def generate_launch_description() -> LaunchDescription:
     ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
     ld.add_action(LBRROS2ControlMixin.arg_ctrl())
 
-    # static transform world -> robot_name/world
+    # static transform world -> <robot_name>_floating_link
     ld.add_action(
         LBRDescriptionMixin.node_static_tf(
             tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
             parent="world",
-            child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
+            child=PythonExpression(
+                ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"]
+            ),
         )
     )
 
diff --git a/lbr_bringup/launch/mock.launch.py b/lbr_bringup/launch/mock.launch.py
@@ -1,7 +1,7 @@
 from launch import LaunchDescription
 from launch.actions import RegisterEventHandler
 from launch.event_handlers import OnProcessStart
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.substitutions import LaunchConfiguration, PythonExpression
 from lbr_bringup.description import LBRDescriptionMixin
 from lbr_bringup.ros2_control import LBRROS2ControlMixin
 
@@ -16,12 +16,14 @@ def generate_launch_description() -> LaunchDescription:
     ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg())
     ld.add_action(LBRROS2ControlMixin.arg_ctrl())
 
-    # static transform world -> robot_name/world
+    # static transform world -> <robot_name>_floating_link
     ld.add_action(
         LBRDescriptionMixin.node_static_tf(
             tf=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
             parent="world",
-            child=PathJoinSubstitution([LaunchConfiguration("robot_name"), "world"]),
+            child=PythonExpression(
+                ["'", LaunchConfiguration("robot_name"), "' + '_floating_link'"]
+            ),
         )
     )
 
diff --git a/lbr_description/urdf/iiwa14/iiwa14.xacro b/lbr_description/urdf/iiwa14/iiwa14.xacro
@@ -12,13 +12,13 @@
         name="system_config_path"
         default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" />
 
-    <!-- fixed to world, see http://classic.gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros -->
-    <link name="world" />
+    <!-- KDL requires a link without inertia -->
+    <link name="$(arg robot_name)_floating_link" />
 
     <!--joint
-        between world and link_0-->
-    <joint name="world_joint" type="fixed">
-        <parent link="world" />
+        between <robot_name>_floating_link and robot_name_link_0-->
+    <joint name="$(arg robot_name)_floating_joint" type="fixed">
+        <parent link="$(arg robot_name)_floating_link" />
         <child link="$(arg robot_name)_link_0" />
     </joint>
 
diff --git a/lbr_description/urdf/iiwa7/iiwa7.xacro b/lbr_description/urdf/iiwa7/iiwa7.xacro
@@ -12,13 +12,13 @@
         name="system_config_path"
         default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" />
 
-    <!-- fixed to world, see http://classic.gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros -->
-    <link name="world" />
+    <!-- KDL requires a link without inertia -->
+    <link name="$(arg robot_name)_floating_link" />
 
     <!--joint
-        between world and link_0-->
-    <joint name="world_joint" type="fixed">
-        <parent link="world" />
+        between <robot_name>_floating_link and robot_name_link_0-->
+    <joint name="$(arg robot_name)_floating_joint" type="fixed">
+        <parent link="$(arg robot_name)_floating_link" />
         <child link="$(arg robot_name)_link_0" />
     </joint>
 
diff --git a/lbr_description/urdf/med14/med14.xacro b/lbr_description/urdf/med14/med14.xacro
@@ -12,13 +12,13 @@
         name="system_config_path"
         default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" />
 
-    <!-- fixed to world, see http://classic.gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros -->
-    <link name="world" />
+    <!-- KDL requires a link without inertia -->
+    <link name="$(arg robot_name)_floating_link" />
 
     <!--joint
-        between world and link_0-->
-    <joint name="world_joint" type="fixed">
-        <parent link="world" />
+        between <robot_name>_floating_link and robot_name_link_0-->
+    <joint name="$(arg robot_name)_floating_joint" type="fixed">
+        <parent link="$(arg robot_name)_floating_link" />
         <child link="$(arg robot_name)_link_0" />
     </joint>
 
diff --git a/lbr_description/urdf/med7/med7.xacro b/lbr_description/urdf/med7/med7.xacro
@@ -12,13 +12,13 @@
         name="system_config_path"
         default="$(find lbr_description)/ros2_control/lbr_system_config.yaml" />
 
-    <!-- fixed to world, see http://classic.gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros -->
-    <link name="world" />
+    <!-- KDL requires a link without inertia -->
+    <link name="$(arg robot_name)_floating_link" />
 
     <!--joint
-        between world and link_0-->
-    <joint name="world_joint" type="fixed">
-        <parent link="world" />
+        between <robot_name>_floating_link and robot_name_link_0-->
+    <joint name="$(arg robot_name)_floating_joint" type="fixed">
+        <parent link="$(arg robot_name)_floating_link" />
         <child link="$(arg robot_name)_link_0" />
     </joint>
 
diff --git a/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz b/lbr_moveit_config/iiwa14_moveit_config/config/moveit.rviz
@@ -66,6 +66,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          lbr_floating_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           lbr_link_0:
             Alpha: 1
             Show Axes: false
@@ -110,10 +114,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Loop Animation: true
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -160,6 +160,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          lbr_floating_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           lbr_link_0:
             Alpha: 1
             Show Axes: false
@@ -204,10 +208,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
diff --git a/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz b/lbr_moveit_config/iiwa7_moveit_config/config/moveit.rviz
@@ -66,6 +66,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          lbr_floating_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           lbr_link_0:
             Alpha: 1
             Show Axes: false
@@ -110,10 +114,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Loop Animation: true
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -160,6 +160,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          lbr_floating_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           lbr_link_0:
             Alpha: 1
             Show Axes: false
@@ -204,10 +208,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
diff --git a/lbr_moveit_config/med14_moveit_config/config/moveit.rviz b/lbr_moveit_config/med14_moveit_config/config/moveit.rviz
@@ -66,6 +66,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          lbr_floating_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           lbr_link_0:
             Alpha: 1
             Show Axes: false
@@ -110,10 +114,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Loop Animation: true
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -160,6 +160,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          lbr_floating_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           lbr_link_0:
             Alpha: 1
             Show Axes: false
@@ -204,10 +208,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
diff --git a/lbr_moveit_config/med7_moveit_config/config/moveit.rviz b/lbr_moveit_config/med7_moveit_config/config/moveit.rviz
@@ -66,6 +66,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          lbr_floating_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           lbr_link_0:
             Alpha: 1
             Show Axes: false
@@ -110,10 +114,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Loop Animation: true
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -160,6 +160,10 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          lbr_floating_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
           lbr_link_0:
             Alpha: 1
             Show Axes: false
@@ -204,10 +208,6 @@ Visualization Manager:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
