moved controllers to hw if, updated lbr_description launch

lbr_demos/lbr_demos_fri_ros2_advanced_cpp/launch/admittance_control_node.launch.py

@@ -1,31 +1,19 @@
-from launch import LaunchContext, LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
+from launch import LaunchDescription
 from launch_ros.actions import Node
 
-from lbr_description import description_factory
-
-
-def launch_setup(context: LaunchContext) -> LaunchDescription:
-    model = LaunchConfiguration("model").perform(context)
-
-    robot_description = description_factory(model=model, sim="false")
-
-    admittance_control_node = Node(
-        package="lbr_demos_fri_ros2_advanced_cpp",
-        executable="admittance_control_node",
-        output="screen",
-        parameters=[robot_description],
-    )
-
-    return [admittance_control_node]
+from lbr_description import LBRDescription
 
 
 def generate_launch_description() -> LaunchDescription:
-    model_arg = DeclareLaunchArgument(
-        name="model",
-        default_value="iiwa7",
-        description="The LBR model in use.",
-        choices=["iiwa7", "iiwa14", "med7", "med14"],
+    ld = LBRDescription(sim=False)
+
+    ld.add_action(
+        Node(
+            package="lbr_demos_fri_ros2_advanced_cpp",
+            executable="admittance_control_node",
+            output="screen",
+            parameters=[ld.robot_description],
+        )
     )
-    return LaunchDescription([model_arg, OpaqueFunction(function=launch_setup)])
+
+    return ld

lbr_demos/lbr_demos_fri_ros2_advanced_python/launch/admittance_control_node.launch.py

@@ -1,31 +1,19 @@
-from launch import LaunchContext, LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
+from launch import LaunchDescription
 from launch_ros.actions import Node
 
-from lbr_description import description_factory
-
-
-def launch_setup(context: LaunchContext) -> LaunchDescription:
-    model = LaunchConfiguration("model").perform(context)
-
-    robot_description = description_factory(model=model, sim="false")
-
-    admittance_control_node = Node(
-        package="lbr_demos_fri_ros2_advanced_python",
-        executable="admittance_control_node",
-        output="screen",
-        parameters=[robot_description],
-    )
-
-    return [admittance_control_node]
+from lbr_description import LBRDescription
 
 
 def generate_launch_description() -> LaunchDescription:
-    model_arg = DeclareLaunchArgument(
-        name="model",
-        default_value="iiwa7",
-        description="The LBR model in use.",
-        choices=["iiwa7", "iiwa14", "med7", "med14"],
+    ld = LBRDescription(sim=False)
+
+    ld.add_action(
+        Node(
+            package="lbr_demos_fri_ros2_advanced_python",
+            executable="admittance_control_node",
+            output="screen",
+            parameters=[ld.robot_description],
+        )
     )
-    return LaunchDescription([model_arg, OpaqueFunction(function=launch_setup)])
+
+    return ld

lbr_demos/lbr_demos_fri_ros2_advanced_python/lbr_demos_fri_ros2_advanced_python/admittance_controller.py

@@ -40,7 +40,7 @@ def __call__(self, lbr_state: LBRState) -> LBRCommand:
 
         self.jacobian_ = self.chain_.jacobian(self.q_)
 
-        self.jacobian_inv_ = np.linalg.pinv(self.jacobian_, rcond=0.2)
+        self.jacobian_inv_ = np.linalg.pinv(self.jacobian_, rcond=0.1)
         self.f_ext_ = self.jacobian_inv_.T @ self.tau_ext_
 
         self.f_ext_ = np.where(

lbr_description/CMakeLists.txt

@@ -11,15 +11,16 @@ find_package(ament_cmake_pytest REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 
 # install
-ament_python_install_package(${PROJECT_NAME})
-
 install(
   DIRECTORY config gazebo launch meshes ros2_control urdf
   DESTINATION share/${PROJECT_NAME}
 )
 
 ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/${PROJECT_NAME}.dsv")
 
+# launch files
+ament_python_install_package(${PROJECT_NAME})
+
 if(BUILD_TESTING)
   ament_add_pytest_test(test_urdf ${CMAKE_CURRENT_SOURCE_DIR}/test/test_urdf.py)
 endif()

lbr_description/launch/view_robot.launch.py

@@ -1,69 +1,41 @@
 import os
-from typing import List
 
 from ament_index_python import get_package_share_directory
-from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
-from lbr_description import description_factory
-
-
-def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
-    model = LaunchConfiguration("model").perform(context)
-    robot_name = LaunchConfiguration("robot_name").perform(context)
-
-    robot_description = description_factory(model=model, robot_name=robot_name)
-
-    joint_state_publisher_gui_node = Node(
-        package="joint_state_publisher_gui",
-        executable="joint_state_publisher_gui",
-    )
-
-    robot_state_publisher_node = Node(
-        package="robot_state_publisher",
-        executable="robot_state_publisher",
-        output="both",
-        parameters=[robot_description],
-    )
-
-    rviz_node = Node(
-        package="rviz2",
-        executable="rviz2",
-        arguments=[
-            "-d",
-            os.path.join(
-                get_package_share_directory("lbr_description"), "config/config.rviz"
-            ),
-        ],
-    )
-
-    return [
-        joint_state_publisher_gui_node,
-        robot_state_publisher_node,
-        rviz_node,
-    ]
+from lbr_description import LBRDescription
 
 
 def generate_launch_description():
-    ld = LaunchDescription()
+    ld = LBRDescription()
 
     ld.add_action(
-        DeclareLaunchArgument(
-            name="model",
-            default_value="iiwa7",
-            description="Set robot model.",
-            choices=["iiwa7", "iiwa14", "med7", "med14"],
+        Node(
+            package="joint_state_publisher_gui",
+            executable="joint_state_publisher_gui",
         )
     )
 
     ld.add_action(
-        DeclareLaunchArgument(
-            name="robot_name", default_value="lbr", description="Set robot name."
+        Node(
+            package="robot_state_publisher",
+            executable="robot_state_publisher",
+            output="both",
+            parameters=[ld.robot_description],
         )
     )
 
-    ld.add_action(OpaqueFunction(function=launch_setup))
+    ld.add_action(
+        Node(
+            package="rviz2",
+            executable="rviz2",
+            arguments=[
+                "-d",
+                os.path.join(
+                    get_package_share_directory("lbr_description"), "config/config.rviz"
+                ),
+            ],
+        )
+    )
 
     return ld

lbr_description/lbr_description/__init__.py

@@ -1 +1 @@
-from .description_factory import description_factory
+from .lbr_description import LBRDescription

lbr_description/lbr_description/lbr_description.py

@@ -0,0 +1,70 @@
+from typing import Dict
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+from launch_ros.substitutions import FindPackageShare
+
+
+class LBRDescription(LaunchDescription):
+    _robot_description: Dict[str, str]
+
+    def __init__(
+        self,
+        sim: bool = True,
+    ) -> None:
+        super().__init__()
+        self._add_launch_arguments()
+        self._robot_description = self._generate_robot_description(sim)
+
+    @property
+    def robot_description(self) -> Dict[str, str]:
+        return self._robot_description
+
+    def _add_launch_arguments(self) -> None:
+        self.add_action(
+            DeclareLaunchArgument(
+                name="model",
+                default_value="iiwa7",
+                description="The LBR model in use.",
+                choices=["iiwa7", "iiwa14", "med7", "med14"],
+            )
+        )
+
+        self.add_action(
+            DeclareLaunchArgument(
+                name="robot_name",
+                default_value="lbr",
+                description="The robot's name.",
+            )
+        )
+
+    def _generate_robot_description(self, sim: bool = True) -> Dict[str, str]:
+        robot_description = {
+            "robot_description": Command(
+                [
+                    FindExecutable(name="xacro"),
+                    " ",
+                    PathJoinSubstitution(
+                        [
+                            FindPackageShare("lbr_description"),
+                            "urdf",
+                            LaunchConfiguration("model"),
+                            LaunchConfiguration("model"),
+                        ]
+                    ),
+                    ".urdf.xacro",
+                    " robot_name:=",
+                    LaunchConfiguration("robot_name"),
+                    " sim:=",
+                    "true" if sim else "false",
+                ]
+            )
+        }
+
+        return robot_description

lbr_fri_ros2/launch/lbr_app.launch.py

@@ -1,44 +1,20 @@
-import os
-
-import xacro
-from ament_index_python import get_package_share_directory
-from launch import LaunchContext, LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.substitutions import LaunchConfiguration
+from launch import LaunchDescription
 from launch_ros.actions import Node
 
+from lbr_description import LBRDescription
 
-def launch_setup(context: LaunchContext):
-    model = LaunchConfiguration("model").perform(context)
 
-    robot_description = {
-        "robot_description": xacro.process(
-            os.path.join(
-                get_package_share_directory("lbr_description"),
-                "urdf",
-                model,
-                f"{model}.urdf.xacro",
-            ),
-            mappings={"sim": "false"},
-        )
-    }
+def generate_launch_description() -> LaunchDescription:
+    ld = LBRDescription(sim=False)
 
-    lbr_app_node = Node(
-        package="lbr_fri_ros2",
-        executable="lbr_app",
-        emulate_tty=True,
-        output="screen",
-        parameters=[robot_description],
+    ld.add_action(
+        Node(
+            package="lbr_fri_ros2",
+            executable="lbr_app",
+            emulate_tty=True,
+            output="screen",
+            parameters=[ld.robot_description],
+        )
     )
 
-    return [lbr_app_node]
-
-
-def generate_launch_description():
-    model_arg = DeclareLaunchArgument(
-        name="model",
-        default_value="iiwa7",
-        description="The LBR model in use.",
-        choices=["iiwa7", "iiwa14", "med7", "med14"],
-    )
-    return LaunchDescription([model_arg, OpaqueFunction(function=launch_setup)])
+    return ld

lbr_fri_ros2/package.xml

@@ -17,6 +17,8 @@
   <depend>rclcpp_components</depend>
   <depend>urdf</depend>
 
+  <exec_depend>lbr_description</exec_depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>