add Cartesian motion-force controller for RT impedance control

flexiv_bringup/config/rizon_cartesian_controllers.yaml

@@ -0,0 +1,20 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    cartesian_motion_controller:
+      type: cartesian_motion_controller/CartesianMotionController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    flexiv_robot_states_broadcaster:
+      type: flexiv_robot_states_broadcaster/FlexivRobotStatesBroadcaster
+
+cartesian_motion_controller:
+  ros__parameters:
+    tcp_name: $(var robot_sn)_tcp
+
+flexiv_robot_states_broadcaster:
+  ros__parameters:
+    robot_sn: $(var robot_sn)

flexiv_bringup/launch/rizon.launch.py

@@ -53,7 +53,7 @@ def generate_launch_description():
         DeclareLaunchArgument(
             rdk_control_mode_param_name,
             default_value="joint_position",
-            description="RDK control mode for the ROS 2 control joint position and velocity interfaces. Options: joint_position, joint_impedance",
+            description="RDK control mode for joint controllers. For Cartesian mode, use rizon_cartesian.launch.py",
             choices=["joint_position", "joint_impedance"],
         )
     )

flexiv_bringup/launch/rizon_cartesian.launch.py

@@ -0,0 +1,241 @@
+"""
+Launch file for Flexiv Rizon robot with Cartesian motion-force control.
+This launch file sets up the robot in RT_CARTESIAN_MOTION_FORCE mode for
+real-time Cartesian pure motion control.
+"""
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    RegisterEventHandler,
+)
+from launch.conditions import IfCondition, UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+
+
+def generate_launch_description():
+    rizon_type_param_name = "rizon_type"
+    robot_sn_param_name = "robot_sn"
+    start_rviz_param_name = "start_rviz"
+    load_mounted_ft_sensor_param_name = "load_mounted_ft_sensor"
+    use_fake_hardware_param_name = "use_fake_hardware"
+    fake_sensor_commands_param_name = "fake_sensor_commands"
+
+    # Declare arguments
+    declared_arguments = []
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            rizon_type_param_name,
+            description="Type of the Flexiv Rizon robot.",
+            default_value="Rizon4",
+            choices=["Rizon4", "Rizon4M", "Rizon4R", "Rizon4s", "Rizon10", "Rizon10s"],
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            robot_sn_param_name,
+            description="Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            start_rviz_param_name,
+            default_value="true",
+            description="Start RViz automatically with the launch file",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            load_mounted_ft_sensor_param_name,
+            default_value="false",
+            description="Flag to load the mounted force torque sensor.",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            use_fake_hardware_param_name,
+            default_value="false",
+            description="Start robot with fake hardware mirroring command to its states.",
+        )
+    )
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            fake_sensor_commands_param_name,
+            default_value="false",
+            description="Enable fake command interfaces for sensors.",
+        )
+    )
+
+    # Initialize Arguments
+    rizon_type = LaunchConfiguration(rizon_type_param_name)
+    robot_sn = LaunchConfiguration(robot_sn_param_name)
+    start_rviz = LaunchConfiguration(start_rviz_param_name)
+    load_mounted_ft_sensor = LaunchConfiguration(load_mounted_ft_sensor_param_name)
+    use_fake_hardware = LaunchConfiguration(use_fake_hardware_param_name)
+    fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_param_name)
+
+    # Fixed to cartesian_motion_force mode
+    rdk_control_mode = "cartesian_motion_force"
+
+    # Get URDF via xacro
+    flexiv_urdf_xacro = PathJoinSubstitution(
+        [FindPackageShare("flexiv_description"), "urdf", "rizon.urdf.xacro"]
+    )
+
+    robot_description_content = ParameterValue(
+        Command(
+            [
+                PathJoinSubstitution([FindExecutable(name="xacro")]),
+                " ",
+                flexiv_urdf_xacro,
+                " ",
+                "robot_sn:=",
+                robot_sn,
+                " ",
+                "rizon_type:=",
+                rizon_type,
+                " ",
+                "rdk_control_mode:=",
+                rdk_control_mode,
+                " ",
+                "load_gripper:=false",
+                " ",
+                "load_mounted_ft_sensor:=",
+                load_mounted_ft_sensor,
+                " ",
+                "use_fake_hardware:=",
+                use_fake_hardware,
+                " ",
+                "fake_sensor_commands:=",
+                fake_sensor_commands,
+            ]
+        ),
+        value_type=str,
+    )
+
+    robot_description = {"robot_description": robot_description_content}
+
+    # RViZ
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("flexiv_description"), "rviz", "view_rizon.rviz"]
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        condition=IfCondition(start_rviz),
+    )
+
+    # Robot controllers for Cartesian mode
+    robot_controllers = PathJoinSubstitution(
+        [FindPackageShare("flexiv_bringup"), "config", "rizon_cartesian_controllers.yaml"]
+    )
+
+    # Controller Manager
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[
+            robot_description,
+            ParameterFile(robot_controllers, allow_substs=True),
+            {"robot_sn": robot_sn},
+            {"rdk_control_mode": rdk_control_mode},
+        ],
+        remappings=[("joint_states", "flexiv_arm/joint_states")],
+        output="both",
+    )
+
+    # Joint state publisher
+    joint_state_publisher_node = Node(
+        package="joint_state_publisher",
+        executable="joint_state_publisher",
+        name="joint_state_publisher",
+        parameters=[
+            {
+                "source_list": ["flexiv_arm/joint_states"],
+                "rate": 30,
+            }
+        ],
+    )
+
+    # Robot state publisher
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    # Run joint state broadcaster
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    # Run Flexiv robot states broadcaster
+    flexiv_robot_states_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["flexiv_robot_states_broadcaster"],
+        parameters=[{"robot_sn": robot_sn}],
+        condition=UnlessCondition(use_fake_hardware),
+    )
+
+    # Run Cartesian motion controller
+    cartesian_motion_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["cartesian_motion_controller", "--controller-manager", "/controller_manager"],
+    )
+
+    # Delay start of cartesian_motion_controller after joint_state_broadcaster
+    delay_cartesian_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[cartesian_motion_controller_spawner],
+        )
+    )
+
+    # Delay rviz start after cartesian_motion_controller_spawner
+    delay_rviz_after_controller_spawner = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=cartesian_motion_controller_spawner,
+            on_exit=[rviz_node],
+        )
+    )
+
+    nodes = [
+        ros2_control_node,
+        joint_state_publisher_node,
+        robot_state_publisher_node,
+        joint_state_broadcaster_spawner,
+        flexiv_robot_states_broadcaster_spawner,
+        delay_cartesian_controller_spawner,
+        delay_rviz_after_controller_spawner,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)

