Merge pull request #213 from lbr-stack/dev-rolling-admittance

LGTM
lbr-stack · Oct 22, 2024 · acde47a · acde47a
2 parents a5071d9 + 48f92c1
commit acde47a
Show file tree

Hide file tree

Showing 108 changed files with 1,748 additions and 1,007 deletions.
diff --git a/README.md b/README.md
@@ -78,8 +78,8 @@ Full documentation available on [Read the Docs](https://lbr-stack.readthedocs.io
     ```shell
     source install/setup.bash
     ros2 launch lbr_bringup rviz.launch.py \
-        rviz_config_pkg:=lbr_bringup \
-        rviz_config:=config/mock.rviz
+        rviz_cfg_pkg:=lbr_bringup \
+        rviz_cfg:=config/mock.rviz
     ```
 
 Now, run the [demos](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Hardware Setup](https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/hardware_setup.html).

diff --git a/lbr_bringup/config/gazebo.rviz b/lbr_bringup/config/gazebo.rviz
@@ -67,59 +67,59 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        link_0:
+        lbr_floating_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        link_1:
+        lbr_link_0:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_2:
+        lbr_link_1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_3:
+        lbr_link_2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_4:
+        lbr_link_3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_5:
+        lbr_link_4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_6:
+        lbr_link_5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_7:
+        lbr_link_6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_ee:
+        lbr_link_7:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        world:
+          Value: true
+        lbr_link_ee:
           Alpha: 1
           Show Axes: false
           Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
       Name: RobotModel
-      TF Prefix: lbr
+      TF Prefix: ""
       Update Interval: 0
       Value: true
       Visual Enabled: true

diff --git a/lbr_bringup/config/hardware.rviz b/lbr_bringup/config/hardware.rviz
@@ -68,59 +68,59 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        link_0:
+        lbr_floating_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        link_1:
+        lbr_link_0:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_2:
+        lbr_link_1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_3:
+        lbr_link_2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_4:
+        lbr_link_3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_5:
+        lbr_link_4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_6:
+        lbr_link_5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_7:
+        lbr_link_6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_ee:
+        lbr_link_7:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        world:
+          Value: true
+        lbr_link_ee:
           Alpha: 1
           Show Axes: false
           Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
       Name: RobotModel
-      TF Prefix: lbr
+      TF Prefix: ""
       Update Interval: 0
       Value: true
       Visual Enabled: true

diff --git a/lbr_bringup/config/mock.rviz b/lbr_bringup/config/mock.rviz
@@ -67,59 +67,59 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        link_0:
+        lbr_floating_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
-        link_1:
+        lbr_link_0:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_2:
+        lbr_link_1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_3:
+        lbr_link_2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_4:
+        lbr_link_3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_5:
+        lbr_link_4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_6:
+        lbr_link_5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_7:
+        lbr_link_6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_ee:
+        lbr_link_7:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        world:
+          Value: true
+        lbr_link_ee:
           Alpha: 1
           Show Axes: false
           Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
       Name: RobotModel
-      TF Prefix: lbr
+      TF Prefix: ""
       Update Interval: 0
       Value: true
       Visual Enabled: true

diff --git a/lbr_bringup/doc/lbr_bringup.rst b/lbr_bringup/doc/lbr_bringup.rst
@@ -114,8 +114,8 @@ This launch file will spin up ``RViz`` for visualization. It will (see `rviz.lau
 .. code:: bash
 
     ros2 launch lbr_bringup rviz.launch.py \
-            rviz_config_pkg:=lbr_bringup \
-            rviz_config:=config/mock.rviz # [gazebo.rviz, hardware.rviz, mock.rviz]
+            rviz_cfg_pkg:=lbr_bringup \
+            rviz_cfg:=config/mock.rviz # [gazebo.rviz, hardware.rviz, mock.rviz]
 
 .. note::
     List all arguments for the launch file via ``ros2 launch lbr_bringup rviz.launch.py -s``.
@@ -156,8 +156,8 @@ The below shows an example of the `rviz.launch.py <https://github.com/lbr-stack/
         ld = LaunchDescription()
 
         # launch arguments
-        ld.add_action(RVizMixin.arg_rviz_config())
-        ld.add_action(RVizMixin.arg_rviz_config_pkg())
+        ld.add_action(RVizMixin.arg_rviz_cfg())
+        ld.add_action(RVizMixin.arg_rviz_cfg_pkg())
 
         # rviz
         ld.add_action(RVizMixin.node_rviz())

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
 
@@ -12,17 +12,20 @@ def generate_launch_description() -> LaunchDescription:
     # launch arguments
     ld.add_action(LBRDescriptionMixin.arg_model())
     ld.add_action(LBRDescriptionMixin.arg_robot_name())
-    ld.add_action(LBRDescriptionMixin.arg_port_id())
+    ld.add_action(LBRROS2ControlMixin.arg_sys_cfg_pkg())
+    ld.add_action(LBRROS2ControlMixin.arg_sys_cfg())
     ld.add_action(LBRROS2ControlMixin.arg_ctrl_cfg_pkg())
     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_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py
@@ -46,8 +46,8 @@ def hidden_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
 
     # RViz if desired
     rviz = RVizMixin.node_rviz(
-        rviz_config_pkg=f"{model}_moveit_config",
-        rviz_config="config/moveit.rviz",
+        rviz_cfg_pkg=f"{model}_moveit_config",
+        rviz_cfg="config/moveit.rviz",
         parameters=LBRMoveGroupMixin.params_rviz(
             moveit_configs=moveit_configs_builder.to_moveit_configs()
         )

diff --git a/lbr_bringup/launch/rviz.launch.py b/lbr_bringup/launch/rviz.launch.py
@@ -6,8 +6,8 @@ def generate_launch_description() -> LaunchDescription:
     ld = LaunchDescription()
 
     # launch arguments
-    ld.add_action(RVizMixin.arg_rviz_config())
-    ld.add_action(RVizMixin.arg_rviz_config_pkg())
+    ld.add_action(RVizMixin.arg_rviz_cfg())
+    ld.add_action(RVizMixin.arg_rviz_cfg_pkg())
 
     # rviz
     ld.add_action(RVizMixin.node_rviz())