flexiv_controllers/cartesian_motion_controller/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.8)
+project(cartesian_motion_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(flexiv_msgs REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+
+generate_parameter_library(cartesian_motion_controller_parameters
+  src/cartesian_motion_controller_parameters.yaml
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/cartesian_motion_controller.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+target_link_libraries(${PROJECT_NAME}
+  cartesian_motion_controller_parameters
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  rclcpp_lifecycle
+  controller_interface
+  hardware_interface
+  pluginlib
+  realtime_tools
+  geometry_msgs
+  std_msgs
+  flexiv_msgs
+)
+
+pluginlib_export_plugin_description_file(controller_interface cartesian_motion_controller.xml)
+
+install(
+  TARGETS ${PROJECT_NAME} cartesian_motion_controller_parameters
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  rclcpp
+  rclcpp_lifecycle
+  controller_interface
+  hardware_interface
+  pluginlib
+  realtime_tools
+  geometry_msgs
+  std_msgs
+  flexiv_msgs
+)
+
+ament_package()

flexiv_controllers/cartesian_motion_controller/cartesian_motion_controller.xml

@@ -0,0 +1,10 @@
+<library path="cartesian_motion_controller">
+  <class name="cartesian_motion_controller/CartesianMotionController"
+         type="cartesian_motion_controller::CartesianMotionController"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>
+      Controller for Flexiv robot real-time Cartesian motion-force control.
+      Subscribes to CartesianMotionForceCommand messages and sends them to the hardware interface.
+    </description>
+  </class>
+</library>